2
* @file sr_robot_lib.cpp
3
* @author Ugo Cupcic <ugo@shadowrobot.com>
4
* @date Wed Jun 22 10:06:14 2011
6
* Copyright 2011 Shadow Robot Company Ltd.
8
* This program is free software: you can redistribute it and/or modify it
9
* under the terms of the GNU General Public License as published by the Free
10
* Software Foundation, either version 2 of the License, or (at your option)
13
* This program is distributed in the hope that it will be useful, but WITHOUT
14
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
15
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
18
* You should have received a copy of the GNU General Public License along
19
* with this program. If not, see <http://www.gnu.org/licenses/>.
22
* @brief This is a generic robot library for Shadow Robot's Hardware.
27
#include "sr_robot_lib/sr_hand_lib.hpp"
29
#include <boost/foreach.hpp>
35
#include "sr_robot_lib/shadow_PSTs.hpp"
36
#include "sr_robot_lib/biotac.hpp"
38
namespace shadow_robot
40
#ifdef DEBUG_PUBLISHER
41
//max of 20 publishers for debug
42
const int SrRobotLib::nb_debug_publishers_const = 20;
43
const int SrRobotLib::debug_mutex_lock_wait_time = 100;
46
SrRobotLib::SrRobotLib(pr2_hardware_interface::HardwareInterface *hw)
47
: main_pic_idle_time(0), main_pic_idle_time_min(1000), nullify_demand_(false), motor_current_state(
48
operation_mode::device_update_state::INITIALIZATION), tactile_current_state(operation_mode::device_update_state::INITIALIZATION),
49
config_index(MOTOR_CONFIG_FIRST_VALUE),
52
//advertise the service to nullify the demand sent to the motor
53
// this makes it possible to easily stop the controllers.
54
nullify_demand_server_ = nh_tilde.advertiseService("nullify_demand", &SrRobotLib::nullify_demand_callback, this);
56
//reading the parameters to check for a specified default control type
57
// using FORCE control if no parameters are set
58
control_type_.control_type = sr_robot_msgs::ControlType::FORCE;
59
std::string default_control_mode;
60
nh_tilde.param<std::string>("default_control_mode", default_control_mode, "force");
61
if( default_control_mode.compare("PWM") == 0 )
63
ROS_INFO("Using PWM control.");
64
control_type_.control_type = sr_robot_msgs::ControlType::PWM;
68
ROS_INFO("Using TORQUE control.");
71
//initialising the change control type service
72
change_control_type_ = nh_tilde.advertiseService( "change_control_type", &SrRobotLib::change_control_type_callback_, this);
74
///Initialising service
75
motor_system_control_server_ = nh_tilde.advertiseService( "change_motor_system_controls", &SrRobotLib::motor_system_controls_callback_, this);
78
#ifdef DEBUG_PUBLISHER
79
debug_motor_indexes_and_data.resize(nb_debug_publishers_const);
80
for( int i = 0; i < nb_debug_publishers_const; ++i )
83
ss << "srh/debug_" << i;
84
debug_publishers.push_back(node_handle.advertise<std_msgs::Int16>(ss.str().c_str(),100));
90
void SrRobotLib::update(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS* status_data)
92
this->status_data = status_data;
94
//read the PIC idle time
95
main_pic_idle_time = status_data->idle_time_us;
96
if (status_data->idle_time_us < main_pic_idle_time_min)
97
main_pic_idle_time_min = status_data->idle_time_us;
99
//get the current timestamp
101
double timestamp = 0.0;
102
if (gettimeofday(&tv, NULL))
104
ROS_WARN("SrRobotLib: Failed to get system time, timestamp in state will be zero");
108
timestamp = double(tv.tv_sec) + double(tv.tv_usec) / 1.0e+6;
111
//First we read the joints informations
112
boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp = joints_vector.begin();
113
for (; joint_tmp != joints_vector.end(); ++joint_tmp)
115
actuator = (joint_tmp->motor->actuator);
117
motor_index_full = joint_tmp->motor->motor_id;
118
actuator->state_.is_enabled_ = 1;
119
actuator->state_.device_id_ = motor_index_full;
120
actuator->state_.halted_ = false;
122
//Fill in the tactiles.
123
if( tactiles != NULL )
124
actuator->state_.tactiles_ = tactiles->get_tactile_data();
126
//calibrate the joint and update the position.
127
calibrate_joint(joint_tmp);
129
//add the last position to the queue
130
joint_tmp->motor->actuator->state_.timestamp_ = timestamp;
132
//filter the position and velocity
133
std::pair<double, double> pos_and_velocity = joint_tmp->pos_filter.compute(
134
joint_tmp->motor->actuator->state_.position_unfiltered_, timestamp);
135
//reset the position to the filtered value
136
joint_tmp->motor->actuator->state_.position_ = pos_and_velocity.first;
137
//set the velocity to the filtered velocity
138
joint_tmp->motor->actuator->state_.velocity_ = pos_and_velocity.second;
141
std::pair<double, double> effort_and_effort_d = joint_tmp->effort_filter.compute(
142
joint_tmp->motor->actuator->state_.force_unfiltered_, timestamp);
143
joint_tmp->motor->actuator->state_.last_measured_effort_ = effort_and_effort_d.first;
145
//if no motor is associated to this joint, then continue
146
if ((motor_index_full == -1))
149
//get the remaining information.
150
bool read_motor_info = false;
152
if (status_data->which_motors == 0)
154
//We sampled the even motor numbers
155
if (motor_index_full % 2 == 0)
156
read_motor_info = true;
160
//we sampled the odd motor numbers
161
if (motor_index_full % 2 == 1)
162
read_motor_info = true;
165
//the position of the motor in the message
166
// is different from the motor index:
167
// the motor indexes range from 0 to 19
168
// while the message contains information
169
// for only 10 motors.
170
index_motor_in_msg = motor_index_full / 2;
172
//setting the position of the motor in the message,
173
// we'll print that in the diagnostics.
174
joint_tmp->motor->msg_motor_id = index_motor_in_msg;
176
//ok now we read the info and add it to the actuator state
178
read_additional_data(joint_tmp);
181
//then we read the tactile sensors information
182
if (tactile_current_state == operation_mode::device_update_state::INITIALIZATION)
184
if( tactiles_init != NULL )
185
tactiles_init->update(status_data);
189
if( tactiles != NULL )
190
tactiles->update(status_data);
194
void SrRobotLib::build_motor_command(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND* command)
196
if (motor_current_state == operation_mode::device_update_state::INITIALIZATION)
198
motor_current_state = motor_updater_->build_init_command(command);
202
//build the motor command
203
motor_current_state = motor_updater_->build_command(command);
206
if (tactile_current_state == operation_mode::device_update_state::INITIALIZATION)
208
if (tactiles_init->sensor_updater->build_init_command(command)
209
!= operation_mode::device_update_state::INITIALIZATION)
211
tactile_current_state = operation_mode::device_update_state::OPERATION;
213
switch (tactiles_init->tactiles_vector->at(0).which_sensor)
215
case TACTILE_SENSOR_PROTOCOL_TYPE_PST3:
216
tactiles = boost::shared_ptr<tactiles::ShadowPSTs>(
217
new tactiles::ShadowPSTs(pst3_sensor_update_rate_configs_vector,
218
operation_mode::device_update_state::OPERATION,
219
tactiles_init->tactiles_vector));
221
ROS_INFO("PST3 tactiles initialized");
224
case TACTILE_SENSOR_PROTOCOL_TYPE_BIOTAC_2_3:
225
tactiles = boost::shared_ptr<tactiles::Biotac>(
226
new tactiles::Biotac(biotac_sensor_update_rate_configs_vector, operation_mode::device_update_state::OPERATION,
227
tactiles_init->tactiles_vector));
229
ROS_INFO("Biotac tactiles initialized");
232
case TACTILE_SENSOR_PROTOCOL_TYPE_INVALID:
233
ROS_WARN_STREAM("TACTILE_SENSOR_PROTOCOL_TYPE_INVALID!!");
235
case TACTILE_SENSOR_PROTOCOL_TYPE_CONFLICTING:
236
ROS_WARN_STREAM("TACTILE_SENSOR_PROTOCOL_TYPE_CONFLICTING!!");
242
tactile_current_state = tactiles->sensor_updater->build_command(command);
245
// Now we chose the command to send to the motor
246
// by default we send a torque demand (we're running
247
// the force control on the motors), but if we have a waiting
248
// configuration, a reset command, or a motor system control
249
// request then we send the configuration
251
if ( reconfig_queue.empty() && reset_motors_queue.empty()
252
&& motor_system_control_flags_.empty() )
255
switch( control_type_.control_type )
257
case sr_robot_msgs::ControlType::FORCE:
258
command->to_motor_data_type = MOTOR_DEMAND_TORQUE;
260
case sr_robot_msgs::ControlType::PWM:
261
command->to_motor_data_type = MOTOR_DEMAND_PWM;
265
command->to_motor_data_type = MOTOR_DEMAND_TORQUE;
269
//loop on all the joints and update their motor: we're sending commands to all the motors.
270
boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp = joints_vector.begin();
271
for (; joint_tmp != joints_vector.end(); ++joint_tmp)
273
if (joint_tmp->has_motor)
275
if( !nullify_demand_ )
277
//We send the computed demand
278
command->motor_data[joint_tmp->motor->motor_id] = joint_tmp->motor->actuator->command_.effort_;
282
//We want to send a demand of 0
283
command->motor_data[joint_tmp->motor->motor_id] = 0;
286
#ifdef DEBUG_PUBLISHER
287
//publish the debug values for the given motors.
288
// NB: debug_motor_indexes_and_data is smaller
289
// than debug_publishers.
290
int publisher_index = 0;
291
boost::shared_ptr<std::pair<int,int> > debug_pair;
292
if( debug_mutex.try_lock() )
294
BOOST_FOREACH(debug_pair, debug_motor_indexes_and_data)
296
if( debug_pair != NULL )
298
//check if we want to publish some data for the current motor
299
if( debug_pair->first == joint_tmp->motor->motor_id )
301
//check if it's the correct data
302
if( debug_pair->second == -1 )
304
msg_debug.data = joint_tmp->motor->actuator->command_.effort_;
305
debug_publishers[publisher_index].publish(msg_debug);
312
debug_mutex.unlock();
316
joint_tmp->motor->actuator->state_.last_commanded_effort_ = joint_tmp->motor->actuator->command_.effort_;
318
} // end for each joint
319
} //endif reconfig_queue.empty()
322
if ( !motor_system_control_flags_.empty() )
324
//treat the first waiting system control and remove it from the queue
325
std::vector<sr_robot_msgs::MotorSystemControls> system_controls_to_send;
326
system_controls_to_send = motor_system_control_flags_.front();
327
motor_system_control_flags_.pop();
329
//set the correct type of command to send to the hand.
330
command->to_motor_data_type = MOTOR_SYSTEM_CONTROLS;
332
std::vector<sr_robot_msgs::MotorSystemControls>::iterator it;
333
for( it = system_controls_to_send.begin(); it != system_controls_to_send.end(); ++it)
335
short combined_flags = 0;
336
if( it->enable_backlash_compensation )
337
combined_flags |= MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_ENABLE;
339
combined_flags |= MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_DISABLE;
341
if( it->increase_sgl_tracking )
342
combined_flags |= MOTOR_SYSTEM_CONTROL_SGL_TRACKING_INC;
343
if( it->decrease_sgl_tracking )
344
combined_flags |= MOTOR_SYSTEM_CONTROL_SGL_TRACKING_DEC;
346
if( it->increase_sgr_tracking )
347
combined_flags |= MOTOR_SYSTEM_CONTROL_SGR_TRACKING_INC;
348
if( it->decrease_sgr_tracking )
349
combined_flags |= MOTOR_SYSTEM_CONTROL_SGR_TRACKING_DEC;
351
if( it->initiate_jiggling )
352
combined_flags |= MOTOR_SYSTEM_CONTROL_INITIATE_JIGGLING;
354
if( it->write_config_to_eeprom )
355
combined_flags |= MOTOR_SYSTEM_CONTROL_EEPROM_WRITE;
357
command->motor_data[ it->motor_id ] = combined_flags;
359
} //end if motor_system_control_flags_.empty
362
if (!reset_motors_queue.empty())
364
//reset the CAN messages counters for the motor we're going to reset.
365
short motor_id = reset_motors_queue.front();
366
boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp = joints_vector.begin();
367
for (; joint_tmp != joints_vector.end(); ++joint_tmp)
369
if( joint_tmp->motor->motor_id == motor_id )
371
joint_tmp->motor->actuator->state_.can_msgs_transmitted_ = 0;
372
joint_tmp->motor->actuator->state_.can_msgs_received_ = 0;
376
//we have some reset command waiting.
377
// We'll send all of them
378
command->to_motor_data_type = MOTOR_SYSTEM_RESET;
380
while (!reset_motors_queue.empty())
382
motor_id = reset_motors_queue.front();
383
reset_motors_queue.pop();
385
// we send the MOTOR_RESET_SYSTEM_KEY
386
// and the motor id (on the bus)
387
crc_unions::union16 to_send;
388
to_send.byte[1] = MOTOR_SYSTEM_RESET_KEY >> 8;
390
to_send.byte[0] = motor_id - 10;
392
to_send.byte[0] = motor_id;
394
command->motor_data[motor_id] = to_send.word;
396
} // end if reset queue not empty
399
if (!reconfig_queue.empty())
401
//we have a waiting config:
402
// we need to send all the config, finishing by the
403
// CRC. We'll remove the config from the queue only
404
// when the whole config has been sent
406
// the motor data type correspond to the index
407
// in the config array.
408
command->to_motor_data_type = static_cast<TO_MOTOR_DATA_TYPE>(config_index);
410
//convert the motor index to the index of the motor in the message
411
int motor_index = reconfig_queue.front().first;
413
//set the data we want to send to the given motor
414
command->motor_data[motor_index] = reconfig_queue.front().second[config_index].word;
416
//We're now sending the CRC. We need to send the correct CRC to
417
// the motor we updated, and CRC=0 to all the other motors in its
418
// group (odd/even) to tell them to ignore the new
420
// Once the config has been transmitted, pop the element
421
// and reset the config_index to the beginning of the
423
if (config_index == static_cast<int>(MOTOR_CONFIG_CRC))
425
//loop on all the motors and send a CRC of 0
426
// except for the motor we're reconfiguring
427
for (int i = 0; i < NUM_MOTORS; ++i)
429
if (i != motor_index)
430
command->motor_data[i] = 0;
433
//reset the config_index and remove the configuration
434
// we just sent from the configurations queue
435
reconfig_queue.pop();
436
config_index = MOTOR_CONFIG_FIRST_VALUE;
440
} //end if reconfig queue not empty
441
} // end else reset_queue.empty
442
} // end else motor_system_control_flags_.empty
443
} //endelse reconfig_queue.empty() && reset_queue.empty()
446
void SrRobotLib::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
447
diagnostic_updater::DiagnosticStatusWrapper &d)
449
boost::ptr_vector<shadow_joints::Joint>::iterator joint = joints_vector.begin();
450
for (; joint != joints_vector.end(); ++joint)
452
std::stringstream name;
454
name << "SRDMotor " << joint->joint_name;
457
if (joint->has_motor)
459
const sr_actuator::SrActuatorState *state(&(joint->motor->actuator)->state_);
461
if (joint->motor->motor_ok)
463
if (joint->motor->bad_data)
465
d.summary(d.WARN, "WARNING, bad CAN data received");
468
d.addf("Motor ID", "%d", joint->motor->motor_id);
470
else //the data is good
472
d.summary(d.OK, "OK");
475
d.addf("Motor ID", "%d", joint->motor->motor_id);
476
d.addf("Motor ID in message", "%d", joint->motor->msg_motor_id);
477
d.addf("Serial Number", "%d", state->serial_number );
478
d.addf("Assembly date", "%d / %d / %d", state->assembly_data_day, state->assembly_data_month, state->assembly_data_year );
480
d.addf("Strain Gauge Left", "%d", state->strain_gauge_left_);
481
d.addf("Strain Gauge Right", "%d", state->strain_gauge_right_);
483
//if some flags are set
484
std::stringstream ss;
485
if (state->flags_.size() > 0)
487
int flags_seriousness = d.OK;
488
std::pair<std::string, bool> flag;
489
BOOST_FOREACH(flag, state->flags_)
493
flags_seriousness = d.ERROR;
495
if (flags_seriousness != d.ERROR)
496
flags_seriousness = d.WARN;
497
ss << flag.first << " | ";
499
d.summary(flags_seriousness, ss.str().c_str());
503
d.addf("Motor Flags", "%s", ss.str().c_str());
505
d.addf("Measured PWM", "%d", state->pwm_);
506
d.addf("Measured Current", "%f", state->last_measured_current_);
507
d.addf("Measured Voltage", "%f", state->motor_voltage_);
508
d.addf("Measured Effort", "%f", state->last_measured_effort_);
509
d.addf("Temperature", "%f", state->temperature_);
511
d.addf("Unfiltered position", "%f", state->position_unfiltered_);
512
d.addf("Unfiltered force", "%f", state->force_unfiltered_);
514
d.addf("Gear Ratio", "%d", state->motor_gear_ratio);
516
d.addf("Number of CAN messages received", "%lld", state->can_msgs_received_);
517
d.addf("Number of CAN messages transmitted", "%lld", state->can_msgs_transmitted_);
519
d.addf("Force control Pterm", "%d", state->force_control_pterm);
520
d.addf("Force control Iterm", "%d", state->force_control_iterm);
521
d.addf("Force control Dterm", "%d", state->force_control_dterm);
523
d.addf("Force control F", "%d", state->force_control_f_);
524
d.addf("Force control P", "%d", state->force_control_p_);
525
d.addf("Force control I", "%d", state->force_control_i_);
526
d.addf("Force control D", "%d", state->force_control_d_);
527
d.addf("Force control Imax", "%d", state->force_control_imax_);
528
d.addf("Force control Deadband", "%d", state->force_control_deadband_);
529
d.addf("Force control Frequency", "%d", state->force_control_frequency_);
531
if (state->force_control_sign_ == 0)
532
d.addf("Force control Sign", "+");
534
d.addf("Force control Sign", "-");
536
d.addf("Last Commanded Effort", "%f", state->last_commanded_effort_);
538
d.addf("Encoder Position", "%f", state->position_);
540
if (state->firmware_modified_)
541
d.addf("Firmware svn revision (server / pic / modified)", "%d / %d / True",
542
state->server_firmware_svn_revision_, state->pic_firmware_svn_revision_);
544
d.addf("Firmware svn revision (server / pic / modified)", "%d / %d / False",
545
state->server_firmware_svn_revision_, state->pic_firmware_svn_revision_);
550
d.summary(d.ERROR, "Motor error");
552
d.addf("Motor ID", "%d", joint->motor->motor_id);
557
d.summary(d.OK, "No motor associated to this joint");
562
} //end for each joints
566
void SrRobotLib::calibrate_joint(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp)
568
actuator->state_.raw_sensor_values_.clear();
569
actuator->state_.calibrated_sensor_values_.clear();
571
if (joint_tmp->joint_to_sensor.calibrate_after_combining_sensors)
573
//first we combine the different sensors and then we
574
// calibrate the value we obtained. This is used for
575
// some compound sensors ( THJ5 = cal(THJ5A + THJ5B))
576
double raw_position = 0.0;
577
//when combining the values, we use the coefficient imported
578
// from the sensor_to_joint.yaml file (in sr_edc_launch/config)
579
BOOST_FOREACH(shadow_joints::PartialJointToSensor joint_to_sensor, joint_tmp->joint_to_sensor.joint_to_sensor_vector)
581
int tmp_raw = status_data->sensors[joint_to_sensor.sensor_id];
582
actuator->state_.raw_sensor_values_.push_back(tmp_raw);
583
raw_position += static_cast<double>(tmp_raw) * joint_to_sensor.coeff;
586
//and now we calibrate
587
calibration_tmp = calibration_map.find(joint_tmp->joint_name);
588
actuator->state_.position_unfiltered_ = calibration_tmp->compute(static_cast<double>(raw_position));
592
//we calibrate the different sensors first and we combine the calibrated
593
//values. This is used in the joint 0s for example ( J0 = cal(J1)+cal(J2) )
594
double calibrated_position = 0.0;
595
shadow_joints::PartialJointToSensor joint_to_sensor;
596
std::string sensor_name;
598
ROS_DEBUG_STREAM("Combining actuator " << joint_tmp->joint_name);
600
for (unsigned int index_joint_to_sensor = 0;
601
index_joint_to_sensor < joint_tmp->joint_to_sensor.joint_to_sensor_vector.size(); ++index_joint_to_sensor)
603
joint_to_sensor = joint_tmp->joint_to_sensor.joint_to_sensor_vector[index_joint_to_sensor];
604
sensor_name = joint_tmp->joint_to_sensor.sensor_names[index_joint_to_sensor];
606
//get the raw position
607
int raw_pos = status_data->sensors[joint_to_sensor.sensor_id];
608
//push the new raw values
609
actuator->state_.raw_sensor_values_.push_back(raw_pos);
611
//calibrate and then combine
612
calibration_tmp = calibration_map.find(sensor_name);
613
double tmp_cal_value = calibration_tmp->compute(static_cast<double>(raw_pos));
615
//push the new calibrated values.
616
actuator->state_.calibrated_sensor_values_.push_back(tmp_cal_value);
618
calibrated_position += tmp_cal_value * joint_to_sensor.coeff;
620
ROS_DEBUG_STREAM(" -> "<< sensor_name<< " raw = " << raw_pos << " calibrated = " << calibrated_position);
622
actuator->state_.position_unfiltered_ = calibrated_position;
623
ROS_DEBUG_STREAM(" => "<< actuator->state_.position_unfiltered_);
625
} //end calibrate_joint()
627
void SrRobotLib::read_additional_data(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp)
629
//check the masks to see if the CAN messages arrived to the motors
630
//the flag should be set to 1 for each motor
631
joint_tmp->motor->motor_ok = sr_math_utils::is_bit_mask_index_true(status_data->which_motor_data_arrived,
634
//check the masks to see if a bad CAN message arrived
635
//the flag should be 0
636
joint_tmp->motor->bad_data = sr_math_utils::is_bit_mask_index_true(status_data->which_motor_data_had_errors,
639
crc_unions::union16 tmp_value;
641
if (joint_tmp->motor->motor_ok && !(joint_tmp->motor->bad_data))
643
#ifdef DEBUG_PUBLISHER
644
int publisher_index = 0;
645
//publish the debug values for the given motors.
646
// NB: debug_motor_indexes_and_data is smaller
647
// than debug_publishers.
648
boost::shared_ptr<std::pair<int,int> > debug_pair;
650
if( debug_mutex.try_lock() )
652
BOOST_FOREACH(debug_pair, debug_motor_indexes_and_data)
654
if( debug_pair != NULL )
656
//check if we want to publish some data for the current motor
657
if( debug_pair->first == joint_tmp->motor->motor_id )
659
//if < 0, then we're not asking for a FROM_MOTOR_DATA_TYPE
660
if( debug_pair->second > 0 )
662
//check if it's the correct data
663
if( debug_pair->second == status_data->motor_data_type )
665
msg_debug.data = status_data->motor_data_packet[index_motor_in_msg].misc;
666
debug_publishers[publisher_index].publish(msg_debug);
674
debug_mutex.unlock();
678
//we received the data and it was correct
679
bool read_torque = true;
680
switch (status_data->motor_data_type)
683
actuator->state_.strain_gauge_left_ =
684
static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].misc);
686
#ifdef DEBUG_PUBLISHER
687
if( joint_tmp->motor->motor_id == 8 )
689
//ROS_ERROR_STREAM("SGL " <<actuator->state_.strain_gauge_left_);
690
msg_debug.data = actuator->state_.strain_gauge_left_;
691
debug_publishers[0].publish(msg_debug);
696
actuator->state_.strain_gauge_right_ =
697
static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].misc);
699
#ifdef DEBUG_PUBLISHER
700
if( joint_tmp->motor->motor_id == 8 )
702
//ROS_ERROR_STREAM("SGR " <<actuator->state_.strain_gauge_right_);
703
msg_debug.data = actuator->state_.strain_gauge_right_;
704
debug_publishers[1].publish(msg_debug);
709
actuator->state_.pwm_ =
710
static_cast<int>(static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].misc));
712
case MOTOR_DATA_FLAGS:
713
actuator->state_.flags_ = humanize_flags(status_data->motor_data_packet[index_motor_in_msg].misc);
715
case MOTOR_DATA_CURRENT:
716
//we're receiving the current in milli amps
717
actuator->state_.last_measured_current_ =
718
static_cast<double>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc))
721
#ifdef DEBUG_PUBLISHER
722
if( joint_tmp->motor->motor_id == 8 )
724
//ROS_ERROR_STREAM("Current " <<actuator->state_.last_measured_current_);
725
msg_debug.data = static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
726
debug_publishers[2].publish(msg_debug);
730
case MOTOR_DATA_VOLTAGE:
731
actuator->state_.motor_voltage_ =
732
static_cast<double>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc)) / 256.0;
734
#ifdef DEBUG_PUBLISHER
735
if( joint_tmp->motor->motor_id == 8 )
737
//ROS_ERROR_STREAM("Voltage " <<actuator->state_.motor_voltage_);
738
msg_debug.data = static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
739
debug_publishers[3].publish(msg_debug);
743
case MOTOR_DATA_TEMPERATURE:
744
actuator->state_.temperature_ =
745
static_cast<double>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc)) / 256.0;
747
case MOTOR_DATA_CAN_NUM_RECEIVED:
748
// those are 16 bits values and will overflow -> we compute the real value.
749
// This needs to be updated faster than the overflowing period (which should be roughly every 30s)
750
actuator->state_.can_msgs_received_ = sr_math_utils::counter_with_overflow(
751
actuator->state_.can_msgs_received_,
752
static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
754
case MOTOR_DATA_CAN_NUM_TRANSMITTED:
755
// those are 16 bits values and will overflow -> we compute the real value.
756
// This needs to be updated faster than the overflowing period (which should be roughly every 30s)
757
actuator->state_.can_msgs_transmitted_ = sr_math_utils::counter_with_overflow(
758
actuator->state_.can_msgs_received_,
759
static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
762
case MOTOR_DATA_SLOW_MISC:
763
//We received a slow data:
764
// the slow data type is contained in .torque, while
765
// the actual data is in .misc.
766
// so we won't read torque information from .torque
769
switch (static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].torque))
771
case MOTOR_SLOW_DATA_SVN_REVISION:
772
actuator->state_.pic_firmware_svn_revision_ =
773
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
775
case MOTOR_SLOW_DATA_SVN_SERVER_REVISION:
776
actuator->state_.server_firmware_svn_revision_ =
777
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
779
case MOTOR_SLOW_DATA_SVN_MODIFIED:
780
actuator->state_.firmware_modified_ =
781
static_cast<bool>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
783
case MOTOR_SLOW_DATA_SERIAL_NUMBER_LOW:
784
actuator->state_.set_serial_number_low (
785
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc)) );
787
case MOTOR_SLOW_DATA_SERIAL_NUMBER_HIGH:
788
actuator->state_.set_serial_number_high (
789
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc)) );
791
case MOTOR_SLOW_DATA_GEAR_RATIO:
792
actuator->state_.motor_gear_ratio =
793
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
795
case MOTOR_SLOW_DATA_ASSEMBLY_DATE_YYYY:
796
actuator->state_.assembly_data_year =
797
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
799
case MOTOR_SLOW_DATA_ASSEMBLY_DATE_MMDD:
800
actuator->state_.assembly_data_month =
801
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc)
803
actuator->state_.assembly_data_day =
804
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc)
807
case MOTOR_SLOW_DATA_CONTROLLER_F:
808
actuator->state_.force_control_f_ =
809
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
811
case MOTOR_SLOW_DATA_CONTROLLER_P:
812
actuator->state_.force_control_p_ =
813
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
815
case MOTOR_SLOW_DATA_CONTROLLER_I:
816
actuator->state_.force_control_i_ =
817
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
819
case MOTOR_SLOW_DATA_CONTROLLER_D:
820
actuator->state_.force_control_d_ =
821
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
823
case MOTOR_SLOW_DATA_CONTROLLER_IMAX:
824
actuator->state_.force_control_imax_ =
825
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
827
case MOTOR_SLOW_DATA_CONTROLLER_DEADSIGN:
828
tmp_value.word = status_data->motor_data_packet[index_motor_in_msg].misc;
829
actuator->state_.force_control_deadband_ = static_cast<int>(tmp_value.byte[0]);
830
actuator->state_.force_control_sign_ = static_cast<int>(tmp_value.byte[1]);
832
case MOTOR_SLOW_DATA_CONTROLLER_FREQUENCY:
833
actuator->state_.force_control_frequency_ =
834
static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
842
case MOTOR_DATA_CAN_ERROR_COUNTERS:
843
actuator->state_.can_error_counters =
844
static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
846
case MOTOR_DATA_PTERM:
847
actuator->state_.force_control_pterm =
848
static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
850
case MOTOR_DATA_ITERM:
851
actuator->state_.force_control_iterm =
852
static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
854
case MOTOR_DATA_DTERM:
855
actuator->state_.force_control_dterm =
856
static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
865
actuator->state_.force_unfiltered_ =
866
static_cast<double>(static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].torque));
868
#ifdef DEBUG_PUBLISHER
869
if( joint_tmp->motor->motor_id == 8 )
871
msg_debug.data = static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].torque);
872
debug_publishers[4].publish(msg_debug);
877
//Check the message to see if everything has already been received
878
if (motor_current_state == operation_mode::device_update_state::INITIALIZATION)
880
if (motor_data_checker->check_message(
881
joint_tmp, status_data->motor_data_type,
882
static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].torque)))
884
motor_updater_->update_state = operation_mode::device_update_state::OPERATION;
885
motor_current_state = operation_mode::device_update_state::OPERATION;
887
ROS_INFO("All motors were initialized.");
893
std::vector<std::pair<std::string, bool> > SrRobotLib::humanize_flags(int flag)
895
std::vector<std::pair<std::string, bool> > flags;
897
//16 is the number of flags
898
for (unsigned int i = 0; i < 16; ++i)
900
std::pair<std::string, bool> new_flag;
901
//if the flag is set add the name
902
if (sr_math_utils::is_bit_mask_index_true(flag, i))
904
if (sr_math_utils::is_bit_mask_index_true(SERIOUS_ERROR_FLAGS, i))
905
new_flag.second = true;
907
new_flag.second = false;
909
new_flag.first = error_flag_names[i];
910
flags.push_back(new_flag);
916
void SrRobotLib::generate_force_control_config(int motor_index, int max_pwm, int sg_left, int sg_right, int f, int p,
917
int i, int d, int imax, int deadband, int sign)
920
"Setting new pid values for motor" << motor_index << ": max_pwm="<< max_pwm <<" sg_left=" << sg_left << " sg_right=" << sg_right << " f=" << f << " p=" << p << " i=" << i << " d="<< d << " imax=" << imax << " deadband="<< deadband << " sign=" << sign);
922
//the vector is of the size of the TO_MOTOR_DATA_TYPE enum.
923
//the value of the element at a given index is the value
924
//for the given MOTOR_CONFIG.
925
std::vector<crc_unions::union16> full_config(MOTOR_CONFIG_CRC + 1);
926
crc_unions::union16 value;
928
value.word = max_pwm;
929
full_config.at(MOTOR_CONFIG_MAX_PWM) = value;
931
value.byte[0] = sg_left;
932
value.byte[1] = sg_right;
933
full_config.at(MOTOR_CONFIG_SG_REFS) = value;
936
full_config.at(MOTOR_CONFIG_F) = value;
939
full_config.at(MOTOR_CONFIG_P) = value;
942
full_config.at(MOTOR_CONFIG_I) = value;
945
full_config.at(MOTOR_CONFIG_D) = value;
948
full_config.at(MOTOR_CONFIG_IMAX) = value;
950
value.byte[0] = deadband;
951
value.byte[1] = sign;
952
full_config.at(MOTOR_CONFIG_DEADBAND_SIGN) = value;
954
"deadband: " << static_cast<int>(static_cast<int8u>(value.byte[0]) ) << " value: " << static_cast<int16u>(value.word));
958
for (unsigned int i = MOTOR_CONFIG_FIRST_VALUE; i <= MOTOR_CONFIG_LAST_VALUE; ++i)
960
crc_byte = full_config.at(i).byte[0];
961
INSERT_CRC_CALCULATION_HERE;
963
crc_byte = full_config.at(i).byte[1];
964
INSERT_CRC_CALCULATION_HERE;
967
//never send a CRC of 0, send 1 if the
968
// computed CRC is 0 (0 is a code for
969
// ignoring the config)
972
value.word = crc_result;
973
full_config.at(MOTOR_CONFIG_CRC) = value;
976
config.first = motor_index;
977
config.second = full_config;
978
//push the new config to the configuration queue
979
reconfig_queue.push(config);
982
void SrRobotLib::reinitialize_motors()
984
//Create a new MotorUpdater object
985
motor_updater_ = boost::shared_ptr<generic_updater::MotorUpdater>(new generic_updater::MotorUpdater(motor_update_rate_configs_vector, operation_mode::device_update_state::INITIALIZATION));
986
motor_current_state = operation_mode::device_update_state::INITIALIZATION;
987
//Initialize the motor data checker
988
motor_data_checker = boost::shared_ptr<generic_updater::MotorDataChecker>(new generic_updater::MotorDataChecker(joints_vector, motor_updater_->initialization_configs_vector));
991
void SrRobotLib::reinitialize_sensors()
993
//Create a new GenericTactiles object
994
tactiles_init = boost::shared_ptr<tactiles::GenericTactiles>( new tactiles::GenericTactiles(generic_sensor_update_rate_configs_vector, operation_mode::device_update_state::INITIALIZATION) );
995
tactile_current_state = operation_mode::device_update_state::INITIALIZATION;
998
bool SrRobotLib::nullify_demand_callback( sr_robot_msgs::NullifyDemand::Request& request,
999
sr_robot_msgs::NullifyDemand::Response& response )
1001
if( request.nullify_demand )
1002
ROS_INFO_STREAM("Nullifying the demand sent to the motor. Will ignore the values computed by the controllers and send 0.");
1004
ROS_INFO_STREAM("Using the value computed by the controllers to send the demands to the motors.");
1006
nullify_demand_ = request.nullify_demand;
1010
bool SrRobotLib::change_control_type_callback_( sr_robot_msgs::ChangeControlType::Request& request,
1011
sr_robot_msgs::ChangeControlType::Response& response )
1013
if( (request.control_type.control_type != sr_robot_msgs::ControlType::PWM) &&
1014
(request.control_type.control_type != sr_robot_msgs::ControlType::FORCE) )
1016
ROS_ERROR_STREAM(" The value you specified for the control type (" << request.control_type
1017
<< ") is incorrect. Using FORCE control.");
1019
control_type_.control_type = sr_robot_msgs::ControlType::FORCE;
1021
response.result = control_type_;
1025
control_type_ = request.control_type;
1027
response.result = control_type_;
1031
bool SrRobotLib::motor_system_controls_callback_( sr_robot_msgs::ChangeMotorSystemControls::Request& request,
1032
sr_robot_msgs::ChangeMotorSystemControls::Response& response )
1034
std::vector<sr_robot_msgs::MotorSystemControls> tmp_motor_controls;
1036
response.result = sr_robot_msgs::ChangeMotorSystemControls::Response::SUCCESS;
1037
bool no_motor_id_out_of_range = true;
1039
for( unsigned int i=0; i < request.motor_system_controls.size(); ++i)
1041
if( request.motor_system_controls[i].motor_id >= NUM_MOTORS ||
1042
request.motor_system_controls[i].motor_id < 0)
1044
response.result = sr_robot_msgs::ChangeMotorSystemControls::Response::MOTOR_ID_OUT_OF_RANGE;
1045
no_motor_id_out_of_range = false;
1049
//only pushes the demands with a correct motor_id
1050
tmp_motor_controls.push_back( request.motor_system_controls[i] );
1054
//add the request to the queue if it's not empty
1055
if( tmp_motor_controls.size() > 0 )
1056
motor_system_control_flags_.push( tmp_motor_controls );
1058
return no_motor_id_out_of_range;
1063
/* For the emacs weenies in the crowd.
added
removed
sr_robot_lib/src/sr_robot_lib.cpp
