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