sr_motor_robot_lib.hpp
Go to the documentation of this file.
1 /*
2 * @file sr_motor_robot_lib.hpp
3 * @author Ugo Cupcic <ugo@shadowrobot.com>, Toni Oliver <toni@shadowrobot.com>, contact <software@shadowrobot.com>
4 * @date Tue Mar 19 17:12:13 2013
5 *
6 *
7 /* Copyright 2013 Shadow Robot Company Ltd.
8 *
9 * This program is free software: you can redistribute it and/or modify it
10 * under the terms of the GNU General Public License as published by the Free
11 * Software Foundation version 2 of the License.
12 *
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
16 * more details.
17 *
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/>.
20 *
21 * @brief This is a generic robot library for Shadow Robot's motor-actuated Hardware.
22 *
23 *
24 */
25 
26 #ifndef _SR_MOTOR_ROBOT_LIB_HPP_
27 #define _SR_MOTOR_ROBOT_LIB_HPP_
28 
30 
31 #include <sr_robot_msgs/ForceController.h>
32 #include <sr_robot_msgs/ControlType.h>
33 #include <sr_robot_msgs/ChangeControlType.h>
34 #include <sr_robot_msgs/MotorSystemControls.h>
35 #include <sr_robot_msgs/ChangeMotorSystemControls.h>
36 
39 
40 #include <string>
41 #include <queue>
42 #include <utility>
43 #include <list>
44 #include <vector>
45 
46 namespace shadow_robot
47 {
48 template<class StatusType, class CommandType>
50  public SrRobotLib<StatusType, CommandType>
51 {
52 public:
53  SrMotorRobotLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh, ros::NodeHandle nhtilde,
54  std::string device_id, std::string joint_prefix);
55 
56  /*
57  * This function is called each time a new etherCAT message
58  * is received in the sr06.cpp driver. It updates the joints_vector,
59  * updating the different values, computing the calibrated joint
60  * positions, etc... It also updates the tactile sensors values.
61  *
62  * @param status_data the received etherCAT message
63  */
64  void update(StatusType *status_data);
65 
66  /*
67  * Builds a motor command: either send a torque demand or a configuration
68  * demand if one is waiting.
69  *
70  * @param command The command we're building.
71  */
72  void build_command(CommandType *command);
73 
74  /*
75  * This function adds the diagnostics for the hand to the
76  * multi diagnostic status published in sr06.cpp.
77  */
78  void add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
80 
81  /*
82  * Initiates the process to retrieve the initialization information from the motors
83  */
84  void reinitialize_motors();
85 
86 
87  // Current update state of the motor (initialization, operation..)
89 
90 
91 protected:
92  /*
93  * Initializes the hand library with the needed values.
94  *
95  * @param joint_names A vector containing all the joint names.
96  * @param actuator_ids A vector containing the corresponding actuator ids.
97  * @param joint_to_sensors A vector mapping the joint to the sensor index we read from the palm.
98  */
99  virtual void initialize(std::vector<std::string> joint_names, std::vector<int> actuator_ids,
100  std::vector<shadow_joints::JointToSensor> joint_to_sensors) = 0;
101 
102  /*
103  * Compute the calibrated position for the given joint. This method is called
104  * from the update method, each time a new message is received.
105  *
106  * @param joint_tmp The joint we want to calibrate.
107  * @param status_data The status information that comes from the robot
108  */
109  void calibrate_joint(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data);
110 
111  /*
112  * Read additional data from the latest message and stores it into the
113  * joints_vector.
114  *
115  * @param joint_tmp The joint we want to read the data for.
116  * @param status_data The status information that comes from the robot
117  */
118  void read_additional_data(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data);
119 
120  /*
121  * Calibrates and filters the position information (and computes velocity) for a give joint.
122  * This method is called from the update method, each time a new message is received.
123  *
124  * @param joint_tmp The joint we process data from.
125  * @param status_data The status information that comes from the robot
126  * @param timestamp Timestamp of the data acquisition time
127  */
128  void process_position_sensor_data(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data,
129  double timestamp);
130 
131  /*
132  * Transforms the incoming flag as a human
133  * readable vector of strings.
134  *
135  * @param flag incoming flag.
136  *
137  * @return human readable flags.
138  */
139  std::vector<std::pair<std::string, bool> > humanize_flags(int flag);
140 
141  /*
142  * Generates a force control config and adds it to the reconfig_queue with its
143  * CRC. The config will be sent as soon as possible.
144  *
145  * @param motor_index The motor index.
146  * @param max_pwm The max pwm the motor will apply
147  * @param sg_left Strain gauge left
148  * @param sg_right Strain gauge right
149  * @param f The feedforward term (directly adds f*error to the output of the PID)
150  * @param p The p value.
151  * @param i the i value.
152  * @param d the d value.
153  * @param imax the imax value.
154  * @param deadband the deadband on the force.
155  * @param sign can be 0 or 1 depending on the way the motor is plugged in.
156  */
158  int motor_index, int max_pwm, int sg_left, int sg_right, int f, int p, int i, int d,
159  int imax, int deadband, int sign, int torque_limit, int torque_limiter_gain);
160 
161  /*
162  * Returns a pointer to the actuator for a certain joint.
163  *
164  * @param joint_tmp The joint we want to get the actuator from.
165  *
166  * @return a pointer to the actuator
167  */
168  sr_actuator::SrMotorActuator *get_joint_actuator(std::vector<shadow_joints::Joint>::iterator joint_tmp)
169  {
170  return static_cast<sr_actuator::SrMotorActuator *>(joint_tmp->actuator_wrapper->actuator);
171  }
172 
173  /*
174  * The motor updater is used to create a correct command to send to the motor.
175  * It's build_command() is called each time the SR06::packCommand()
176  * is called.
177  */
179 
180 
181  /*
182  * The ForceConfig type consists of an int representing the motor index for this config
183  * followed by a vector of config: the index in the vector of config corresponds to the
184  * type of the data, and the value at this index corresponds to the value we want to set.
185  */
186  typedef std::pair<int, std::vector<crc_unions::union16> > ForceConfig;
187  /*
188  * This queue contains the force PID config waiting to be pushed to the motor.
189  */
190  std::queue<ForceConfig, std::list<ForceConfig> > reconfig_queue;
191  // this index is used to iterate over the config we're sending.
193 
194  // contains a queue of motor indexes to reset
195  std::queue<int16_t, std::list<int16_t> > reset_motors_queue;
196 
197 
198  // The index of the motor in all the 20 motors
200  // The index of the motor in the current message (from 0 to 9)
202 
206 
207 
208  // The update rate for each motor information
209  std::vector<generic_updater::UpdateConfig> motor_update_rate_configs_vector;
210 
212 
213 
214  // The current type of control (FORCE demand or PWM demand sent to the motors)
215  sr_robot_msgs::ControlType control_type_;
216  /*
217  * Flag to signal that there has been a change in the value of control_type_ and certain actions are required.
218  * The flag is set in the callback function of the change_control_type_ service.
219  * The flag is checked in build_command() and the necessary actions are taken there.
220  * These actions involve calling services in the controller manager and all the active controllers. This is the
221  * reason why we don't do it directly in the callback function. As we use a single thread to serve the callbacks,
222  * doing so would cause a deadlock, thus we do it in the realtime loop thread instead.
223  */
225  // A service server used to change the control type on the fly.
227  // A mutual exclusion object to ensure that no command will be sent to the robot while a change
228  // in the control type (PWM or torque) is ongoing
230 
231  /*
232  * The callback to the change_control_type_ service. Updates
233  * the current control_type_ with the requested control_type.
234  *
235  * @param request Requested control_type_
236  * @param response The new control_type we'll use
237  *
238  * @return true if success, false if bad control type requested
239  */
240  bool change_control_type_callback_(sr_robot_msgs::ChangeControlType::Request &request,
241  sr_robot_msgs::ChangeControlType::Response &response);
242 
243  /*
244  * Load the necessary parameters in the Parameter Server and
245  * calls a service for every controller currently loaded in the controller manager to make it
246  * reload (resetGains()) its parameters from the Parameter Server
247  *
248  * @param control_type The new active control type (PWM or torque)
249  *
250  * @return true if all the steps successful
251  */
252  bool change_control_parameters(int16_t control_type);
253 
254  // The Flag which will be sent to change the motor controls
255  std::queue<std::vector<sr_robot_msgs::MotorSystemControls>,
256  std::list<std::vector<sr_robot_msgs::MotorSystemControls> > > motor_system_control_flags_;
257  // A service server used to call the different motor system controls "buttons"
259 
260  /*
261  * The callback to the control_motor_ service. Sets the correct flags to 1 or 0
262  * for the MOTOR_SYSTEM_CONTROLS, to control the motors (backlash compensation
263  * on/off, increase sg tracking, jiggling, write config to EEprom)
264  *
265  * @param request Contains the different flags the user wants to set
266  * @param response SUCCESS if success, MOTOR_ID_OUT_OF_RANGE if bad motor_id given
267  *
268  * @return false if motor_id is out of range
269  */
270  bool motor_system_controls_callback_(sr_robot_msgs::ChangeMotorSystemControls::Request &request,
271  sr_robot_msgs::ChangeMotorSystemControls::Response &response);
272 
273 
274  int find_joint_by_name(string joint_name);
275  bool check_if_joint_coupled(string joint_name);
276 }; // end class
277 } // namespace shadow_robot
278 
279 /* For the emacs weenies in the crowd.
280 Local Variables:
281  c-basic-offset: 2
282 End:
283 */
284 
285 #endif
d
unsigned short int16u
void generate_force_control_config(int motor_index, int max_pwm, int sg_left, int sg_right, int f, int p, int i, int d, int imax, int deadband, int sign, int torque_limit, int torque_limiter_gain)
std::pair< int, std::vector< crc_unions::union16 > > ForceConfig
virtual void initialize(std::vector< std::string > joint_names, std::vector< int > actuator_ids, std::vector< shadow_joints::JointToSensor > joint_to_sensors)=0
SrMotorRobotLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh, ros::NodeHandle nhtilde, std::string device_id, std::string joint_prefix)
void read_additional_data(std::vector< shadow_joints::Joint >::iterator joint_tmp, StatusType *status_data)
sr_robot_msgs::ControlType control_type_
std::vector< generic_updater::UpdateConfig > motor_update_rate_configs_vector
unsigned char int8u
ROSLIB_DECL std::string command(const std::string &cmd)
boost::shared_ptr< generic_updater::MotorUpdater< CommandType > > motor_updater_
std::queue< int16_t, std::list< int16_t > > reset_motors_queue
boost::shared_ptr< generic_updater::MotorDataChecker > motor_data_checker
sr_actuator::SrMotorActuator * get_joint_actuator(std::vector< shadow_joints::Joint >::iterator joint_tmp)
bool motor_system_controls_callback_(sr_robot_msgs::ChangeMotorSystemControls::Request &request, sr_robot_msgs::ChangeMotorSystemControls::Response &response)
bool change_control_parameters(int16_t control_type)
bool check_if_joint_coupled(string joint_name)
boost::shared_ptr< boost::mutex > lock_command_sending_
ros::ServiceServer motor_system_control_server_
bool change_control_type_callback_(sr_robot_msgs::ChangeControlType::Request &request, sr_robot_msgs::ChangeControlType::Response &response)
void build_command(CommandType *command)
void add_diagnostics(std::vector< diagnostic_msgs::DiagnosticStatus > &vec, diagnostic_updater::DiagnosticStatusWrapper &d)
void calibrate_joint(std::vector< shadow_joints::Joint >::iterator joint_tmp, StatusType *status_data)
operation_mode::device_update_state::DeviceUpdateState motor_current_state
void update(StatusType *status_data)
std::queue< std::vector< sr_robot_msgs::MotorSystemControls >, std::list< std::vector< sr_robot_msgs::MotorSystemControls > > > motor_system_control_flags_
int find_joint_by_name(string joint_name)
void process_position_sensor_data(std::vector< shadow_joints::Joint >::iterator joint_tmp, StatusType *status_data, double timestamp)
std::vector< std::pair< std::string, bool > > humanize_flags(int flag)
std::queue< ForceConfig, std::list< ForceConfig > > reconfig_queue


sr_robot_lib
Author(s): Ugo Cupcic, Toni Oliver
autogenerated on Mon Feb 28 2022 23:50:43