Revision 07e68eb7
| examples/humotion_yarp_icub/CMakeLists.txt.user | ||
|---|---|---|
| 1 | 1 |
<?xml version="1.0" encoding="UTF-8"?> |
| 2 | 2 |
<!DOCTYPE QtCreatorProject> |
| 3 |
<!-- Written by QtCreator 3.0.1, 2016-02-15T14:37:35. -->
|
|
| 3 |
<!-- Written by QtCreator 3.0.1, 2016-02-16T13:25:29. -->
|
|
| 4 | 4 |
<qtcreator> |
| 5 | 5 |
<data> |
| 6 | 6 |
<variable>ProjectExplorer.Project.ActiveTarget</variable> |
| examples/humotion_yarp_icub/src/icub_faceinterface.cpp | ||
|---|---|---|
| 28 | 28 |
#include "humotion_yarp_icub/icub_faceinterface.h" |
| 29 | 29 |
|
| 30 | 30 |
#include <boost/format.hpp> |
| 31 |
#include <boost/lexical_cast.hpp> |
|
| 31 | 32 |
|
| 32 | 33 |
#include <algorithm> |
| 33 | 34 |
#include <string> |
| ... | ... | |
| 46 | 47 |
std::string emotion_scope = scope + "/face/raw/in"; |
| 47 | 48 |
cout << "opening connection to '"<< emotion_scope << "'\n"; |
| 48 | 49 |
|
| 50 |
bool init_ok = true; |
|
| 49 | 51 |
for (int i = 0; i < 4; i++) {
|
| 50 | 52 |
// strange, if we use one output port only the first command is executed?! flushing issues? |
| 51 |
std::string emotion_port_out = "/emotionwriter" + std::to_string((unsigned long long)i);
|
|
| 53 |
std::string emotion_port_out = "/emotionwriter" + boost::lexical_cast<std::string>(i);
|
|
| 52 | 54 |
if (!emotion_port[i].open(emotion_port_out.c_str())) {
|
| 53 | 55 |
cerr << "ERROR: failed to open emotion port '" << emotion_port_out << "'\n"; |
| 54 |
exit(EXIT_FAILURE);
|
|
| 56 |
init_ok = false;
|
|
| 55 | 57 |
} |
| 56 | 58 |
if (!Network::connect(emotion_port_out.c_str(), emotion_scope.c_str())) {
|
| 57 | 59 |
cerr << "ERROR: failed to connect to emotion port '" << emotion_port_out << "'\n"; |
| 58 |
exit(EXIT_FAILURE);
|
|
| 60 |
init_ok = false;
|
|
| 59 | 61 |
} |
| 60 | 62 |
} |
| 63 |
|
|
| 64 |
if (!init_ok) {
|
|
| 65 |
cerr << "ERROR: failed to set up emotion component\n"; |
|
| 66 |
cerr << " please make sure that the faceExpressions yarpdev is started\n"; |
|
| 67 |
cerr << " (e.g. yarpdev --name /icub/face/raw --device serial ... is running)\n"; |
|
| 68 |
exit(EXIT_FAILURE); |
|
| 69 |
} |
|
| 61 | 70 |
} |
| 62 | 71 |
|
| 63 | 72 |
//! destructor |
| ... | ... | |
| 138 | 147 |
|
| 139 | 148 |
std::string cmd_s; |
| 140 | 149 |
if (id == iCubJointInterface::ICUB_ID_EYES_LEFT_BROW) {
|
| 141 |
cmd_s = "L0" + std::to_string((unsigned long long)icub_val);
|
|
| 150 |
cmd_s = "L0" + boost::lexical_cast<std::string>(icub_val);
|
|
| 142 | 151 |
} else {
|
| 143 |
cmd_s = "R0" + std::to_string((unsigned long long)icub_val);
|
|
| 152 |
cmd_s = "R0" + boost::lexical_cast<std::string>(icub_val);
|
|
| 144 | 153 |
} |
| 145 | 154 |
|
| 146 | 155 |
cout << "SETTING EYEBROW " << id << " (" << angle << " -> " << cmd_s << ")\n";
|
| examples/humotion_yarp_icub/src/icub_jointinterface.cpp | ||
|---|---|---|
| 131 | 131 |
enum_id_bimap_t::const_iterator it; |
| 132 | 132 |
for (it = enum_id_bimap.begin(); it != enum_id_bimap.end(); ++it) {
|
| 133 | 133 |
int id = it->left; |
| 134 |
pv_mix_pid_p_[id] = 15.5;
|
|
| 134 |
pv_mix_pid_p_[id] = 5.5; |
|
| 135 | 135 |
pv_mix_pid_d_[id] = 0.3; |
| 136 | 136 |
pv_mix_last_error_[id] = 0.0; |
| 137 | 137 |
} |
| ... | ... | |
| 172 | 172 |
float target_position_pan = (target_position_right + target_position_left) / 2; |
| 173 | 173 |
float target_position_vergence = (target_position_right - target_position_left); |
| 174 | 174 |
|
| 175 |
cout << "LR " << target_position_left << " " << target_position_right << |
|
| 176 |
" PAN " << target_position_pan << " VERGENCE " << target_position_vergence << "\n"; |
|
| 175 |
/*cout << "LR " << target_position_left << " " << target_position_right <<
|
|
| 176 |
" PAN " << target_position_pan << " VERGENCE " << target_position_vergence << "\n";*/
|
|
| 177 | 177 |
|
| 178 | 178 |
// calculate target velocities |
| 179 | 179 |
// for now just use the same velocity for pan and vergence |
| ... | ... | |
| 197 | 197 |
//! \param id of joint |
| 198 | 198 |
//! \param float value of position |
| 199 | 199 |
void iCubJointInterface::store_icub_joint_target(int icub_id, float position, float velocity) {
|
| 200 |
cout << "store_icub_joint_target(" << icub_id << ", " << position << ", ..)\n";
|
|
| 200 |
//cout << "store_icub_joint_target(" << icub_id << ", " << position << ", ..)\n";
|
|
| 201 | 201 |
|
| 202 | 202 |
if ((icub_id == ICUB_ID_NECK_PAN) || (icub_id == ICUB_ID_EYES_VERGENCE)) {
|
| 203 | 203 |
// icub uses an inverted neck pan specification |
| src/server/eye_motion_generator.cpp | ||
|---|---|---|
| 133 | 133 |
joint_interface->set_target(JointInterface::ID_EYES_BOTH_UD, |
| 134 | 134 |
reflexxes_position_output->NewPositionVector->VecData[2], |
| 135 | 135 |
reflexxes_position_output->NewVelocityVector->VecData[2]); |
| 136 |
|
|
| 137 |
cout << reflexxes_position_output->NewPositionVector->VecData[2] << " " << reflexxes_position_output->NewVelocityVector->VecData[2] ; |
|
| 138 | 136 |
} |
| 139 | 137 |
|
| 140 | 138 |
|
Also available in: Unified diff