← Back to branch summary

~shadowrobot/sr-ros-interface-ethercat/electric

« back to all changes in this revision

Viewing changes to sr_hand/src/hand/real_shadowhand.cpp

  • Committer: Ugo Cupcic
  • Date: 2012-08-08 10:17:21 UTC
  • mfrom: (1.1.552 shadow_robot_ethercat)
  • Revision ID: ugo@shadowrobot.com-20120808101721-lutmwmwmt06srqya
1.0.0 stable release for the etherCAT hardware

Show diffs side-by-side

added added

removed removed

Lines of Context:
sr_hand/src/hand/real_shadowhand.cpp
1
 
/**
2
 
 * @file   real_shadowhand.cpp
3
 
 * @author Ugo Cupcic <ugo@shadowrobot.com>, Contact <contact@shadowrobot.com>
4
 
 * @date   Tue May 25 17:50:42 2010
5
 
 *
6
 
 * @brief
7
 
 *
8
 
 *
9
 
 */
10
 
 
11
 
//our robot code
12
 
#include <robot/robot.h>
13
 
#include <robot/hand.h>
14
 
#include <robot/hand_protocol.h>
15
 
 
16
 
#include <ros/ros.h>
17
 
 
18
 
#include "sr_hand/hand/real_shadowhand.h"
19
 
 
20
 
namespace shadowhand
21
 
{
22
 
  RealShadowhand::RealShadowhand()
23
 
    : Shadowhand()
24
 
  {
25
 
    /* We need to attach the program to the robot, or fail if we cannot. */
26
 
    if (robot_init()<0)
27
 
      {
28
 
        ROS_FATAL("Robot interface broken\n");
29
 
        ROS_BREAK();
30
 
      }
31
 
 
32
 
    /* We need to attach the program to the hand as well, or fail if we cannot. */
33
 
    if (hand_init()<0)
34
 
      {
35
 
        ROS_FATAL("Hand interface broken\n");
36
 
        ROS_BREAK();
37
 
      }
38
 
 
39
 
 
40
 
    initializeMap();
41
 
  }
42
 
 
43
 
  RealShadowhand::~RealShadowhand()
44
 
  {
45
 
  }
46
 
 
47
 
  void RealShadowhand::initializeMap()
48
 
  {
49
 
    JointData tmpData;
50
 
 
51
 
    tmpData.position = 0.0;
52
 
    tmpData.target = 0.0;
53
 
    tmpData.temperature = 0.0;
54
 
    tmpData.current = 0.0;
55
 
    tmpData.force = 0.0;
56
 
    tmpData.flags = "";
57
 
 
58
 
    for(unsigned int i=0; i<NUM_HAND_JOINTS + 4; i++)
59
 
      {
60
 
        std::string name=hand_joints[i].joint_name;     
61
 
        tmpData.jointIndex = i;
62
 
 
63
 
        joints_map[name] = tmpData;
64
 
 
65
 
        ROS_INFO("NAME[%d]: %s ", i, name.c_str());
66
 
      }
67
 
 
68
 
 
69
 
    parameters_map["d"] = PARAM_d;
70
 
    parameters_map["i"] = PARAM_i;
71
 
    parameters_map["p"] = PARAM_p;
72
 
    parameters_map["target"] = PARAM_target;
73
 
    parameters_map["sensor"] = PARAM_sensor;
74
 
    
75
 
    parameters_map["valve"] = PARAM_valve;
76
 
    parameters_map["dead"] = PARAM_deadband;
77
 
    parameters_map["deadband"] = PARAM_deadband;
78
 
    parameters_map["imax"] = PARAM_imax;
79
 
    parameters_map["offset"] = PARAM_output_offset;
80
 
    parameters_map["shift"] = PARAM_shift;
81
 
    parameters_map["max"] = PARAM_output_max;
82
 
    
83
 
    //! the parameters for the motors
84
 
    parameters_map["motor_maxforce"] = PARAM_motor_maxforce;
85
 
    parameters_map["motor_safeforce"] = PARAM_motor_safeforce;
86
 
    
87
 
    parameters_map["force_p"] = PARAM_force_p;
88
 
    parameters_map["force_i"] = PARAM_force_i;
89
 
    parameters_map["force_d"] = PARAM_force_d;
90
 
    
91
 
    parameters_map["force_imax"] = PARAM_force_imax;
92
 
    parameters_map["force_out_shift"] = PARAM_force_out_shift;
93
 
    parameters_map["force_deadband"] = PARAM_force_deadband;
94
 
    parameters_map["force_offset"] = PARAM_force_offset;
95
 
    
96
 
    parameters_map["sensor_imax"] = PARAM_sensor_imax;
97
 
    parameters_map["sensor_out_shift"] = PARAM_sensor_out_shift;
98
 
    parameters_map["sensor_deadband"] = PARAM_sensor_deadband;
99
 
    parameters_map["sensor_offset"] = PARAM_sensor_offset;
100
 
    parameters_map["max_temp"] = PARAM_max_temperature;
101
 
    parameters_map["max_temperature"] = PARAM_max_temperature;
102
 
    parameters_map["max_current"] = PARAM_max_current;
103
 
  }
104
 
 
105
 
  short RealShadowhand::sendupdate(std::string joint_name, double target)
106
 
  {               
107
 
    //search the sensor in the hash_map
108
 
    JointsMap::iterator iter = joints_map.find(joint_name);
109
 
      
110
 
    if(iter != joints_map.end())
111
 
      {
112
 
        JointData tmpData = joints_map.find(joint_name)->second;
113
 
        int index_hand_joints = tmpData.jointIndex;
114
 
 
115
 
        //trim to the correct range
116
 
        if (target < hand_joints[index_hand_joints].min_angle)
117
 
          target = hand_joints[index_hand_joints].min_angle;
118
 
        if (target > hand_joints[index_hand_joints].max_angle)
119
 
          target = hand_joints[index_hand_joints].max_angle;
120
 
          
121
 
        //here we update the actual hand angles
122
 
        robot_update_sensor(&hand_joints[index_hand_joints].joint_target, target);
123
 
        return 0;
124
 
      }
125
 
    else
126
 
      {
127
 
        ROS_ERROR("Can't open sensor [%s]", joint_name.c_str());
128
 
        return -1;
129
 
      }
130
 
 
131
 
  }
132
 
 
133
 
  JointData RealShadowhand::getJointData(std::string joint_name)
134
 
  {
135
 
    JointsMap::iterator iter = joints_map.find(joint_name);
136
 
 
137
 
    //joint not found
138
 
    if(iter == joints_map.end())
139
 
      {
140
 
        ROS_ERROR("Joint %s not found.", joint_name.c_str());
141
 
        JointData noData;
142
 
        noData.position = 0.0;
143
 
        noData.target = 0.0;
144
 
        noData.temperature = 0.0;
145
 
        noData.current = 0.0;
146
 
        noData.force = 0.0;
147
 
        noData.flags = "";
148
 
        noData.jointIndex = 0;
149
 
        return noData;
150
 
      }
151
 
 
152
 
    //joint found
153
 
    JointData tmpData = iter->second;
154
 
    int index = tmpData.jointIndex;
155
 
 
156
 
    tmpData.position = robot_read_sensor(&hand_joints[index].position);
157
 
    tmpData.target = robot_read_sensor(&hand_joints[index].joint_target);
158
 
 
159
 
    //more information
160
 
    if (hand_joints[index].a.smartmotor.has_sensors) 
161
 
      {
162
 
        tmpData.temperature =  robot_read_sensor( &hand_joints[index].a.smartmotor.temperature );
163
 
        tmpData.current = robot_read_sensor( &hand_joints[index].a.smartmotor.current );
164
 
        tmpData.force = robot_read_sensor( &hand_joints[index].a.smartmotor.torque );
165
 
        tmpData.flags = "";
166
 
      }
167
 
 
168
 
    joints_map[joint_name] = tmpData;
169
 
 
170
 
 
171
 
    return tmpData;
172
 
    /*
173
 
 
174
 
      shadowhand::joints_data msg;
175
 
      std::vector<shadowhand::joint> myVector;
176
 
  
177
 
      sensor_msgs::JointState jointstate_msg;
178
 
  
179
 
      //get the data from the hand
180
 
      for (unsigned int i=0; i<NUM_HAND_JOINTS + 4; ++i)
181
 
      {
182
 
      shadowhand::joint joint;
183
 
      //name
184
 
      std::string jointName = hand_joints[i].joint_name;
185
 
      joint.joint_name = jointName;
186
 
            
187
 
      // /!\ don't publish the joint0s to joint_states otherwise
188
 
      // it won't match the model         
189
 
      bool isJoint0 = false;
190
 
      std::string joint0 = "J0";
191
 
 
192
 
      int foundAt = jointName.find(joint0);
193
 
      if( foundAt != -1 )
194
 
      isJoint0 = true;
195
 
      //      if( isJoint0 )
196
 
      //        {
197
 
      //          jointstate_msg.effort.push_back(0.0);
198
 
      //          }
199
 
 
200
 
      //publish 0 for joint1 effort
201
 
      std::string joint1 = "FJ1";
202
 
      foundAt = jointName.find(joint1);
203
 
      if( foundAt != -1 )
204
 
      jointstate_msg.effort.push_back(0.0);
205
 
      
206
 
      if( !isJoint0 )
207
 
      {
208
 
      //target & position
209
 
      joint.joint_position = robot_read_sensor(&hand_joints[i].position);
210
 
 
211
 
      jointstate_msg.name.push_back(hand_joints[i].joint_name);
212
 
 
213
 
      jointstate_msg.position.push_back( (robot_read_sensor(&hand_joints[i].position)*3.14159f / 180.0f));
214
 
 
215
 
      //WARNING the velocity vector contains the targets... IT's a
216
 
      //hack
217
 
      //TODO: remove this hack
218
 
      jointstate_msg.velocity.push_back((robot_read_sensor(&hand_joints[i].joint_target)*3.14159f / 180.0f));
219
 
      }
220
 
      joint.joint_target = robot_read_sensor(&hand_joints[i].joint_target);
221
 
      
222
 
      //more information
223
 
      if (hand_joints[i].a.smartmotor.has_sensors) 
224
 
      {
225
 
      //torque
226
 
      joint.joint_torque = robot_read_sensor( &hand_joints[i].a.smartmotor.torque );
227
 
      //if( !isJoint0 )
228
 
      jointstate_msg.effort.push_back(robot_read_sensor( &hand_joints[i].a.smartmotor.torque ));
229
 
          
230
 
      //temperature & current
231
 
      joint.joint_temperature = robot_read_sensor(&hand_joints[i].a.smartmotor.temperature);
232
 
      joint.joint_current = robot_read_sensor(&hand_joints[i].a.smartmotor.current);
233
 
          
234
 
      //check for error_flags
235
 
      uint64_t uuid = robot_node_id(hand_joints[i].a.smartmotor.nodename);
236
 
      struct hand_protocol_flags fl;
237
 
      fl = hand_protocol_get_status_flags(uuid);
238
 
      if (fl.valid) 
239
 
      {
240
 
      struct hand_protocol_flags_smart_motor f;
241
 
      f = fl.u.smart_motor;
242
 
      //concatenate the flags in a stringstream
243
 
      std::stringstream ss;
244
 
              
245
 
      bool at_least_one_error_flag = false;
246
 
      if( f.nfault_pin )
247
 
      {
248
 
      at_least_one_error_flag = true;
249
 
      ss << "NFAULT ";
250
 
      ROS_ERROR( "[%s]: NFAULT", hand_joints[i].joint_name );
251
 
      }
252
 
      if( f.temperature_cutout )
253
 
      {
254
 
      at_least_one_error_flag = true;
255
 
      ss << "TEMP ";
256
 
      }
257
 
      if( f.current_throttle )
258
 
      {
259
 
      at_least_one_error_flag = true;
260
 
      ss << "CURRENT ";
261
 
      }
262
 
      if( f.force_hard_limit )
263
 
      {
264
 
      at_least_one_error_flag = true;
265
 
      ss << "FORCE ";
266
 
      }
267
 
      if( hand_protocol_dead(uuid) )
268
 
      {
269
 
      at_least_one_error_flag = true;
270
 
      ss << "DEAD ";
271
 
      }
272
 
              
273
 
      //set the message flags
274
 
      joint.error_flag = ss.str();
275
 
      //if a flag is up, then print a warning as well
276
 
      if( at_least_one_error_flag )
277
 
      ROS_WARN( "[%s]: %s", hand_joints[i].joint_name, (ss.str()).c_str());
278
 
      }
279
 
      }
280
 
      
281
 
      myVector.push_back(joint);
282
 
      }
283
 
  
284
 
      //set the standard message
285
 
      msg.joints_list_length = NUM_HAND_JOINTS;
286
 
      msg.joints_list = myVector;
287
 
  
288
 
      //publish standard data (pos, target, current, temp, force, ...)
289
 
      shadowhand_pub.publish(msg);
290
 
  
291
 
      //publish JointState message
292
 
      shadowhand_jointstate_pub.publish(jointstate_msg);
293
 
    */
294
 
  }
295
 
 
296
 
  Shadowhand::JointsMap RealShadowhand::getAllJointsData()
297
 
  {
298
 
    //update the map for each joints
299
 
    for(JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it)
300
 
      getJointData(it->first);
301
 
 
302
 
    //return the map
303
 
    return joints_map;
304
 
  }
305
 
 
306
 
  short RealShadowhand::setContrl(std::string contrlr_name, JointControllerData ctrlr_data)
307
 
  {
308
 
    struct controller_config myConfig;
309
 
    memset(&myConfig, 0, sizeof(myConfig));
310
 
 
311
 
    //set the nodename from contrlr_name
312
 
    myConfig.nodename = contrlr_name.c_str();
313
 
 
314
 
    controller_read_from_hardware(&myConfig);
315
 
    ROS_DEBUG("%s", controller_to_string(&myConfig));
316
 
 
317
 
    for( unsigned int i = 0; i < ctrlr_data.data.size() ; ++i )
318
 
      {
319
 
        std::string name = ctrlr_data.data[i].name;
320
 
        ParametersMap::iterator iter = parameters_map.find(name);
321
 
       
322
 
        //parameter not found
323
 
        if(iter == parameters_map.end())
324
 
          {
325
 
            ROS_ERROR("Parameter %s not found.", name.c_str());
326
 
            continue;
327
 
          }
328
 
       
329
 
        //parameter found
330
 
        controller_update_param( &myConfig, (controller_param)iter->second, ctrlr_data.data[i].value.c_str() );
331
 
      }
332
 
 
333
 
    int result_ctrlr = controller_write_to_hardware(&myConfig);
334
 
    if (result_ctrlr)
335
 
      {
336
 
        ROS_ERROR("Failed to update contrlr (%s)", myConfig.nodename );
337
 
        return -1;
338
 
      }
339
 
 
340
 
    return 0;
341
 
  }  
342
 
 
343
 
  JointControllerData RealShadowhand::getContrl(std::string contrlr_name)
344
 
  {
345
 
    struct controller_config myConfig;
346
 
    memset(&myConfig, 0, sizeof(myConfig));
347
 
 
348
 
    //set the nodename from contrlr_name
349
 
    myConfig.nodename = contrlr_name.c_str();
350
 
 
351
 
    controller_read_from_hardware(&myConfig);
352
 
    
353
 
    JointControllerData tmp_data;
354
 
 
355
 
    ROS_WARN("The get contrlr function is not implemented in the real shadowhand.");
356
 
 
357
 
    return tmp_data;
358
 
 
359
 
  }
360
 
 
361
 
  short RealShadowhand::setConfig(std::vector<std::string> myConfig)
362
 
  {
363
 
    ROS_WARN("The set config function is not implemented in the real shadowhand.");
364
 
 
365
 
 
366
 
    /*
367
 
      hand_protocol_config_t cfg;
368
 
      hand_protocol_get_config(cfg);
369
 
 
370
 
      //set the transmit rate value
371
 
      int value = msg->rate_value;
372
 
      cfg->u.palm.tx_freq[num]=value;
373
 
 
374
 
      //send the config to the palm.
375
 
      hand_protocol_set_config(cfg);
376
 
    */
377
 
 
378
 
 
379
 
    return 0;
380
 
  }
381
 
 
382
 
  void RealShadowhand::getConfig(std::string joint_name)
383
 
  {
384
 
    ROS_WARN("The get config function is not yet implement in the real shadow hand.");
385
 
  }
386
 
 
387
 
/*
388
 
  char* ShadowhandDiagnosticer::get_setpoint_name(uint16_t s_num)
389
 
  {
390
 
    //  struct sensor temp;
391
 
    static char name_buff[256];
392
 
 
393
 
    snprintf(name_buff, 256, "%s.%d", "smart_motor_setpoints", s_num);
394
 
 
395
 
    //TODO: get the sensor name
396
 
    if (0==robot_channel_to_sensor("smart_motor_setpoints", s_num, &temp))
397
 
      {
398
 
      const char *name = robot_sensor_calibration_name(&temp);
399
 
      return (char*)name;
400
 
      }
401
 
    return name_buff;
402
 
  }*/
403
 
 
404
 
  std::vector<DiagnosticData> RealShadowhand::getDiagnostics()
405
 
  {
406
 
    std::vector<DiagnosticData> returnVector;
407
 
    
408
 
    DiagnosticData tmpData;
409
 
 
410
 
    //concatenate the flags in a stringstream
411
 
    std::stringstream ss;
412
 
 
413
 
 
414
 
    //get the data from the hand
415
 
    for (unsigned int index=0; index<NUM_HAND_JOINTS + 4; ++index)
416
 
      {
417
 
        tmpData.joint_name = std::string( hand_joints[index].joint_name );
418
 
        tmpData.level = 0;
419
 
 
420
 
        tmpData.position = robot_read_sensor(&hand_joints[index].position);
421
 
        tmpData.target = robot_read_sensor(&hand_joints[index].joint_target);
422
 
 
423
 
        //more information
424
 
        if (hand_joints[index].a.smartmotor.has_sensors) 
425
 
          {
426
 
            tmpData.temperature =  robot_read_sensor( &hand_joints[index].a.smartmotor.temperature );
427
 
            tmpData.current = robot_read_sensor( &hand_joints[index].a.smartmotor.current );
428
 
            tmpData.force = robot_read_sensor( &hand_joints[index].a.smartmotor.torque );
429
 
 
430
 
            //check for error_flags
431
 
            uint64_t uuid = robot_node_id(hand_joints[index].a.smartmotor.nodename);
432
 
            struct hand_protocol_flags fl;
433
 
            fl = hand_protocol_get_status_flags(uuid);
434
 
            if (fl.valid) 
435
 
              {
436
 
                struct hand_protocol_flags_smart_motor f;
437
 
                f = fl.u.smart_motor;
438
 
 
439
 
                //empty the stringstream
440
 
                ss.str("");
441
 
              
442
 
                bool at_least_one_error_flag = false;
443
 
                if( f.nfault_pin )
444
 
                  {
445
 
                    at_least_one_error_flag = true;
446
 
                    ss << "NFAULT ";
447
 
                    ROS_ERROR( "[%s]: NFAULT", hand_joints[index].joint_name );
448
 
                  }
449
 
                if( f.temperature_cutout )
450
 
                  {
451
 
                    at_least_one_error_flag = true;
452
 
                    ss << "TEMP ";
453
 
                  }
454
 
                if( f.current_throttle )
455
 
                  {
456
 
                    at_least_one_error_flag = true;
457
 
                    ss << "CURRENT ";
458
 
                  }
459
 
                if( f.force_hard_limit )
460
 
                  {
461
 
                    at_least_one_error_flag = true;
462
 
                    ss << "FORCE ";
463
 
                  }
464
 
                if( hand_protocol_dead(uuid) )
465
 
                  {
466
 
                    at_least_one_error_flag = true;
467
 
                    ss << "DEAD ";
468
 
                  }
469
 
              
470
 
                //set the message flags
471
 
                tmpData.flags = ss.str();
472
 
                //if a flag is up, then print a warning as well
473
 
                if( at_least_one_error_flag )
474
 
                  {
475
 
                    ROS_WARN( "[%s]: %s", hand_joints[index].joint_name, (ss.str()).c_str());
476
 
                    tmpData.level = 1;
477
 
                  }
478
 
              }
479
 
          }
480
 
        
481
 
        returnVector.push_back(tmpData);
482
 
      }
483
 
 
484
 
    return returnVector;
485
 
  }
486
 
} //end namespace