humotion / examples / meka / src / mekajointinterface.cpp @ a4795834
History | View | Annotate | Download (30.938 KB)
| 1 | 2be6243f | Sebastian Meyer zu Borgsen | #include "mekajointinterface.h" |
|---|---|---|---|
| 2 | using namespace std; |
||
| 3 | |||
| 4 | //WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED
|
||
| 5 | #define POSITION_CONTROL 1 |
||
| 6 | |||
| 7 | 473a6a6c | Simon Schulz | void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & msg){ |
| 8 | a4795834 | Simon Schulz | //fetch current timestamp
|
| 9 | double timestamp = get_timestamp_ms();
|
||
| 10 | |||
| 11 | //iterate through incoming joints and filter out joints we need:
|
||
| 12 | for(int i=0; i<msg.name.size(); i++){ |
||
| 13 | string name = msg.name[i];
|
||
| 14 | //printf("incoming data for joint '%s'\n", name.c_str());
|
||
| 15 | |||
| 16 | int id = -1; |
||
| 17 | if (name == "NECK_PAN"){ |
||
| 18 | id = ID_NECK_PAN; |
||
| 19 | }else if(name == "NECK_TILT"){ |
||
| 20 | id = ID_NECK_TILT; |
||
| 21 | } |
||
| 22 | |||
| 23 | //store data:
|
||
| 24 | if (id != -1){ |
||
| 25 | //printf("> storing joint data for joint id %d\n", id);
|
||
| 26 | JointInterface::store_incoming_position(id, msg.position[i], timestamp); |
||
| 27 | JointInterface::store_incoming_speed( id, msg.velocity[i], timestamp); |
||
| 28 | } |
||
| 29 | } |
||
| 30 | |||
| 31 | //store dummy positions for joints we do not know about:
|
||
| 32 | store_dummy_data(ID_LIP_LEFT_UPPER, timestamp); |
||
| 33 | store_dummy_data(ID_LIP_LEFT_LOWER, timestamp); |
||
| 34 | store_dummy_data(ID_LIP_CENTER_UPPER, timestamp); |
||
| 35 | store_dummy_data(ID_LIP_CENTER_LOWER, timestamp); |
||
| 36 | store_dummy_data(ID_LIP_RIGHT_UPPER, timestamp); |
||
| 37 | store_dummy_data(ID_LIP_RIGHT_LOWER, timestamp); |
||
| 38 | |||
| 39 | store_dummy_data(ID_NECK_ROLL, timestamp); |
||
| 40 | store_dummy_data(ID_EYES_BOTH_UD, timestamp); |
||
| 41 | store_dummy_data(ID_EYES_LEFT_LR, timestamp); |
||
| 42 | store_dummy_data(ID_EYES_RIGHT_LR, timestamp); |
||
| 43 | store_dummy_data(ID_EYES_LEFT_LID_LOWER, timestamp); |
||
| 44 | store_dummy_data(ID_EYES_LEFT_LID_UPPER, timestamp); |
||
| 45 | store_dummy_data(ID_EYES_LEFT_BROW, timestamp); |
||
| 46 | store_dummy_data(ID_EYES_RIGHT_LID_LOWER, timestamp); |
||
| 47 | store_dummy_data(ID_EYES_RIGHT_LID_UPPER, timestamp); |
||
| 48 | store_dummy_data(ID_EYES_RIGHT_BROW, timestamp); |
||
| 49 | |||
| 50 | } |
||
| 51 | |||
| 52 | void MekaJointInterface::store_dummy_data(int id, double timestamp){ |
||
| 53 | JointInterface::store_incoming_position(id, 0.0, timestamp); |
||
| 54 | JointInterface::store_incoming_speed(id, 0.0, timestamp); |
||
| 55 | 473a6a6c | Simon Schulz | } |
| 56 | |||
| 57 | 2be6243f | Sebastian Meyer zu Borgsen | //! constructor
|
| 58 | MekaJointInterface::MekaJointInterface(string _scope) : humotion::server::JointInterface(){
|
||
| 59 | scope = _scope; |
||
| 60 | a4795834 | Simon Schulz | string js_topic = scope + "joint_states"; |
| 61 | string target_topic = scope + "meka_roscontrol/head_position_trajectory_controller/command"; |
||
| 62 | 2be6243f | Sebastian Meyer zu Borgsen | |
| 63 | 473a6a6c | Simon Schulz | //subscribe to meka joint states
|
| 64 | 99ebae32 | Simon Schulz | int argc = 0; |
| 65 | ros::init(argc, (char**)NULL, "meka_humotion"); |
||
| 66 | 473a6a6c | Simon Schulz | ros::NodeHandle n; |
| 67 | 2be6243f | Sebastian Meyer zu Borgsen | |
| 68 | |||
| 69 | a4795834 | Simon Schulz | printf("> listening on jointstates on '%s'\n",js_topic.c_str());
|
| 70 | joint_state_subscriber = n.subscribe(js_topic, 150, &MekaJointInterface::incoming_jointstates , this); |
||
| 71 | 2be6243f | Sebastian Meyer zu Borgsen | |
| 72 | a4795834 | Simon Schulz | printf("> sending targets on '%s'\n", target_topic.c_str());
|
| 73 | target_publisher = n.advertise<trajectory_msgs::JointTrajectory>(target_topic, 100);
|
||
| 74 | 2be6243f | Sebastian Meyer zu Borgsen | |
| 75 | |||
| 76 | a4795834 | Simon Schulz | //tell humotion about min/max joint values:
|
| 77 | init_joints(); |
||
| 78 | 2be6243f | Sebastian Meyer zu Borgsen | } |
| 79 | |||
| 80 | //! destructor
|
||
| 81 | MekaJointInterface::~MekaJointInterface(){
|
||
| 82 | } |
||
| 83 | |||
| 84 | 473a6a6c | Simon Schulz | |
| 85 | |||
| 86 | void MekaJointInterface::run(){
|
||
| 87 | //iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this);
|
||
| 88 | //data_receiver->start();
|
||
| 89 | a4795834 | Simon Schulz | ros::spin(); |
| 90 | 473a6a6c | Simon Schulz | } |
| 91 | |||
| 92 | |||
| 93 | a4795834 | Simon Schulz | double MekaJointInterface::get_timestamp_ms(){
|
| 94 | struct timespec spec;
|
||
| 95 | clock_gettime(CLOCK_REALTIME, &spec); |
||
| 96 | return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6; |
||
| 97 | } |
||
| 98 | |||
| 99 | 473a6a6c | Simon Schulz | //! set the target position of a joint
|
| 100 | //! \param enum id of joint
|
||
| 101 | //! \param float value
|
||
| 102 | void MekaJointInterface::publish_target_position(int e){ |
||
| 103 | #if 0
|
||
| 104 | //first: convert humotion enum to our enum:
|
||
| 105 | int id = convert_enum_to_motorid(e);
|
||
| 106 | if (id == -1){
|
||
| 107 | return; //we are not interested in that data, so we just return here
|
||
| 108 | }
|
||
| 109 | |||
| 110 | if (id == ICUB_ID_NECK_PAN){
|
||
| 111 | //PAN seems to be swapped
|
||
| 112 | store_joint(ICUB_ID_NECK_PAN, -joint_target[e]);
|
||
| 113 | }else if ((id == ICUB_ID_EYES_LEFT_LR) || ( id == ICUB_ID_EYES_RIGHT_LR)){
|
||
| 114 | //icub handles eyes differently, we have to set pan angle + vergence
|
||
| 115 | float pan = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2;
|
||
| 116 | float vergence = (joint_target[ID_EYES_LEFT_LR] - joint_target[ID_EYES_RIGHT_LR]);
|
||
| 117 | //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);
|
||
| 118 | |||
| 119 | store_joint(ICUB_ID_EYES_PAN, pan);
|
||
| 120 | store_joint(ICUB_ID_EYES_VERGENCE, vergence);
|
||
| 121 | }else{
|
||
| 122 | store_joint(id, joint_target[e]);
|
||
| 123 | }
|
||
| 124 | #endif
|
||
| 125 | }
|
||
| 126 | |||
| 127 | |||
| 128 | //! actually execute the scheduled motion commands
|
||
| 129 | void MekaJointInterface::execute_motion(){
|
||
| 130 | a4795834 | Simon Schulz | //build msg
|
| 131 | trajectory_msgs::JointTrajectory msg;
|
||
| 132 | msg.joint_names.push_back("head_j0");
|
||
| 133 | msg.joint_names.push_back("head_j1");
|
||
| 134 | |||
| 135 | trajectory_msgs::JointTrajectoryPoint p[2];
|
||
| 136 | p[0].positions.push_back(joint_target[ID_NECK_PAN] * M_PI / 180.0);
|
||
| 137 | p[1].positions.push_back(joint_target[ID_NECK_TILT]* M_PI / 180.0);
|
||
| 138 | |||
| 139 | p[0].time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);
|
||
| 140 | p[1].time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);
|
||
| 141 | |||
| 142 | msg.points.push_back(p[0]);
|
||
| 143 | msg.points.push_back(p[1]);
|
||
| 144 | |||
| 145 | target_publisher.publish(msg);
|
||
| 146 | |||
| 147 | /*
|
||
| 148 | void MekaJointInterface::store_min_max(int id, float min, float max){
|
||
| 149 | header:
|
||
| 150 | seq: 636
|
||
| 151 | stamp:
|
||
| 152 | secs: 0
|
||
| 153 | nsecs: 0
|
||
| 154 | frame_id: ''
|
||
| 155 | joint_names: ['head_j0', 'head_j1']
|
||
| 156 | points:
|
||
| 157 | -
|
||
| 158 | positions: [-0.31, 0.01954768762234005]
|
||
| 159 | velocities: []
|
||
| 160 | accelerations: []
|
||
| 161 | effort: []
|
||
| 162 | time_from_start:
|
||
| 163 | secs: 1
|
||
| 164 | nsecs: 0
|
||
| 165 | |||
| 166 | */
|
||
| 167 | |||
| 168 | 473a6a6c | Simon Schulz | #if 0
|
| 169 | // set up neck and eye motion commands:
|
||
| 170 | if (POSITION_CONTROL){
|
||
| 171 | //position control
|
||
| 172 | for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
|
||
| 173 | set_target_in_positionmode(i, target_angle[i]);
|
||
| 174 | }
|
||
| 175 | }else{
|
||
| 176 | //velocity control
|
||
| 177 | for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
|
||
| 178 | set_target_in_velocitymode(i, target_angle[i]);
|
||
| 179 | }
|
||
| 180 | }
|
||
| 181 | //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);
|
||
| 182 | |||
| 183 | |||
| 184 | //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:
|
||
| 185 | set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);
|
||
| 186 | |||
| 187 | //eyebrows are set using a special command as well:
|
||
| 188 | set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW);
|
||
| 189 | set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW);
|
||
| 190 | |||
| 191 | //mouth
|
||
| 192 | set_mouth();
|
||
| 193 | |||
| 194 | #endif
|
||
| 195 | }
|
||
| 196 | |||
| 197 | |||
| 198 | //! prepare and enable a joint
|
||
| 199 | //! NOTE: this should also prefill the min/max positions for this joint
|
||
| 200 | //! \param the enum id of a joint
|
||
| 201 | void MekaJointInterface::enable_joint(int e){
|
||
| 202 | #if 0
|
||
| 203 | //FIXME ADD THIS:
|
||
| 204 | // enable the amplifier and the pid controller on each joint
|
||
| 205 | /*for (i = 0; i < nj; i++) {
|
||
| 206 | amp->enableAmp(i);
|
||
| 207 | pid->enablePid(i);
|
||
| 208 | }*/
|
||
| 209 | |||
| 210 | |||
| 211 | //set up smooth motion controller
|
||
| 212 | //step1: set up framerate
|
||
| 213 | //dev->set_register_blocking(XSC3_REGISTER_PID_RAMP, framerate, true);
|
||
| 214 | |||
| 215 | //step2: set controllertype:
|
||
| 216 | //printf("> activating smooth motion control for joint id 0x%02X (%s)\n", motor_id, joint_name.c_str());
|
||
| 217 | //dev->set_register_blocking(XSC3_REGISTER_PID_CONTROLLER, XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK, true);
|
||
| 218 | |||
| 219 | //step3: set pid controller:
|
||
| 220 | /*if ((e == ID_LIP_LEFT_UPPER) || (e == ID_LIP_LEFT_LOWER) || (e == ID_LIP_CENTER_UPPER) || (e == ID_LIP_CENTER_LOWER) || (e == ID_LIP_RIGHT_UPPER) || (e == ID_LIP_RIGHT_LOWER)){
|
||
| 221 | printf("> fixing PID i controller value for smooth motion (FIXME: restore old value!!)\n");
|
||
| 222 | dev->set_register_blocking(XSC3_REGISTER_CONST_I, 10, true);
|
||
| 223 | }*/
|
||
| 224 | |||
| 225 | //uint16_t result = dev->get_register_blocking_raw(XSC3_REGISTER_PID_CONTROLLER);
|
||
| 226 | |||
| 227 | //check if setting pid controllertype was successfull:
|
||
| 228 | /*if (result != XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK){
|
||
| 229 | printf("> failed to set smooth motion controller for joint %s (res=0x%04X)\n",joint_name.c_str(),result);
|
||
| 230 | exit(1);
|
||
| 231 | }*/
|
||
| 232 | |||
| 233 | //fetch min/max:
|
||
| 234 | // init_joint(e);
|
||
| 235 | |||
| 236 | //ok fine, now enable motor:
|
||
| 237 | //printf("> enabling motor %s\n", joint_name.c_str());
|
||
| 238 | //dev->set_register_blocking(XSC3_REGISTER_STATUS, XSC3_PROTOCOL_STATUS_BITMASK_MOTOR_ENABLE, true);
|
||
| 239 | #endif
|
||
| 240 | } |
||
| 241 | |||
| 242 | //! shutdown and disable a joint
|
||
| 243 | //! \param the enum id of a joint
|
||
| 244 | void MekaJointInterface::disable_joint(int e){ |
||
| 245 | /*
|
||
| 246 | //first: convert humotion enum to our enum:
|
||
| 247 | int motor_id = convert_enum_to_motorid(e);
|
||
| 248 | if (motor_id == -1){
|
||
| 249 | return; //we are not interested in that data, so we just return here
|
||
| 250 | }
|
||
| 251 | |||
| 252 | //fetch device:
|
||
| 253 | Device *dev = get_device(motor_id);
|
||
| 254 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 255 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 256 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 257 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 258 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 259 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 260 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 261 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 262 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 263 | */
|
||
| 264 | } |
||
| 265 | |||
| 266 | a4795834 | Simon Schulz | void MekaJointInterface::store_min_max(int id, float min, float max){ |
| 267 | joint_min[id] = min; |
||
| 268 | joint_max[id] = max; |
||
| 269 | } |
||
| 270 | |||
| 271 | void MekaJointInterface::init_joints(){
|
||
| 272 | store_min_max(ID_NECK_TILT, -10, 10); |
||
| 273 | store_min_max(ID_NECK_PAN, -10, 10); |
||
| 274 | |||
| 275 | store_min_max(ID_NECK_ROLL, -1, 1); |
||
| 276 | store_min_max(ID_EYES_BOTH_UD, -1, 1); |
||
| 277 | store_min_max(ID_EYES_LEFT_LR, -1, 1); |
||
| 278 | store_min_max(ID_EYES_RIGHT_LR, -1, 1); |
||
| 279 | store_min_max(ID_EYES_LEFT_LID_UPPER, -1, 1); |
||
| 280 | store_min_max(ID_EYES_LEFT_LID_LOWER, -1, 1); |
||
| 281 | store_min_max(ID_EYES_RIGHT_LID_UPPER, -1, 1); |
||
| 282 | store_min_max(ID_EYES_RIGHT_LID_LOWER, -1, 1); |
||
| 283 | store_min_max(ID_EYES_LEFT_BROW, -1, 1); |
||
| 284 | store_min_max(ID_EYES_RIGHT_BROW, -1, 1); |
||
| 285 | store_min_max(ID_LIP_CENTER_UPPER, -1, 1); |
||
| 286 | store_min_max(ID_LIP_CENTER_LOWER, -1, 1); |
||
| 287 | store_min_max(ID_LIP_LEFT_UPPER, -1, 1); |
||
| 288 | store_min_max(ID_LIP_LEFT_LOWER, -1, 1); |
||
| 289 | store_min_max(ID_LIP_RIGHT_UPPER, -1, 1); |
||
| 290 | store_min_max(ID_LIP_RIGHT_LOWER, -1, 1); |
||
| 291 | } |
||
| 292 | |||
| 293 | 473a6a6c | Simon Schulz | #if 0
|
| 294 | 2be6243f | Sebastian Meyer zu Borgsen | |
| 295 | //! conversion table for humotion motor ids to our ids:
|
||
| 296 | //! \param enum from JointInterface::JOINT_ID_ENUM
|
||
| 297 | //! \return int value of motor id
|
||
| 298 | int iCubJointInterface::convert_enum_to_motorid(int e){
|
||
| 299 | enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(e);
|
||
| 300 | if(it == enum_id_bimap.right.end()) {
|
||
| 301 | //key does not exists, we are not interested in that dataset, return -1
|
||
| 302 | return -1;
|
||
| 303 | }
|
||
| 304 | return it->second;
|
||
| 305 | }
|
||
| 306 | |||
| 307 | |||
| 308 | //! conversion table for our ids to humotion motor ids:
|
||
| 309 | //! \param int value of motor id
|
||
| 310 | //! \return enum from JointInterface::JOINT_ID_ENUM
|
||
| 311 | int iCubJointInterface::convert_motorid_to_enum(int id){
|
||
| 312 | enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(id);
|
||
| 313 | if(it == enum_id_bimap.left.end()) {
|
||
| 314 | //key does not exists, we are not interested in that dataset, return -1
|
||
| 315 | return -1;
|
||
| 316 | }
|
||
| 317 | return it->second;
|
||
| 318 | }
|
||
| 319 | |||
| 320 | //! special command to set eyelid angle
|
||
| 321 | //! \param angle in degrees
|
||
| 322 | void iCubJointInterface::set_eyelid_angle(double angle){
|
||
| 323 | if (emotion_port[0].getOutputCount()>0){
|
||
| 324 | //try to set the value based on the upper one
|
||
| 325 | //some guesses from the sim: S30 = 0° / S40 = 10°
|
||
| 326 | int opening = (25.0 + 0.8*angle);
|
||
| 327 | opening = min(48, max(24, opening));
|
||
| 328 | |||
| 329 | if (opening == lid_opening_previous){
|
||
| 330 | //no update necessary
|
||
| 331 | return;
|
||
| 332 | }
|
||
| 333 | |||
| 334 | lid_angle = angle;
|
||
| 335 | lid_opening_previous = opening;
|
||
| 336 | |||
| 337 | char buf[20];
|
||
| 338 | sprintf(buf, "S%2d", opening);
|
||
| 339 | |||
| 340 | //printf("> SETTING EYELID '%s' (%f -> %d)\n",buf,angle,opening);
|
||
| 341 | Bottle &cmd = emotion_port[0].prepare();
|
||
| 342 | cmd.clear();
|
||
| 343 | cmd.addString(buf);
|
||
| 344 | emotion_port[0].write();
|
||
| 345 | }else{
|
||
| 346 | printf("> ERROR: no icub emotion output\n");
|
||
| 347 | exit(EXIT_FAILURE);
|
||
| 348 | }
|
||
| 349 | }
|
||
| 350 | |||
| 351 | //! special command to set the eyebrow angle
|
||
| 352 | //! \param id {0=left, 1=right)
|
||
| 353 | //! \param angle in degrees
|
||
| 354 | void iCubJointInterface::set_eyebrow_angle(int id){
|
||
| 355 | int port_id;
|
||
| 356 | if (id == ICUB_ID_EYES_LEFT_BROW){
|
||
| 357 | port_id = 1;
|
||
| 358 | }else{
|
||
| 359 | port_id = 2;
|
||
| 360 | }
|
||
| 361 | |||
| 362 | if (emotion_port[port_id].getOutputCount()>0){
|
||
| 363 | double angle = target_angle[id];
|
||
| 364 | int icub_val = 0;
|
||
| 365 | |||
| 366 | //swap rotation direction:
|
||
| 367 | if (id==ICUB_ID_EYES_LEFT_BROW) angle = -angle;
|
||
| 368 | |||
| 369 | //convert to icub representation
|
||
| 370 | if (angle < -20){
|
||
| 371 | icub_val = 1;
|
||
| 372 | }else if (angle<10){
|
||
| 373 | icub_val = 2;
|
||
| 374 | }else if (angle<20){
|
||
| 375 | icub_val = 4;
|
||
| 376 | }else{
|
||
| 377 | icub_val = 8;
|
||
| 378 | }
|
||
| 379 | |||
| 380 | //make sure to update only on new values:
|
||
| 381 | if (icub_val == target_angle_previous[id]){
|
||
| 382 | //no updata necessary
|
||
| 383 | return;
|
||
| 384 | }
|
||
| 385 | |||
| 386 | //store actual value:
|
||
| 387 | target_angle_previous[id] = icub_val;
|
||
| 388 | |||
| 389 | |||
| 390 | string cmd_s;
|
||
| 391 | if (id==ICUB_ID_EYES_LEFT_BROW){
|
||
| 392 | cmd_s = "L0" + to_string(icub_val);
|
||
| 393 | }else{
|
||
| 394 | cmd_s = "R0" + to_string(icub_val);
|
||
| 395 | }
|
||
| 396 | |||
| 397 | printf("> SETTING EYEBROW %d (%f -> %s)\n",id,angle,cmd_s.c_str());
|
||
| 398 | |||
| 399 | Bottle &cmd = emotion_port[port_id].prepare();
|
||
| 400 | cmd.clear();
|
||
| 401 | cmd.addString(cmd_s);
|
||
| 402 | emotion_port[port_id].write();
|
||
| 403 | }else{
|
||
| 404 | printf("> ERROR: no icub emotion output\n");
|
||
| 405 | exit(EXIT_FAILURE);
|
||
| 406 | }
|
||
| 407 | }
|
||
| 408 | |||
| 409 | void iCubJointInterface::run(){
|
||
| 410 | iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this);
|
||
| 411 | data_receiver->start();
|
||
| 412 | }
|
||
| 413 | |||
| 414 | //! set the target position of a joint
|
||
| 415 | //! \param enum id of joint
|
||
| 416 | //! \param float value
|
||
| 417 | void iCubJointInterface::publish_target_position(int e){
|
||
| 418 | //first: convert humotion enum to our enum:
|
||
| 419 | int id = convert_enum_to_motorid(e);
|
||
| 420 | if (id == -1){
|
||
| 421 | return; //we are not interested in that data, so we just return here
|
||
| 422 | }
|
||
| 423 | |||
| 424 | if (id == ICUB_ID_NECK_PAN){
|
||
| 425 | //PAN seems to be swapped
|
||
| 426 | store_joint(ICUB_ID_NECK_PAN, -joint_target[e]);
|
||
| 427 | }else if ((id == ICUB_ID_EYES_LEFT_LR) || ( id == ICUB_ID_EYES_RIGHT_LR)){
|
||
| 428 | //icub handles eyes differently, we have to set pan angle + vergence
|
||
| 429 | float pan = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2;
|
||
| 430 | float vergence = (joint_target[ID_EYES_LEFT_LR] - joint_target[ID_EYES_RIGHT_LR]);
|
||
| 431 | //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);
|
||
| 432 | |||
| 433 | store_joint(ICUB_ID_EYES_PAN, pan);
|
||
| 434 | store_joint(ICUB_ID_EYES_VERGENCE, vergence);
|
||
| 435 | }else{
|
||
| 436 | store_joint(id, joint_target[e]);
|
||
| 437 | }
|
||
| 438 | }
|
||
| 439 | |||
| 440 | |||
| 441 | //! set the target position of a joint
|
||
| 442 | //! \param id of joint
|
||
| 443 | //! \param float value of position
|
||
| 444 | void iCubJointInterface::store_joint(int id, float value){
|
||
| 445 | //printf("> set joint %d = %f\n",id,value);
|
||
| 446 | target_angle[id] = value;
|
||
| 447 | //ipos->positionMove(id, value);
|
||
| 448 | }
|
||
| 449 | |||
| 450 | //! execute a move in position mode
|
||
| 451 | //! \param id of joint
|
||
| 452 | //! \param angle
|
||
| 453 | void iCubJointInterface::set_target_in_positionmode(int id, double value){
|
||
| 454 | if (id>ICUB_ID_EYES_VERGENCE){
|
||
| 455 | printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,value);
|
||
| 456 | return;
|
||
| 457 | }
|
||
| 458 | #if 0
|
||
| 459 | //set speed cacluated as in velocity + set position -> replicates smoothmotion from flobi?!
|
||
| 460 | |||
| 461 | //first: calculate necessary speed to reach the given target within the next clock tick:
|
||
| 462 | double distance = value - target_angle_previous[id];
|
||
| 463 | //make the motion smooth: we want to reach 85% of the target in the next iteration:
|
||
| 464 | //calculate speed for that:
|
||
| 465 | double speed = 0.85 * distance * ((double)MAIN_LOOP_FREQUENCY);
|
||
| 466 | |||
| 467 | //set up speed for controller:
|
||
| 468 | ipos->setRefSpeed(id, speed);
|
||
| 469 | #endif
|
||
| 470 | //execute motion
|
||
| 471 | ipos->positionMove(id, value); |
||
| 472 | } |
||
| 473 | |||
| 474 | //! execute a move in velocity mode
|
||
| 475 | //! \param id of joint
|
||
| 476 | //! \param angle
|
||
| 477 | void iCubJointInterface::set_target_in_velocitymode(int id, double value){ |
||
| 478 | //first: calculate necessary speed to reach the given target within the next clock tick:
|
||
| 479 | double distance = value - target_angle_previous[id];
|
||
| 480 | //make the motion smooth: we want to reach 85% of the target in the next iteration:
|
||
| 481 | distance = 0.85 * distance; |
||
| 482 | //calculate speed
|
||
| 483 | double speed = distance * ((double)MAIN_LOOP_FREQUENCY); |
||
| 484 | //execute:
|
||
| 485 | ivel->velocityMove(id, speed); |
||
| 486 | //if (id == ICUB_ID_NECK_PAN) printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],value,distance,speed);
|
||
| 487 | |||
| 488 | target_angle_previous[id] = get_ts_position(convert_motorid_to_enum(id)).get_newest_value(); |
||
| 489 | } |
||
| 490 | |||
| 491 | //! actually execute the scheduled motion commands
|
||
| 492 | void iCubJointInterface::execute_motion(){
|
||
| 493 | |||
| 494 | // set up neck and eye motion commands:
|
||
| 495 | if (POSITION_CONTROL){
|
||
| 496 | //position control
|
||
| 497 | for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){ |
||
| 498 | set_target_in_positionmode(i, target_angle[i]); |
||
| 499 | } |
||
| 500 | }else{
|
||
| 501 | //velocity control
|
||
| 502 | for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){ |
||
| 503 | set_target_in_velocitymode(i, target_angle[i]); |
||
| 504 | } |
||
| 505 | } |
||
| 506 | //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);
|
||
| 507 | |||
| 508 | |||
| 509 | //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:
|
||
| 510 | set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]); |
||
| 511 | |||
| 512 | //eyebrows are set using a special command as well:
|
||
| 513 | set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW); |
||
| 514 | set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW); |
||
| 515 | |||
| 516 | //mouth
|
||
| 517 | set_mouth(); |
||
| 518 | |||
| 519 | |||
| 520 | } |
||
| 521 | |||
| 522 | void iCubJointInterface::set_mouth(){
|
||
| 523 | //convert from 6DOF mouth displacement to icub leds:
|
||
| 524 | int led_value = 0; |
||
| 525 | |||
| 526 | //fetch center opening:
|
||
| 527 | double center_opening = target_angle[ICUB_ID_LIP_CENTER_LOWER] - target_angle[ICUB_ID_LIP_CENTER_UPPER];
|
||
| 528 | bool mouth_open = (center_opening>15.0)?true:false; |
||
| 529 | |||
| 530 | //side of mouth high or low?
|
||
| 531 | double center_avg = (target_angle[ICUB_ID_LIP_CENTER_LOWER] + target_angle[ICUB_ID_LIP_CENTER_UPPER])/2.0; |
||
| 532 | double left_avg = (target_angle[ICUB_ID_LIP_LEFT_LOWER] + target_angle[ICUB_ID_LIP_LEFT_UPPER])/2.0; |
||
| 533 | double right_avg = (target_angle[ICUB_ID_LIP_RIGHT_LOWER] + target_angle[ICUB_ID_LIP_RIGHT_UPPER])/2.0; |
||
| 534 | |||
| 535 | //happy, neutral or sad?
|
||
| 536 | double diff_l = center_avg - left_avg;
|
||
| 537 | double diff_r = center_avg - right_avg;
|
||
| 538 | double diff = (diff_l+diff_r)/2.0; |
||
| 539 | |||
| 540 | if (diff > 2.0){ |
||
| 541 | if (mouth_open){
|
||
| 542 | led_value = 0x14;
|
||
| 543 | }else{
|
||
| 544 | if (diff > 2.6){ |
||
| 545 | led_value = 0x0A;
|
||
| 546 | }else{
|
||
| 547 | led_value = 0x0B;
|
||
| 548 | } |
||
| 549 | } |
||
| 550 | }else if (diff < -3.0){ |
||
| 551 | if (mouth_open){
|
||
| 552 | led_value = 0x06;
|
||
| 553 | }else{
|
||
| 554 | led_value = 0x18;
|
||
| 555 | } |
||
| 556 | }else if (diff < -2.0){ |
||
| 557 | if (mouth_open){
|
||
| 558 | led_value = 0x04; //0x25; |
||
| 559 | }else{
|
||
| 560 | led_value = 0x08;
|
||
| 561 | } |
||
| 562 | }else{
|
||
| 563 | if (mouth_open){
|
||
| 564 | led_value = 0x16;
|
||
| 565 | }else{
|
||
| 566 | led_value = 0x08;
|
||
| 567 | } |
||
| 568 | } |
||
| 569 | |||
| 570 | |||
| 571 | if (led_value == previous_mouth_state){
|
||
| 572 | //no update necessary
|
||
| 573 | return;
|
||
| 574 | } |
||
| 575 | |||
| 576 | previous_mouth_state = led_value; |
||
| 577 | |||
| 578 | //convert to string:
|
||
| 579 | char buf[10]; |
||
| 580 | sprintf(buf, "M%02X",led_value);
|
||
| 581 | |||
| 582 | /*printf("> sending mouth '%s'\n",buf);
|
||
| 583 | printf("> mouth angles: %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_UPPER],target_angle[ICUB_ID_LIP_CENTER_UPPER],target_angle[ICUB_ID_LIP_RIGHT_UPPER]);
|
||
| 584 | printf(" mouth %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_LOWER],target_angle[ICUB_ID_LIP_CENTER_LOWER],target_angle[ICUB_ID_LIP_RIGHT_LOWER]);
|
||
| 585 | printf(" mouth open=%3.2f diff=%3.2f\n", center_opening, diff);*/
|
||
| 586 | |||
| 587 | //add mouth:
|
||
| 588 | Bottle &cmd = emotion_port[3].prepare();
|
||
| 589 | cmd.clear(); |
||
| 590 | cmd.addString(buf); |
||
| 591 | emotion_port[3].write();
|
||
| 592 | |||
| 593 | |||
| 594 | //store joint values which we do not handle on icub here:
|
||
| 595 | double timestamp = get_timestamp_ms();
|
||
| 596 | JointInterface::store_incoming_position(ID_LIP_LEFT_UPPER, target_angle[ICUB_ID_LIP_LEFT_UPPER], timestamp); |
||
| 597 | JointInterface::store_incoming_position(ID_LIP_LEFT_LOWER, target_angle[ICUB_ID_LIP_LEFT_LOWER], timestamp); |
||
| 598 | JointInterface::store_incoming_position(ID_LIP_CENTER_UPPER, target_angle[ICUB_ID_LIP_CENTER_UPPER], timestamp); |
||
| 599 | JointInterface::store_incoming_position(ID_LIP_CENTER_LOWER, target_angle[ICUB_ID_LIP_CENTER_LOWER], timestamp); |
||
| 600 | JointInterface::store_incoming_position(ID_LIP_RIGHT_UPPER, target_angle[ICUB_ID_LIP_RIGHT_UPPER], timestamp); |
||
| 601 | JointInterface::store_incoming_position(ID_LIP_RIGHT_LOWER, target_angle[ICUB_ID_LIP_RIGHT_LOWER], timestamp); |
||
| 602 | |||
| 603 | } |
||
| 604 | |||
| 605 | double iCubJointInterface::get_timestamp_ms(){
|
||
| 606 | struct timespec spec;
|
||
| 607 | clock_gettime(CLOCK_REALTIME, &spec); |
||
| 608 | return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6; |
||
| 609 | } |
||
| 610 | |||
| 611 | //! set the current position of a joint
|
||
| 612 | //! \param id of joint
|
||
| 613 | //! \param float value of position
|
||
| 614 | //! \param double timestamp
|
||
| 615 | void iCubJointInterface::fetch_position(int id, double value, double timestamp){ |
||
| 616 | //store joint based on id:
|
||
| 617 | switch(id){
|
||
| 618 | default:
|
||
| 619 | printf("> ERROR: unhandled joint id %d\n",id);
|
||
| 620 | return;
|
||
| 621 | |||
| 622 | case(100): |
||
| 623 | JointInterface::store_incoming_position(ID_EYES_RIGHT_LID_UPPER, lid_angle, timestamp); |
||
| 624 | break;
|
||
| 625 | |||
| 626 | case(2): |
||
| 627 | //PAN is inverted!
|
||
| 628 | JointInterface::store_incoming_position(ID_NECK_PAN, -value, timestamp); |
||
| 629 | break;
|
||
| 630 | |||
| 631 | case(0): |
||
| 632 | JointInterface::store_incoming_position(ID_NECK_TILT, value, timestamp); |
||
| 633 | break;
|
||
| 634 | |||
| 635 | case(1): |
||
| 636 | JointInterface::store_incoming_position(ID_NECK_ROLL, value, timestamp); |
||
| 637 | break;
|
||
| 638 | |||
| 639 | case(3): |
||
| 640 | JointInterface::store_incoming_position(ID_EYES_BOTH_UD, value, timestamp); |
||
| 641 | break;
|
||
| 642 | |||
| 643 | //icub handles eyes differently, we have to set pan angle + vergence
|
||
| 644 | case(4): {//pan |
||
| 645 | last_pos_eye_pan = value; |
||
| 646 | float left = last_pos_eye_pan + last_pos_eye_vergence/2.0; |
||
| 647 | float right = last_pos_eye_pan - last_pos_eye_vergence/2.0; |
||
| 648 | |||
| 649 | //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);
|
||
| 650 | JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp); |
||
| 651 | JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp); |
||
| 652 | break;
|
||
| 653 | } |
||
| 654 | |||
| 655 | case(5): { //vergence |
||
| 656 | last_pos_eye_vergence = value; |
||
| 657 | float left = last_pos_eye_pan + last_pos_eye_vergence/2.0; |
||
| 658 | float right = last_pos_eye_pan - last_pos_eye_vergence/2.0; |
||
| 659 | |||
| 660 | //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);
|
||
| 661 | JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp); |
||
| 662 | JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp); |
||
| 663 | break;
|
||
| 664 | } |
||
| 665 | } |
||
| 666 | |||
| 667 | |||
| 668 | } |
||
| 669 | |||
| 670 | //! set the current speed of a joint
|
||
| 671 | //! \param enum id of joint
|
||
| 672 | //! \param float value of speed
|
||
| 673 | //! \param double timestamp
|
||
| 674 | void iCubJointInterface::fetch_speed(int id, double value, double timestamp){ |
||
| 675 | |||
| 676 | switch(id){
|
||
| 677 | default:
|
||
| 678 | printf("> ERROR: unhandled joint id %d\n",id);
|
||
| 679 | return;
|
||
| 680 | |||
| 681 | case(2): |
||
| 682 | JointInterface::store_incoming_speed(ID_NECK_PAN, value, timestamp); |
||
| 683 | break;
|
||
| 684 | |||
| 685 | case(0): |
||
| 686 | JointInterface::store_incoming_speed(ID_NECK_TILT, value, timestamp); |
||
| 687 | break;
|
||
| 688 | |||
| 689 | case(1): |
||
| 690 | JointInterface::store_incoming_speed(ID_NECK_ROLL, value, timestamp); |
||
| 691 | break;
|
||
| 692 | |||
| 693 | case(3): |
||
| 694 | JointInterface::store_incoming_speed(ID_EYES_BOTH_UD, value, timestamp); |
||
| 695 | break;
|
||
| 696 | |||
| 697 | //icub handles eyes differently, we have to set pan angle + vergence
|
||
| 698 | case(4): {//pan |
||
| 699 | last_vel_eye_pan = value; |
||
| 700 | float left = last_vel_eye_pan + last_vel_eye_vergence/2.0; |
||
| 701 | float right = last_vel_eye_pan - last_vel_eye_vergence/2.0; |
||
| 702 | |||
| 703 | //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);
|
||
| 704 | JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp); |
||
| 705 | JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp); |
||
| 706 | break;
|
||
| 707 | } |
||
| 708 | |||
| 709 | case(5): { //vergence |
||
| 710 | last_vel_eye_pan = value; |
||
| 711 | float left = last_vel_eye_pan + last_vel_eye_vergence/2.0; |
||
| 712 | float right = last_vel_eye_pan - last_vel_eye_vergence/2.0; |
||
| 713 | |||
| 714 | //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);
|
||
| 715 | JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp); |
||
| 716 | JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp); |
||
| 717 | break;
|
||
| 718 | } |
||
| 719 | } |
||
| 720 | /*
|
||
| 721 | JointInterface::store_incoming_speed(ID_LIP_LEFT_UPPER, 0.0, timestamp);
|
||
| 722 | JointInterface::store_incoming_speed(ID_LIP_LEFT_LOWER, 0.0, timestamp);
|
||
| 723 | JointInterface::store_incoming_speed(ID_LIP_CENTER_UPPER, 0.0, timestamp);
|
||
| 724 | JointInterface::store_incoming_speed(ID_LIP_CENTER_LOWER, 0.0, timestamp);
|
||
| 725 | JointInterface::store_incoming_speed(ID_LIP_RIGHT_UPPER, 0.0, timestamp);
|
||
| 726 | JointInterface::store_incoming_speed(ID_LIP_RIGHT_LOWER, 0.0, timestamp);
|
||
| 727 | |||
| 728 | JointInterface::store_incoming_speed(ID_EYES_LEFT_LID_LOWER, 0.0, timestamp);
|
||
| 729 | JointInterface::store_incoming_speed(ID_EYES_LEFT_LID_UPPER, 0.0, timestamp);
|
||
| 730 | JointInterface::store_incoming_speed(ID_EYES_LEFT_BROW, 0.0, timestamp);
|
||
| 731 | |||
| 732 | JointInterface::store_incoming_speed(ID_EYES_RIGHT_LID_LOWER, 0.0, timestamp);
|
||
| 733 | JointInterface::store_incoming_speed(ID_EYES_RIGHT_LID_UPPER,0.0, timestamp);
|
||
| 734 | JointInterface::store_incoming_speed(ID_EYES_RIGHT_BROW, 0.0, timestamp);*/
|
||
| 735 | /*
|
||
| 736 | //fetch enum id:
|
||
| 737 | int e = convert_motorid_to_enum(device->get_device_id());
|
||
| 738 | if (e == -1){
|
||
| 739 | return; //we are not interested in that data, so we just return here
|
||
| 740 | }
|
||
| 741 | |||
| 742 | JointInterface::store_incoming_speed(e, device->get_register(XSC3_REGISTER_MOTORSPEED), timestamp);*/
|
||
| 743 | |||
| 744 | } |
||
| 745 | /*
|
||
| 746 | //! conversion table for humotion motor ids to our ids:
|
||
| 747 | //! \param enum from JointInterface::JOINT_ID_ENUM
|
||
| 748 | //! \return int value of motor id
|
||
| 749 | int HumotionJointInterface::convert_enum_to_motorid(int e){
|
||
| 750 | enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(e);
|
||
| 751 | if(it == enum_id_bimap.right.end()) {
|
||
| 752 | //key does not exists, we are not interested in that dataset, return -1
|
||
| 753 | return -1;
|
||
| 754 | }
|
||
| 755 | |||
| 756 | return it->second;
|
||
| 757 | }
|
||
| 758 | |||
| 759 | |||
| 760 | //! conversion table for our ids to humotion motor ids:
|
||
| 761 | //! \param int value of motor id
|
||
| 762 | //! \return enum from JointInterface::JOINT_ID_ENUM
|
||
| 763 | int HumotionJointInterface::convert_motorid_to_enum(int id){
|
||
| 764 | enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(id);
|
||
| 765 | if(it == enum_id_bimap.left.end()) {
|
||
| 766 | //key does not exists, we are not interested in that dataset, return -1
|
||
| 767 | return -1;
|
||
| 768 | }
|
||
| 769 | |||
| 770 | return it->second;
|
||
| 771 | }
|
||
| 772 | */
|
||
| 773 | |||
| 774 | //! prepare and enable a joint
|
||
| 775 | //! NOTE: this should also prefill the min/max positions for this joint
|
||
| 776 | //! \param the enum id of a joint
|
||
| 777 | void iCubJointInterface::enable_joint(int e){ |
||
| 778 | //FIXME ADD THIS:
|
||
| 779 | // enable the amplifier and the pid controller on each joint
|
||
| 780 | /*for (i = 0; i < nj; i++) {
|
||
| 781 | amp->enableAmp(i);
|
||
| 782 | pid->enablePid(i);
|
||
| 783 | }*/
|
||
| 784 | |||
| 785 | |||
| 786 | //set up smooth motion controller
|
||
| 787 | //step1: set up framerate
|
||
| 788 | //dev->set_register_blocking(XSC3_REGISTER_PID_RAMP, framerate, true);
|
||
| 789 | |||
| 790 | //step2: set controllertype:
|
||
| 791 | //printf("> activating smooth motion control for joint id 0x%02X (%s)\n", motor_id, joint_name.c_str());
|
||
| 792 | //dev->set_register_blocking(XSC3_REGISTER_PID_CONTROLLER, XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK, true);
|
||
| 793 | |||
| 794 | //step3: set pid controller:
|
||
| 795 | /*if ((e == ID_LIP_LEFT_UPPER) || (e == ID_LIP_LEFT_LOWER) || (e == ID_LIP_CENTER_UPPER) || (e == ID_LIP_CENTER_LOWER) || (e == ID_LIP_RIGHT_UPPER) || (e == ID_LIP_RIGHT_LOWER)){
|
||
| 796 | printf("> fixing PID i controller value for smooth motion (FIXME: restore old value!!)\n");
|
||
| 797 | dev->set_register_blocking(XSC3_REGISTER_CONST_I, 10, true);
|
||
| 798 | }*/
|
||
| 799 | |||
| 800 | //uint16_t result = dev->get_register_blocking_raw(XSC3_REGISTER_PID_CONTROLLER);
|
||
| 801 | |||
| 802 | //check if setting pid controllertype was successfull:
|
||
| 803 | /*if (result != XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK){
|
||
| 804 | printf("> failed to set smooth motion controller for joint %s (res=0x%04X)\n",joint_name.c_str(),result);
|
||
| 805 | exit(1);
|
||
| 806 | }*/
|
||
| 807 | |||
| 808 | //fetch min/max:
|
||
| 809 | // init_joint(e);
|
||
| 810 | |||
| 811 | //ok fine, now enable motor:
|
||
| 812 | //printf("> enabling motor %s\n", joint_name.c_str());
|
||
| 813 | //dev->set_register_blocking(XSC3_REGISTER_STATUS, XSC3_PROTOCOL_STATUS_BITMASK_MOTOR_ENABLE, true);
|
||
| 814 | |||
| 815 | } |
||
| 816 | |||
| 817 | void iCubJointInterface::store_min_max(IControlLimits *ilimits, int id, int e){ |
||
| 818 | double min, max;
|
||
| 819 | ilimits->getLimits(id, &min, &max); |
||
| 820 | joint_min[e] = min; |
||
| 821 | joint_max[e] = max; |
||
| 822 | } |
||
| 823 | |||
| 824 | //! initialise a joint (set up controller mode etc)
|
||
| 825 | //! \param joint enum
|
||
| 826 | void iCubJointInterface::init_joints(){
|
||
| 827 | store_min_max(ilimits, 0, ID_NECK_TILT);
|
||
| 828 | store_min_max(ilimits, 1, ID_NECK_ROLL);
|
||
| 829 | store_min_max(ilimits, 2, ID_NECK_PAN);
|
||
| 830 | store_min_max(ilimits, 3, ID_EYES_BOTH_UD);
|
||
| 831 | |||
| 832 | //icub handles eyes differently, we have to set pan angle + vergence
|
||
| 833 | double pan_min, pan_max, vergence_min, vergence_max;
|
||
| 834 | ilimits->getLimits(4, &pan_min, &pan_max);
|
||
| 835 | ilimits->getLimits(5, &vergence_min, &vergence_max);
|
||
| 836 | |||
| 837 | //this is not 100% correct, should be fixed:
|
||
| 838 | joint_min[ID_EYES_LEFT_LR] = pan_min; // - vergence_max/2;
|
||
| 839 | joint_max[ID_EYES_LEFT_LR] = pan_max; // - vergence_max/2;
|
||
| 840 | joint_min[ID_EYES_RIGHT_LR] = joint_min[ID_EYES_LEFT_LR]; |
||
| 841 | joint_max[ID_EYES_RIGHT_LR] = joint_max[ID_EYES_LEFT_LR]; |
||
| 842 | |||
| 843 | //eyelids:
|
||
| 844 | joint_min[ID_EYES_RIGHT_LID_UPPER] = -50; //24-30; |
||
| 845 | joint_max[ID_EYES_RIGHT_LID_UPPER] = 50; //48-30; |
||
| 846 | lid_angle = joint_max[ID_EYES_RIGHT_LID_UPPER]; |
||
| 847 | |||
| 848 | //eyebrows:
|
||
| 849 | joint_min[ID_EYES_LEFT_BROW] = -50;
|
||
| 850 | joint_max[ID_EYES_LEFT_BROW] = 50;
|
||
| 851 | joint_min[ID_EYES_RIGHT_BROW] = joint_min[ID_EYES_LEFT_BROW]; |
||
| 852 | joint_max[ID_EYES_RIGHT_BROW] = joint_max[ID_EYES_LEFT_BROW]; |
||
| 853 | |||
| 854 | //mouth:
|
||
| 855 | joint_min[ID_LIP_CENTER_UPPER] = 5;
|
||
| 856 | joint_max[ID_LIP_CENTER_UPPER] = 50;
|
||
| 857 | joint_min[ID_LIP_CENTER_LOWER] = 5;
|
||
| 858 | joint_max[ID_LIP_CENTER_LOWER] = 50;
|
||
| 859 | joint_min[ID_LIP_LEFT_UPPER] = 5;
|
||
| 860 | joint_max[ID_LIP_LEFT_UPPER] = 50;
|
||
| 861 | joint_min[ID_LIP_LEFT_LOWER] = 5;
|
||
| 862 | joint_max[ID_LIP_LEFT_LOWER] = 50;
|
||
| 863 | joint_min[ID_LIP_RIGHT_UPPER] = 5;
|
||
| 864 | joint_max[ID_LIP_RIGHT_UPPER] = 50;
|
||
| 865 | joint_min[ID_LIP_RIGHT_LOWER] = 5;
|
||
| 866 | joint_max[ID_LIP_RIGHT_LOWER] = 50;
|
||
| 867 | |||
| 868 | |||
| 869 | } |
||
| 870 | |||
| 871 | //! shutdown and disable a joint
|
||
| 872 | //! \param the enum id of a joint
|
||
| 873 | void iCubJointInterface::disable_joint(int e){ |
||
| 874 | /*
|
||
| 875 | //first: convert humotion enum to our enum:
|
||
| 876 | int motor_id = convert_enum_to_motorid(e);
|
||
| 877 | if (motor_id == -1){
|
||
| 878 | return; //we are not interested in that data, so we just return here
|
||
| 879 | }
|
||
| 880 | |||
| 881 | //fetch device:
|
||
| 882 | Device *dev = get_device(motor_id);
|
||
| 883 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 884 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 885 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 886 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 887 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 888 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 889 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 890 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 891 | printf("> FIXME: ADD DISABLE CODE\n");
|
||
| 892 | */
|
||
| 893 | } |
||
| 894 | 473a6a6c | Simon Schulz | #endif |