Go to the documentation of this file.00001
00028 #include "sr_mechanism_model/joint_0_transmission_for_muscle.hpp"
00029
00030 #include <math.h>
00031 #include <pluginlib/class_list_macros.h>
00032 #include "pr2_mechanism_model/robot.h"
00033 #include "pr2_mechanism_model/simple_transmission.h"
00034
00035 #include <sr_hardware_interface/sr_actuator.hpp>
00036
00037 using namespace pr2_hardware_interface;
00038
00039 PLUGINLIB_DECLARE_CLASS(sr_mechanism_model, J0TransmissionForMuscle,
00040 sr_mechanism_model::J0TransmissionForMuscle,
00041 pr2_mechanism_model::Transmission)
00042
00043
00044 namespace sr_mechanism_model
00045 {
00046 bool J0TransmissionForMuscle::initXml(TiXmlElement *elt, pr2_mechanism_model::Robot *robot)
00047 {
00048 const char *name = elt->Attribute("name");
00049 name_ = name ? name : "";
00050
00051 TiXmlElement *jel = elt->FirstChildElement("joint1");
00052 init_joint(jel, robot);
00053 TiXmlElement *jel2 = elt->FirstChildElement("joint2");
00054 init_joint(jel2, robot);
00055
00056 TiXmlElement *ael = elt->FirstChildElement("actuator");
00057 const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00058 Actuator *a;
00059 if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
00060 {
00061 ROS_ERROR("J0TransmissionForMuscle could not find actuator named \"%s\"", actuator_name);
00062 return false;
00063 }
00064 a->command_.enable_ = true;
00065 actuator_names_.push_back(actuator_name);
00066
00067 mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
00068
00069 return true;
00070 }
00071
00072 bool J0TransmissionForMuscle::initXml(TiXmlElement *elt)
00073 {
00074 const char *name = elt->Attribute("name");
00075 name_ = name ? name : "";
00076
00077 TiXmlElement *jel = elt->FirstChildElement("joint1");
00078 init_joint(jel, NULL);
00079 TiXmlElement *jel2 = elt->FirstChildElement("joint2");
00080 init_joint(jel2, NULL);
00081
00082
00083 TiXmlElement *ael = elt->FirstChildElement("actuator");
00084 const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00085 if (!actuator_name)
00086 {
00087 ROS_ERROR("J0TransmissionForMuscle could not find actuator named \"%s\"", actuator_name);
00088 return false;
00089 }
00090 actuator_names_.push_back(actuator_name);
00091
00092 mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
00093
00094 return true;
00095 }
00096
00097 bool J0TransmissionForMuscle::init_joint(TiXmlElement *jel, pr2_mechanism_model::Robot *robot)
00098 {
00099 const char *joint_name = jel ? jel->Attribute("name") : NULL;
00100 if (!joint_name)
00101 {
00102 ROS_ERROR("J0TransmissionForMuscle did not specify joint name");
00103 return false;
00104 }
00105
00106 if( robot != NULL )
00107 {
00108 const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00109 if (!joint)
00110 {
00111 ROS_ERROR("J0TransmissionForMuscle could not find joint named \"%s\"", joint_name);
00112 return false;
00113 }
00114 }
00115 joint_names_.push_back(joint_name);
00116
00117 return true;
00118 }
00119
00120 void J0TransmissionForMuscle::propagatePosition(
00121 std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<pr2_mechanism_model::JointState*>& js)
00122 {
00123 ROS_DEBUG(" propagate position for j0");
00124
00125 assert(as.size() == 1);
00126 assert(js.size() == 2);
00127
00128
00129
00130 int size = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.calibrated_sensor_values_.size();
00131 if( size != 0)
00132 {
00133 if( size == 2 )
00134 {
00135 ROS_DEBUG_STREAM( "READING pos " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_
00136 << " J1 " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.calibrated_sensor_values_[0]
00137 << " J2 " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.calibrated_sensor_values_[1] );
00138
00139 js[0]->position_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.calibrated_sensor_values_[0];
00140 js[1]->position_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.calibrated_sensor_values_[1];
00141
00142 js[0]->velocity_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.velocity_ / 2.0;
00143 js[1]->velocity_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.velocity_ / 2.0;
00144
00145
00146
00147
00148
00149
00150 js[0]->measured_effort_ = (static_cast<double>(static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.pressure_[1]) * 0x10000) +
00151 static_cast<double>(static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.pressure_[0]);
00152 js[1]->measured_effort_ = (static_cast<double>(static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.pressure_[1]) * 0x10000) +
00153 static_cast<double>(static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.pressure_[0]);
00154 }
00155 }
00156 else
00157 {
00158 ROS_DEBUG_STREAM( "READING pos from Gazebo " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_
00159 << " J1 " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_ / 2.0
00160 << " J2 " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_ / 2.0 );
00161
00162
00163
00164 js[0]->position_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_ / 2.0;
00165 js[1]->position_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_ / 2.0;
00166
00167 js[0]->velocity_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.velocity_ / 2.0;
00168 js[1]->velocity_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.velocity_ / 2.0;
00169
00170 js[0]->measured_effort_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.last_measured_effort_ / 2.0;
00171 js[1]->measured_effort_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.last_measured_effort_ / 2.0;
00172 }
00173
00174 ROS_DEBUG("end propagate position for j0");
00175 }
00176
00177 void J0TransmissionForMuscle::propagatePositionBackwards(
00178 std::vector<pr2_mechanism_model::JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
00179 {
00180 ROS_DEBUG("propagate pos backward for j0");
00181
00182 assert(as.size() == 1);
00183 assert(js.size() == 2);
00184
00185 ROS_DEBUG_STREAM(" pos = " << js[0]->position_ << " + " << js[1]->position_ << " = " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_);
00186 static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_ = js[0]->position_ + js[1]->position_;
00187 static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.velocity_ = js[0]->velocity_ + js[1]->velocity_;
00188 static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.last_measured_effort_ = js[0]->measured_effort_ + js[1]->measured_effort_;
00189
00190
00191 if (! simulated_actuator_timestamp_initialized_)
00192 {
00193
00194 static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.sample_timestamp_ = ros::Duration(0);
00195
00196
00197 if (ros::isStarted())
00198 {
00199 simulated_actuator_start_time_ = ros::Time::now();
00200 simulated_actuator_timestamp_initialized_ = true;
00201 }
00202 }
00203 else
00204 {
00205
00206 static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
00207 }
00208
00209 static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.timestamp_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.sample_timestamp_.toSec();
00210
00211
00212 this->joint_calibration_simulator_.simulateJointCalibration(js[0],static_cast<sr_actuator::SrMuscleActuator*>(as[0]));
00213
00214 ROS_DEBUG(" end propagate pos backward for j0");
00215 }
00216
00217 void J0TransmissionForMuscle::propagateEffort(
00218 std::vector<pr2_mechanism_model::JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
00219 {
00220 ROS_DEBUG(" propagate effort for j0");
00221
00222 assert(as.size() == 1);
00223 assert(js.size() == 2);
00224 static_cast<sr_actuator::SrMuscleActuator*>(as[0])->command_.enable_ = true;
00225
00226
00227
00228
00229
00230 double valve_0 = fmod(js[0]->commanded_effort_, 0x10);
00231 int8_t valve_0_tmp = static_cast<int8_t>(valve_0 + 0.5);
00232 if (valve_0_tmp >= 8)
00233 {
00234 valve_0_tmp -= 8;
00235 valve_0_tmp *= (-1);
00236 }
00237
00238 int8_t valve_1_tmp = static_cast<int8_t>(((fmod(js[0]->commanded_effort_, 0x100) - valve_0) / 0x10) + 0.5);
00239 if (valve_1_tmp >= 8)
00240 {
00241 valve_1_tmp -= 8;
00242 valve_1_tmp *= (-1);
00243 }
00244
00245 static_cast<sr_actuator::SrMuscleActuator*>(as[0])->command_.valve_[0] = valve_0_tmp;
00246 static_cast<sr_actuator::SrMuscleActuator*>(as[0])->command_.valve_[1] = valve_1_tmp;
00247
00248 ROS_DEBUG("end propagate effort for j0");
00249 }
00250
00251 void J0TransmissionForMuscle::propagateEffortBackwards(
00252 std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<pr2_mechanism_model::JointState*>& js)
00253 {
00254 ROS_DEBUG("propagate effort backward for j0");
00255
00256 assert(as.size() == 1);
00257 assert(js.size() == 2);
00258 js[0]->commanded_effort_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->command_.effort_;
00259 js[1]->commanded_effort_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->command_.effort_;
00260
00261 ROS_DEBUG("end propagate effort backward for j0");
00262 }
00263
00264 }
00265
00266
00267
00268
00269
00270