Go to the documentation of this file.00001
00029 #include "sr_mechanism_controllers/srh_joint_muscle_valve_controller.hpp"
00030 #include "angles/angles.h"
00031 #include "pluginlib/class_list_macros.h"
00032 #include <sstream>
00033 #include <math.h>
00034 #include "sr_utilities/sr_math_utils.hpp"
00035
00036 #include <std_msgs/Float64.h>
00037
00038 PLUGINLIB_EXPORT_CLASS(controller::SrhJointMuscleValveController, controller_interface::ControllerBase)
00039
00040 using namespace std;
00041
00042 namespace controller
00043 {
00044
00045 SrhJointMuscleValveController::SrhJointMuscleValveController()
00046 : cmd_valve_muscle_min_(-4),
00047 cmd_valve_muscle_max_(4)
00048 {
00049 }
00050
00051 bool SrhJointMuscleValveController::init(ros_ethercat_model::RobotState *robot, ros::NodeHandle &n)
00052 {
00053 ROS_ASSERT(robot);
00054 robot_ = robot;
00055 node_ = n;
00056
00057 if (!node_.getParam("joint", joint_name_))
00058 {
00059 ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00060 return false;
00061 }
00062
00063 controller_state_publisher_.reset(new realtime_tools::RealtimePublisher<sr_robot_msgs::JointMuscleValveControllerState>
00064 (node_, "state", 1));
00065
00066 ROS_DEBUG(" --------- ");
00067 ROS_DEBUG_STREAM("Init: " << joint_name_);
00068
00069
00070 has_j2 = is_joint_0();
00071 if (has_j2)
00072 {
00073 get_joints_states_1_2();
00074 if (!joint_state_)
00075 {
00076 ROS_ERROR("SrhJointMuscleValveController could not find the first joint relevant to \"%s\"\n", joint_name_.c_str());
00077 return false;
00078 }
00079 if (!joint_state_2)
00080 {
00081 ROS_ERROR("SrhJointMuscleValveController could not find the second joint relevant to \"%s\"\n", joint_name_.c_str());
00082 return false;
00083 }
00084 }
00085 else
00086 {
00087 joint_state_ = robot_->getJointState(joint_name_);
00088 if (!joint_state_)
00089 {
00090 ROS_ERROR("SrhJointMuscleValveController could not find joint named \"%s\"\n", joint_name_.c_str());
00091 return false;
00092 }
00093 }
00094
00095 friction_compensator.reset(new sr_friction_compensation::SrFrictionCompensator(joint_name_));
00096
00097
00098 serve_reset_gains_ = node_.advertiseService("reset_gains", &SrhJointMuscleValveController::resetGains, this);
00099
00100 ROS_DEBUG_STREAM(" joint_state name: " << joint_state_->joint_->name);
00101 ROS_DEBUG_STREAM(" In Init: " << getJointName() << " This: " << this
00102 << " joint_state: " << joint_state_);
00103
00104
00105 sub_command_ = node_.subscribe<sr_robot_msgs::JointMuscleValveControllerCommand>("command", 1, &SrhJointMuscleValveController::setCommandCB, this);
00106 return true;
00107 }
00108
00109 void SrhJointMuscleValveController::starting(const ros::Time& time)
00110 {
00111 command_ = 0.0;
00112 read_parameters();
00113 }
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129 bool SrhJointMuscleValveController::resetGains(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp)
00130 {
00131 command_ = 0.0;
00132
00133 read_parameters();
00134
00135 if (has_j2)
00136 ROS_WARN_STREAM("Reseting controller gains: " << joint_state_->joint_->name << " and " << joint_state_2->joint_->name);
00137 else
00138 ROS_WARN_STREAM("Reseting controller gains: " << joint_state_->joint_->name);
00139
00140 return true;
00141 }
00142
00143 void SrhJointMuscleValveController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
00144 {
00145 }
00146
00147 void SrhJointMuscleValveController::update(const ros::Time& time, const ros::Duration& period)
00148 {
00149
00150 int8_t valve[2];
00151 unsigned int i;
00152
00153 ROS_ASSERT(robot_ != NULL);
00154 ROS_ASSERT(joint_state_->joint_);
00155
00156 if (!initialized_)
00157 {
00158 initialized_ = true;
00159 cmd_valve_muscle_[0] = 0.0;
00160 cmd_valve_muscle_[1] = 0.0;
00161 cmd_duration_ms_[0] = 0;
00162 cmd_duration_ms_[1] = 0;
00163 current_duration_ms_[0] = cmd_duration_ms_[0];
00164 current_duration_ms_[1] = cmd_duration_ms_[1];
00165 }
00166
00167
00168
00169
00170
00171
00172
00173
00174 double pressure_0_tmp = fmod(joint_state_->effort_, 0x10000);
00175 double pressure_1_tmp = (fmod(joint_state_->effort_, 0x100000000) - pressure_0_tmp) / 0x10000;
00176 uint16_t pressure_0 = static_cast<uint16_t> (pressure_0_tmp + 0.5);
00177 uint16_t pressure_1 = static_cast<uint16_t> (pressure_1_tmp + 0.5);
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196 for (i = 0; i < 2; ++i)
00197 {
00198 if (cmd_duration_ms_[i] == 0)
00199 {
00200
00201 valve[i] = cmd_valve_muscle_[i];
00202 }
00203 else
00204 {
00205 if (current_duration_ms_[i] > 0)
00206 {
00207
00208 valve[i] = cmd_valve_muscle_[i];
00209
00210 current_duration_ms_[i]--;
00211 }
00212 else
00213 {
00214
00215 valve[i] = 0;
00216 }
00217 }
00218 }
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236 uint16_t valve_tmp[2];
00237 for (i = 0; i < 2; ++i)
00238 {
00239
00240 if (valve[i] > 4)
00241 valve[i] = 4;
00242 if (valve[i] < -4)
00243 valve[i] = -4;
00244
00245 if (valve[i] < 0)
00246 valve_tmp[i] = -valve[i] + 8;
00247 else
00248 valve_tmp[i] = valve[i];
00249 }
00250
00251
00252
00253 joint_state_->commanded_effort_ = static_cast<double> (valve_tmp[0]) + static_cast<double> (valve_tmp[1] << 4);
00254
00255
00256
00257
00258
00259
00260 if (loop_count_ % 10 == 0)
00261 {
00262 if (controller_state_publisher_ && controller_state_publisher_->trylock())
00263 {
00264 controller_state_publisher_->msg_.header.stamp = time;
00265
00266 controller_state_publisher_->msg_.set_valve_muscle_0 = cmd_valve_muscle_[0];
00267 controller_state_publisher_->msg_.set_valve_muscle_1 = cmd_valve_muscle_[1];
00268 controller_state_publisher_->msg_.set_duration_muscle_0 = cmd_duration_ms_[0];
00269 controller_state_publisher_->msg_.set_duration_muscle_1 = cmd_duration_ms_[1];
00270 controller_state_publisher_->msg_.current_valve_muscle_0 = valve[0];
00271 controller_state_publisher_->msg_.current_valve_muscle_1 = valve[1];
00272 controller_state_publisher_->msg_.current_duration_muscle_0 = current_duration_ms_[0];
00273 controller_state_publisher_->msg_.current_duration_muscle_1 = current_duration_ms_[1];
00274 controller_state_publisher_->msg_.packed_valve = joint_state_->commanded_effort_;
00275 controller_state_publisher_->msg_.muscle_pressure_0 = pressure_0;
00276 controller_state_publisher_->msg_.muscle_pressure_1 = pressure_1;
00277 controller_state_publisher_->msg_.time_step = period.toSec();
00278
00279 controller_state_publisher_->unlockAndPublish();
00280 }
00281 }
00282 loop_count_++;
00283 }
00284
00285 void SrhJointMuscleValveController::read_parameters()
00286 {
00287
00288 }
00289
00290 void SrhJointMuscleValveController::setCommandCB(const sr_robot_msgs::JointMuscleValveControllerCommandConstPtr& msg)
00291 {
00292 cmd_valve_muscle_[0] = clamp_command(msg->cmd_valve_muscle[0]);
00293 cmd_valve_muscle_[1] = clamp_command(msg->cmd_valve_muscle[1]);
00294
00295 cmd_duration_ms_[0] = static_cast<unsigned int> (msg->cmd_duration_ms[0]);
00296 cmd_duration_ms_[1] = static_cast<unsigned int> (msg->cmd_duration_ms[1]);
00297
00298 current_duration_ms_[0] = cmd_duration_ms_[0];
00299 current_duration_ms_[1] = cmd_duration_ms_[1];
00300 }
00301
00303
00304 int8_t SrhJointMuscleValveController::clamp_command(int8_t cmd)
00305 {
00306 if (cmd < cmd_valve_muscle_min_)
00307 return cmd_valve_muscle_min_;
00308
00309 if (cmd > cmd_valve_muscle_max_)
00310 return cmd_valve_muscle_max_;
00311
00312 return cmd;
00313 }
00314 }
00315
00316
00317
00318
00319
00320