00001
00027 #include <ros/ros.h>
00028
00029 #include "sr_hand/hand/real_shadowhand.h"
00030
00031 #include <robot/robot.h>
00032 #include <robot/hand.h>
00033 #include <robot/hand_protocol.h>
00034
00035 namespace shadowrobot
00036 {
00037 RealShadowhand::RealShadowhand() :
00038 SRArticulatedRobot()
00039 {
00040
00041 if( robot_init() < 0 )
00042 {
00043 ROS_FATAL("Robot interface broken\n");
00044 ROS_BREAK();
00045 }
00046
00047
00048 if( hand_init() < 0 )
00049 {
00050 ROS_FATAL("Hand interface broken\n");
00051 ROS_BREAK();
00052 }
00053
00054 initializeMap();
00055 }
00056
00057 RealShadowhand::~RealShadowhand()
00058 {
00059 }
00060
00061 void RealShadowhand::initializeMap()
00062 {
00063 joints_map_mutex.lock();
00064 parameters_map_mutex.lock();
00065
00066 JointData tmpData;
00067
00068 tmpData.position = 0.0;
00069 tmpData.target = 0.0;
00070 tmpData.temperature = 0.0;
00071 tmpData.current = 0.0;
00072 tmpData.force = 0.0;
00073 tmpData.flags = "";
00074
00075 for( unsigned int i = 0; i < START_OF_ARM; i++ )
00076 {
00077 std::string name = hand_joints[i].joint_name;
00078 tmpData.jointIndex = i;
00079
00080 joints_map[name] = tmpData;
00081
00082 ROS_INFO("NAME[%d]: %s ", i, name.c_str());
00083 }
00084
00085 parameters_map["d"] = PARAM_d;
00086 parameters_map["i"] = PARAM_i;
00087 parameters_map["p"] = PARAM_p;
00088 parameters_map["target"] = PARAM_target;
00089 parameters_map["sensor"] = PARAM_sensor;
00090
00091 parameters_map["valve"] = PARAM_valve;
00092 parameters_map["dead"] = PARAM_deadband;
00093 parameters_map["deadband"] = PARAM_deadband;
00094 parameters_map["imax"] = PARAM_imax;
00095 parameters_map["offset"] = PARAM_output_offset;
00096 parameters_map["shift"] = PARAM_shift;
00097 parameters_map["max"] = PARAM_output_max;
00098
00100 parameters_map["motor_maxforce"] = PARAM_motor_maxforce;
00101 parameters_map["motor_safeforce"] = PARAM_motor_safeforce;
00102
00103 parameters_map["force_p"] = PARAM_force_p;
00104 parameters_map["force_i"] = PARAM_force_i;
00105 parameters_map["force_d"] = PARAM_force_d;
00106
00107 parameters_map["force_imax"] = PARAM_force_imax;
00108 parameters_map["force_out_shift"] = PARAM_force_out_shift;
00109 parameters_map["force_deadband"] = PARAM_force_deadband;
00110 parameters_map["force_offset"] = PARAM_force_offset;
00111
00112 parameters_map["sensor_imax"] = PARAM_sensor_imax;
00113 parameters_map["sensor_out_shift"] = PARAM_sensor_out_shift;
00114 parameters_map["sensor_deadband"] = PARAM_sensor_deadband;
00115 parameters_map["sensor_offset"] = PARAM_sensor_offset;
00116 parameters_map["max_temp"] = PARAM_max_temperature;
00117 parameters_map["max_temperature"] = PARAM_max_temperature;
00118 parameters_map["max_current"] = PARAM_max_current;
00119
00120 parameters_map_mutex.unlock();
00121 joints_map_mutex.unlock();
00122 }
00123
00124 short RealShadowhand::sendupdate( std::string joint_name, double target )
00125 {
00126 joints_map_mutex.lock();
00127
00128
00129 JointsMap::iterator iter = joints_map.find(joint_name);
00130
00131 if( iter != joints_map.end() )
00132 {
00133 JointData tmpData = joints_map.find(joint_name)->second;
00134 int index_hand_joints = tmpData.jointIndex;
00135
00136
00137 if( target < hand_joints[index_hand_joints].min_angle )
00138 target = hand_joints[index_hand_joints].min_angle;
00139 if( target > hand_joints[index_hand_joints].max_angle )
00140 target = hand_joints[index_hand_joints].max_angle;
00141
00142
00143 robot_update_sensor(&hand_joints[index_hand_joints].joint_target, target);
00144 joints_map_mutex.unlock();
00145 return 0;
00146 }
00147
00148 ROS_DEBUG("Joint %s not found", joint_name.c_str());
00149
00150 joints_map_mutex.unlock();
00151 return -1;
00152 }
00153
00154 JointData RealShadowhand::getJointData( std::string joint_name )
00155 {
00156 joints_map_mutex.lock();
00157
00158 JointsMap::iterator iter = joints_map.find(joint_name);
00159
00160
00161 if( iter == joints_map.end() )
00162 {
00163 ROS_ERROR("Joint %s not found.", joint_name.c_str());
00164 JointData noData;
00165 noData.position = 0.0;
00166 noData.target = 0.0;
00167 noData.temperature = 0.0;
00168 noData.current = 0.0;
00169 noData.force = 0.0;
00170 noData.flags = "";
00171 noData.jointIndex = 0;
00172
00173 joints_map_mutex.unlock();
00174 return noData;
00175 }
00176
00177
00178 JointData tmpData = iter->second;
00179 int index = tmpData.jointIndex;
00180
00181 tmpData.position = robot_read_sensor(&hand_joints[index].position);
00182 tmpData.target = robot_read_sensor(&hand_joints[index].joint_target);
00183
00184
00185 if( hand_joints[index].a.smartmotor.has_sensors )
00186 {
00187 tmpData.temperature = robot_read_sensor(&hand_joints[index].a.smartmotor.temperature);
00188 tmpData.current = robot_read_sensor(&hand_joints[index].a.smartmotor.current);
00189 tmpData.force = robot_read_sensor(&hand_joints[index].a.smartmotor.torque);
00190 tmpData.flags = "";
00191 }
00192
00193 joints_map[joint_name] = tmpData;
00194
00195 joints_map_mutex.unlock();
00196 return tmpData;
00197 }
00198
00199 SRArticulatedRobot::JointsMap RealShadowhand::getAllJointsData()
00200 {
00201
00202 for( JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it )
00203 getJointData(it->first);
00204
00205 JointsMap tmp = JointsMap(joints_map);
00206
00207
00208 return tmp;
00209 }
00210
00211 short RealShadowhand::setContrl( std::string contrlr_name, JointControllerData ctrlr_data )
00212 {
00213 parameters_map_mutex.lock();
00214
00215 struct controller_config myConfig;
00216 memset(&myConfig, 0, sizeof(myConfig));
00217
00218
00219 myConfig.nodename = contrlr_name.c_str();
00220
00221 controller_read_from_hardware(&myConfig);
00222 ROS_DEBUG("%s", controller_to_string(&myConfig));
00223
00224 for( unsigned int i = 0; i < ctrlr_data.data.size(); ++i )
00225 {
00226 std::string name = ctrlr_data.data[i].name;
00227 ParametersMap::iterator iter = parameters_map.find(name);
00228
00229
00230 if( iter == parameters_map.end() )
00231 {
00232 ROS_ERROR("Parameter %s not found.", name.c_str());
00233 continue;
00234 }
00235
00236
00237 controller_update_param(&myConfig, (controller_param)iter->second, ctrlr_data.data[i].value.c_str());
00238 }
00239
00240 parameters_map_mutex.unlock();
00241
00242 int result_ctrlr = controller_write_to_hardware(&myConfig);
00243 if( result_ctrlr )
00244 {
00245 ROS_ERROR("Failed to update contrlr (%s)", myConfig.nodename );
00246 return -1;
00247 }
00248
00249 return 0;
00250 }
00251
00252 JointControllerData RealShadowhand::getContrl( std::string contrlr_name )
00253 {
00254 struct controller_config myConfig;
00255 memset(&myConfig, 0, sizeof(myConfig));
00256
00257
00258 myConfig.nodename = contrlr_name.c_str();
00259
00260 controller_read_from_hardware(&myConfig);
00261
00262 JointControllerData tmp_data;
00263
00264 ROS_WARN("The get contrlr function is not implemented in the real shadowhand.");
00265
00266 return tmp_data;
00267
00268 }
00269
00270 short RealShadowhand::setConfig( std::vector<std::string> myConfig )
00271 {
00272 ROS_WARN("The set config function is not implemented in the real shadowhand.");
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286 return 0;
00287 }
00288
00289 void RealShadowhand::getConfig( std::string joint_name )
00290 {
00291 ROS_WARN("The get config function is not yet implement in the real shadow hand.");
00292 }
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311 std::vector<DiagnosticData> RealShadowhand::getDiagnostics()
00312 {
00313 std::vector<DiagnosticData> returnVector;
00314
00315 DiagnosticData tmpData;
00316
00317
00318 std::stringstream ss;
00319
00320
00321 for( unsigned int index = 0; index < START_OF_ARM; ++index )
00322 {
00323 tmpData.joint_name = std::string(hand_joints[index].joint_name);
00324 tmpData.level = 0;
00325
00326 tmpData.position = robot_read_sensor(&hand_joints[index].position);
00327 tmpData.target = robot_read_sensor(&hand_joints[index].joint_target);
00328
00329
00330 if( hand_joints[index].a.smartmotor.has_sensors )
00331 {
00332 tmpData.temperature = robot_read_sensor(&hand_joints[index].a.smartmotor.temperature);
00333 tmpData.current = robot_read_sensor(&hand_joints[index].a.smartmotor.current);
00334 tmpData.force = robot_read_sensor(&hand_joints[index].a.smartmotor.torque);
00335
00336
00337 uint64_t uuid = robot_node_id(hand_joints[index].a.smartmotor.nodename);
00338 struct hand_protocol_flags fl;
00339 fl = hand_protocol_get_status_flags(uuid);
00340 if( fl.valid )
00341 {
00342 struct hand_protocol_flags_smart_motor f;
00343 f = fl.u.smart_motor;
00344
00345
00346 ss.str("");
00347
00348 bool at_least_one_error_flag = false;
00349 if( f.nfault_pin )
00350 {
00351 at_least_one_error_flag = true;
00352 ss << "NFAULT ";
00353 ROS_WARN( "[%s]: NFAULT", hand_joints[index].joint_name );
00354 }
00355 if( f.temperature_cutout )
00356 {
00357 at_least_one_error_flag = true;
00358 ss << "TEMP ";
00359 }
00360 if( f.current_throttle )
00361 {
00362 at_least_one_error_flag = true;
00363 ss << "CURRENT ";
00364 }
00365 if( f.force_hard_limit )
00366 {
00367 at_least_one_error_flag = true;
00368 ss << "FORCE ";
00369 }
00370 if( hand_protocol_dead(uuid) )
00371 {
00372 at_least_one_error_flag = true;
00373 ss << "DEAD ";
00374 }
00375
00376
00377 tmpData.flags = ss.str();
00378
00379 if( at_least_one_error_flag )
00380 {
00381 ROS_WARN( "[%s]: %s", hand_joints[index].joint_name, (ss.str()).c_str());
00382 tmpData.level = 1;
00383 }
00384 }
00385 }
00386
00387 returnVector.push_back(tmpData);
00388 }
00389
00390 return returnVector;
00391 }
00392 }