sr_motor_hand_lib.hpp
Go to the documentation of this file.
1 /*
2 * @file sr_motor_hand_lib.hpp
3 * @author Ugo Cupcic <ugo@shadowrobot.com>
4 * @date Fri Jun 3 12:12:13 2011
5 *
6 *
7 /* Copyright 2011 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 library for the etherCAT hand.
22 * You can find it instantiated in the sr_edc_ethercat_drivers.
23 *
24 *
25 */
26 
27 #ifndef _SR_MOTOR_HAND_LIB_HPP_
28 #define _SR_MOTOR_HAND_LIB_HPP_
29 
31 #include <std_srvs/Empty.h>
32 
33 // to be able to load the configuration from the
34 // parameter server
35 #include <ros/ros.h>
36 #include <string>
37 #include <utility>
38 #include <map>
39 #include <vector>
40 
41 namespace shadow_robot
42 {
43 template<class StatusType, class CommandType>
45  public SrMotorRobotLib<StatusType, CommandType>
46 {
47 public:
48  SrMotorHandLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh, ros::NodeHandle nhtilde,
49  std::string device_id, std::string joint_prefix);
50 
51  /*
52  * The service callback for setting the Force PID values. There's only one callback
53  * function, but it can called for any motors. We know which motor called the service
54  * thanks to the motor_index.
55  *
56  * @param request The request contains the new parameters for the controllers.
57  * @param response True if succeeded.
58  * @param motor_index The index of the motor for which the service has been called.
59  *
60  * @return true if succeeded.
61  */
62  bool force_pid_callback(sr_robot_msgs::ForceController::Request &request,
63  sr_robot_msgs::ForceController::Response &response,
64  int motor_index);
65 
66  /*
67  * Reset the motor at motor index.
68  *
69  * @param request empty
70  * @param response empty
71  * @param joint A pair containing the index of the motor for the given
72  * joint followed by the name of the joint we're resetting
73  *
74  * @return true if success
75  */
76  bool reset_motor_callback(std_srvs::Empty::Request &request,
77  std_srvs::Empty::Response &response,
78  std::pair<int, std::string> joint);
79 
80 #ifdef DEBUG_PUBLISHER
81  /*
82  * This is a service callback: we set the debug data we want to publish
83  * at full speed in the debug topics.
84  *
85  * @param request Contains the motor index and the MOTOR_DATA type
86  * @param response True if succeeded.
87  *
88  * @return true if succeeded.
89  */
90  bool set_debug_data_to_publish(sr_robot_msgs::SetDebugData::Request& request,
91  sr_robot_msgs::SetDebugData::Response& response);
92 #endif
93 
94 protected:
95  /*
96  * Initializes the hand library with the needed values.
97  *
98  * @param joint_names A vector containing all the joint names.
99  * @param actuator_ids A vector containing the corresponding actuator ids.
100  * @param joint_to_sensors A vector mapping the joint to the sensor index we read from the palm.
101  */
102  virtual void initialize(std::vector<std::string> joint_names, std::vector<int> actuator_ids,
103  std::vector<shadow_joints::JointToSensor> joint_to_sensors);
104 
105  /*
106  * Updates the parameter values for the force control in the Parameter Server
107  *
108  * @param joint_name The name of the joint.
109  * @param max_pwm The max pwm the motor will apply
110  * @param sg_left Strain gauge left
111  * @param sg_right Strain gauge right
112  * @param f The feedforward term (directly adds f*error to the output of the PID)
113  * @param p The p value.
114  * @param i the i value.
115  * @param d the d value.
116  * @param imax the imax value.
117  * @param deadband the deadband on the force.
118  * @param sign can be 0 or 1 depending on the way the motor is plugged in.
119  */
121  std::string joint_name, int max_pwm, int sg_left, int sg_right, int f, int p, int i,
122  int d, int imax, int deadband, int sign, int torque_limit, int torque_limiter_gain);
123 
124  /*
125  * Finds the joint name for a certain motor index
126  *
127  * @param motor_index The integer motor index
128  */
129  std::string find_joint_name(int motor_index);
130 
131 private:
132  /*
133  * Reads the mapping associating a joint to a motor.
134  * If the motor index is -1, then no motor is associated
135  * to this joint.
136  *
137  *
138  * @return a vector of motor indexes, ordered by joint.
139  */
140  std::vector<int> read_joint_to_motor_mapping();
141 
142 
143  static const int nb_motor_data;
144  static const char *human_readable_motor_data_types[];
145  static const int32u motor_data_types[];
146 
147  /*
148  * Read the motor board force pids from the parameter servers,
149  * called when resetting the motor.
150  *
151  * @param joint_name the joint we want to reset
152  * @param motor_index the index of the motor for this joint
153  */
154  void resend_pids(std::string joint_name, int motor_index);
155 
156  /*
157  * A map used to keep the timers created in reset_motor_callback alive.
158  * We're using a map to keep only one timer per joint.
159  */
160  std::map<std::string, ros::Timer> pid_timers;
161 };
162 } // namespace shadow_robot
163 
164 /* For the emacs weenies in the crowd.
165 Local Variables:
166  c-basic-offset: 2
167 End:
168 */
169 
170 #endif
d
bool force_pid_callback(sr_robot_msgs::ForceController::Request &request, sr_robot_msgs::ForceController::Response &response, int motor_index)
std::string find_joint_name(int motor_index)
f
bool reset_motor_callback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response, std::pair< int, std::string > joint)
unsigned int int32u
std::map< std::string, ros::Timer > pid_timers
virtual void initialize(std::vector< std::string > joint_names, std::vector< int > actuator_ids, std::vector< shadow_joints::JointToSensor > joint_to_sensors)
std::vector< int > read_joint_to_motor_mapping()
void resend_pids(std::string joint_name, int motor_index)
static const int32u motor_data_types[]
SrMotorHandLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh, ros::NodeHandle nhtilde, std::string device_id, std::string joint_prefix)
void update_force_control_in_param_server(std::string joint_name, 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)
static const char * human_readable_motor_data_types[]


sr_robot_lib
Author(s): Ugo Cupcic, Toni Oliver
autogenerated on Tue Oct 13 2020 04:01:57