sr_muscle_robot_lib.hpp
Go to the documentation of this file.
1 /*
2 * @file sr_muscle_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 muscle-actuated Hardware.
22 *
23 *
24 */
25 
26 #ifndef _SR_MUSCLE_ROBOT_LIB_HPP_
27 #define _SR_MUSCLE_ROBOT_LIB_HPP_
28 
30 
32 #include <sr_utilities/calibration.hpp>
33 #include <string>
34 #include <map>
35 #include <queue>
36 #include <utility>
37 #include <list>
38 #include <vector>
39 
40 #define NUM_MUSCLE_DRIVERS 4
41 
42 namespace shadow_robot
43 {
44 template<class StatusType, class CommandType>
46  public SrRobotLib<StatusType, CommandType>
47 {
48 public:
49  SrMuscleRobotLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh, ros::NodeHandle nhtilde,
50  std::string device_id, std::string joint_prefix);
51 
52  /*
53  * This function is called each time a new etherCAT message
54  * is received in the sr06.cpp driver. It updates the joints_vector,
55  * updating the different values, computing the calibrated joint
56  * positions, etc... It also updates the tactile sensors values.
57  *
58  * @param status_data the received etherCAT message
59  */
60  void update(StatusType *status_data);
61 
62  /*
63  * Builds a motor command: either send a torque demand or a configuration
64  * demand if one is waiting.
65  *
66  * @param command The command we're building.
67  */
68  void build_command(CommandType *command);
69 
70  /*
71  * This function adds the diagnostics for the hand to the
72  * multi diagnostic status published in sr06.cpp.
73  */
74  void add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
76 
77  /*
78  * Initiates the process to retrieve the initialization information from the motors
79  */
80  void reinitialize_motors();
81 
82 
83  // Current update state of the motor (initialization, operation..)
85 
86 
87 protected:
88  /*
89  * Initializes the hand library with the needed values.
90  *
91  * @param joint_names A vector containing all the joint names.
92  * @param actuator_ids A vector containing the corresponding actuator ids.
93  * @param joint_to_sensors A vector mapping the joint to the sensor index we read from the palm.
94  */
95  virtual void initialize(std::vector<std::string> joint_names, std::vector<int> actuator_ids,
96  std::vector<shadow_joints::JointToSensor> joint_to_sensors) = 0;
97 
98  /*
99  * Compute the calibrated position for the given joint. This method is called
100  * from the update method, each time a new message is received.
101  *
102  * @param joint_tmp The joint we want to calibrate.
103  * @param status_data The status information that comes from the robot
104  */
105  void calibrate_joint(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data);
106 
111 
112 
113  /*
114  * Reads the calibration from the parameter server.
115  *
116  * @return a calibration map
117  */
119 
120 
121  /*
122  * Read additional data from the latest message and stores it into the
123  * joints_vector.
124  *
125  * @param joint_tmp The joint we want to read the data for.
126  * @param status_data The status information that comes from the robot
127  */
128  void read_additional_muscle_data(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data);
129 
130  /*
131 * Calibrates and filters the position information (and computes velocity) for a give joint.
132 * This method is called from the update method, each time a new message is received.
133 *
134 * @param joint_tmp The joint we process data from.
135 * @param status_data The status information that comes from the robot
136 * @param timestamp Timestamp of the data acquisition time
137 */
138  void process_position_sensor_data(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data,
139  double timestamp);
140 
141  /*
142  * Read additional data from the latest message and stores it into the
143  * joints_vector.
144  *
145  * @param muscle_driver_tmp The muscle we want to read the data for.
146  * @param status_data The status information that comes from the robot
147  */
148  void read_muscle_driver_data(std::vector<shadow_joints::MuscleDriver>::iterator muscle_driver_tmp,
149  StatusType *status_data);
150 
151  /*
152  * Transforms the incoming flag as a human
153  * readable vector of strings.
154  *
155  * @param flag incoming flag.
156  *
157  * @return human readable flags.
158  */
159  std::vector<std::pair<std::string, bool> > humanize_flags(int flag);
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::SrMuscleActuator *get_joint_actuator(std::vector<shadow_joints::Joint>::iterator joint_tmp)
169  {
170  return static_cast<sr_actuator::SrMuscleActuator *>(joint_tmp->actuator_wrapper->actuator);
171  }
172 
173  /*
174  * Decodes the pressure data for a certain muscle in a certain muscle driver from the status data structure
175  */
176  unsigned int get_muscle_pressure(int muscle_driver_id, int muscle_id, StatusType *status_data);
177 
178  inline void set_muscle_driver_data_received_flags(unsigned int msg_type, int muscle_driver_id);
179 
181 
182  /*
183  * Encodes the required valve value in a 4 bit 2's complement format, and writes it in
184  * the most significant or less significant half of the pointed byte (muscle_data_byte_to_set) depending on
185  * the value of shifting_index.
186  *
187  * @param muscle_data_byte_to_set pointer to the byte where we want to write the result
188  * @param valve_value the integer value of the valve demand
189  * @param shifting_index if 0, valve is written on the 4 MSB of muscle_data_byte_to_set, if 1 on the 4 LSB
190  */
191  inline void set_valve_demand(uint8_t *muscle_data_byte_to_set, int8_t valve_value, uint8_t shifting_index);
192 
193  /*
194  * Callback for the timer that controls the timeout for the muscle initialization period
195  */
196  void init_timer_callback(const ros::TimerEvent &event);
197 
198  std::vector<shadow_joints::MuscleDriver> muscle_drivers_vector_;
199 
200 
201  /*
202  * The motor updater is used to create a correct command to send to the motor.
203  * It's build_command() is called each time the SR06::packCommand()
204  * is called.
205  */
207 
208 
209  // contains a queue of muscle driver indexes to reset
210  std::queue<int16_t, std::list<int16_t> > reset_muscle_driver_queue;
211 
212 
213  // The update rate for each muscle information
214  std::vector<generic_updater::UpdateConfig> muscle_update_rate_configs_vector;
215 
216  /*
217  *
218  * A vector to store information about a certain message from the muscle driver. First in the pair is FROM_MUSCLE_DATA_TYPE
219  * second in the pair is a bit mask where the bit in position id_muscle_driver tells if this data has been received from that muscle driver
220  * It will only contain those FROM_MUSCLE_DATA_TYPE that are considered as initialization parameters
221  */
222  std::map<unsigned int, unsigned int> from_muscle_driver_data_received_flags_;
223 
224 
226  static const double timeout;
228 
229  // A mutual exclusion object to ensure that the initialization timeout event does work without threading issues
231 }; // end class
232 } // namespace shadow_robot
233 
234 /* For the emacs weenies in the crowd.
235 Local Variables:
236  c-basic-offset: 2
237 End:
238 */
239 
240 #endif
d
void set_muscle_driver_data_received_flags(unsigned int msg_type, int muscle_driver_id)
std::queue< int16_t, std::list< int16_t > > reset_muscle_driver_queue
void init_timer_callback(const ros::TimerEvent &event)
std::vector< shadow_joints::MuscleDriver > muscle_drivers_vector_
virtual void initialize(std::vector< std::string > joint_names, std::vector< int > actuator_ids, std::vector< shadow_joints::JointToSensor > joint_to_sensors)=0
boost::shared_ptr< shadow_robot::JointCalibration > pressure_calibration_tmp_
A temporary calibration for a given joint.
void read_additional_muscle_data(std::vector< shadow_joints::Joint >::iterator joint_tmp, StatusType *status_data)
void build_command(CommandType *command)
virtual shadow_joints::CalibrationMap read_pressure_calibration()
void set_valve_demand(uint8_t *muscle_data_byte_to_set, int8_t valve_value, uint8_t shifting_index)
SrMuscleRobotLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh, ros::NodeHandle nhtilde, std::string device_id, std::string joint_prefix)
std::map< unsigned int, unsigned int > from_muscle_driver_data_received_flags_
ROSLIB_DECL std::string command(const std::string &cmd)
operation_mode::device_update_state::DeviceUpdateState muscle_current_state
void process_position_sensor_data(std::vector< shadow_joints::Joint >::iterator joint_tmp, StatusType *status_data, double timestamp)
void read_muscle_driver_data(std::vector< shadow_joints::MuscleDriver >::iterator muscle_driver_tmp, StatusType *status_data)
boost::shared_ptr< generic_updater::MuscleUpdater< CommandType > > muscle_updater_
def msg_type(f)
std::vector< std::pair< std::string, bool > > humanize_flags(int flag)
unsigned int get_muscle_pressure(int muscle_driver_id, int muscle_id, StatusType *status_data)
shadow_joints::CalibrationMap pressure_calibration_map_
The map used to calibrate each pressure sensor.
void calibrate_joint(std::vector< shadow_joints::Joint >::iterator joint_tmp, StatusType *status_data)
threadsafe::Map< boost::shared_ptr< shadow_robot::JointCalibration > > CalibrationMap
sr_actuator::SrMuscleActuator * get_joint_actuator(std::vector< shadow_joints::Joint >::iterator joint_tmp)
void update(StatusType *status_data)
std::vector< generic_updater::UpdateConfig > muscle_update_rate_configs_vector
boost::shared_ptr< boost::mutex > lock_init_timeout_
void add_diagnostics(std::vector< diagnostic_msgs::DiagnosticStatus > &vec, diagnostic_updater::DiagnosticStatusWrapper &d)


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