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