83 #include <boost/lexical_cast.hpp> 85 #define PASSIVE_JOINTS 1 95 if (this->initXml(config))
98 for (std::vector<std::string>::iterator actuator_name = actuator_names_.begin(); actuator_name != actuator_names_.end(); ++actuator_name)
100 if (robot->getActuator(*actuator_name))
102 robot->getActuator(*actuator_name)->command_.enable_ =
true;
106 ROS_ERROR(
"PR2GripperTransmission actuator named \"%s\" not loaded in Robot", actuator_name->c_str());
112 for (std::vector<std::string>::iterator joint_name = joint_names_.begin(); joint_name != joint_names_.end(); ++joint_name)
114 if (!robot->robot_model_.getJoint(*joint_name))
116 ROS_ERROR(
"PR2GripperTransmission joint named \"%s\" not loaded in Robot", joint_name->c_str());
128 bool PR2GripperTransmission::initXml(TiXmlElement *config)
130 const char *name = config->Attribute(
"name");
131 name_ = name ? name :
"";
133 TiXmlElement *ael = config->FirstChildElement(
"actuator");
134 const char *actuator_name = ael ? ael->Attribute(
"name") : NULL;
137 ROS_ERROR(
"PR2GripperTransmission could not find actuator named \"%s\"", actuator_name);
140 actuator_names_.push_back(actuator_name);
142 for (TiXmlElement *j = config->FirstChildElement(
"gap_joint"); j; j = j->NextSiblingElement(
"gap_joint"))
144 const char *gap_joint_name = j->Attribute(
"name");
147 ROS_ERROR(
"PR2GripperTransmission did not specify joint name");
150 gap_joint_ = std::string(gap_joint_name);
151 joint_names_.push_back(gap_joint_name);
154 const char *joint_reduction = j->Attribute(
"mechanical_reduction");
155 if (!joint_reduction)
157 ROS_ERROR(
"PR2GripperTransmission's joint \"%s\" has no coefficient: mechanical reduction.", gap_joint_name);
162 gap_mechanical_reduction_ = boost::lexical_cast<
double>(joint_reduction);
164 catch (boost::bad_lexical_cast &e)
166 ROS_ERROR(
"joint_reduction (%s) is not a float",joint_reduction);
171 const char *screw_reduction_str = j->Attribute(
"screw_reduction");
172 if (screw_reduction_str == NULL)
174 screw_reduction_ = 2.0/1000.0;
175 ROS_WARN(
"PR2GripperTransmission's joint \"%s\" has no coefficient: screw drive reduction, using default for PR2 alpha2.", gap_joint_name);
180 screw_reduction_ = boost::lexical_cast<
double>(screw_reduction_str);
182 catch (boost::bad_lexical_cast &e)
184 ROS_ERROR(
"screw_reduction (%s) is not a float",screw_reduction_str);
189 const char *gear_ratio_str = j->Attribute(
"gear_ratio");
190 if (gear_ratio_str == NULL)
193 ROS_WARN(
"PR2GripperTransmission's joint \"%s\" has no coefficient: gear_ratio, using default for PR2 alpha2.", gap_joint_name);
198 gear_ratio_ = boost::lexical_cast<
double>(gear_ratio_str);
200 catch (boost::bad_lexical_cast &e)
202 ROS_ERROR(
"gear_ratio (%s) is not a float",gear_ratio_str);
207 const char *theta0_str = j->Attribute(
"theta0");
208 if (theta0_str == NULL)
210 theta0_ = 2.97571*M_PI/180.0;
211 ROS_WARN(
"PR2GripperTransmission's joint \"%s\" has no coefficient: theta0, using default for PR2 alpha2.", gap_joint_name);
216 theta0_ = boost::lexical_cast<
double>(theta0_str);
218 catch (boost::bad_lexical_cast &e)
220 ROS_ERROR(
"theta0 (%s) is not a float",theta0_str);
224 const char *phi0_str = j->Attribute(
"phi0");
225 if (phi0_str == NULL)
227 phi0_ = 29.98717*M_PI/180.0;
228 ROS_WARN(
"PR2GripperTransmission's joint \"%s\" has no coefficient: phi0, using default for PR2 alpha2.", gap_joint_name);
233 phi0_ = boost::lexical_cast<
double>(phi0_str);
235 catch (boost::bad_lexical_cast &e)
237 ROS_ERROR(
"phi0 (%s) is not a float",phi0_str);
241 const char *t0_str = j->Attribute(
"t0");
244 t0_ = -0.19543/1000.0;
245 ROS_WARN(
"PR2GripperTransmission's joint \"%s\" has no coefficient: t0, using default for PR2 alpha2.", gap_joint_name);
250 t0_ = boost::lexical_cast<
double>(t0_str);
252 catch (boost::bad_lexical_cast &e)
254 ROS_ERROR(
"t0 (%s) is not a float",t0_str);
258 const char *L0_str = j->Attribute(
"L0");
261 L0_ = 34.70821/1000.0;
262 ROS_WARN(
"PR2GripperTransmission's joint \"%s\" has no coefficient: L0, using default for PR2 alpha2.", gap_joint_name);
267 L0_ = boost::lexical_cast<
double>(L0_str);
269 catch (boost::bad_lexical_cast &e)
271 ROS_ERROR(
"L0 (%s) is not a float",L0_str);
275 const char *h_str = j->Attribute(
"h");
279 ROS_WARN(
"PR2GripperTransmission's joint \"%s\" has no coefficient: h, using default for PR2 alpha2.", gap_joint_name);
284 h_ = boost::lexical_cast<
double>(h_str);
286 catch (boost::bad_lexical_cast &e)
288 ROS_ERROR(
"h (%s) is not a float",h_str);
292 const char *a_str = j->Attribute(
"a");
295 a_ = 67.56801/1000.0;
296 ROS_WARN(
"PR2GripperTransmission's joint \"%s\" has no coefficient: a, using default for PR2 alpha2.", gap_joint_name);
301 a_ = boost::lexical_cast<
double>(a_str);
303 catch (boost::bad_lexical_cast &e)
305 ROS_ERROR(
"a (%s) is not a float",a_str);
309 const char *b_str = j->Attribute(
"b");
312 b_ = 48.97193/1000.0;
313 ROS_WARN(
"PR2GripperTransmission's joint \"%s\" has no coefficient: b, using default for PR2 alpha2.", gap_joint_name);
318 b_ = boost::lexical_cast<
double>(b_str);
320 catch (boost::bad_lexical_cast &e)
322 ROS_ERROR(
"b (%s) is not a float",b_str);
326 const char *r_str = j->Attribute(
"r");
329 r_ = 91.50000/1000.0;
330 ROS_WARN(
"PR2GripperTransmission's joint \"%s\" has no coefficient: r, using default for PR2 alpha2.", gap_joint_name);
335 r_ = boost::lexical_cast<
double>(r_str);
337 catch (boost::bad_lexical_cast &e)
339 ROS_ERROR(
"r (%s) is not a float",r_str);
345 ROS_DEBUG(
"Gripper transmission parameters for %s: a=%f, b=%f, r=%f, h=%f, L0=%f, t0=%f, theta0=%f, phi0=%f, gear_ratio=%f, screw_red=%f",
346 name_.c_str(), a_, b_, r_, h_, L0_, t0_, theta0_, phi0_, gear_ratio_, screw_reduction_);
349 for (TiXmlElement *j = config->FirstChildElement(
"passive_joint"); j; j = j->NextSiblingElement(
"passive_joint"))
351 const char *passive_joint_name = j->Attribute(
"name");
352 if (!passive_joint_name)
354 ROS_ERROR(
"PR2GripperTransmission did not specify joint name");
360 joint_names_.push_back(passive_joint_name);
362 passive_joints_.push_back(passive_joint_name);
366 for (TiXmlElement *j = config->FirstChildElement(
"simulated_actuated_joint"); j; j = j->NextSiblingElement(
"simulated_actuated_joint"))
368 const char *simulated_actuated_joint_name = j->Attribute(
"name");
369 if (simulated_actuated_joint_name)
372 joint_names_.push_back(simulated_actuated_joint_name);
377 ROS_ERROR(
"PR2GripperTransmission simulated_actuated_joint did snot specify joint name");
382 const char *simulated_reduction = j->Attribute(
"simulated_reduction");
383 if (!simulated_reduction)
385 ROS_ERROR(
"PR2GripperTransmission's simulated_actuated_joint \"%s\" has no coefficient: simulated_reduction.", simulated_actuated_joint_name);
390 simulated_reduction_ = boost::lexical_cast<
double>(simulated_reduction);
392 catch (boost::bad_lexical_cast &e)
394 ROS_ERROR(
"simulated_reduction (%s) is not a float",simulated_reduction);
404 const char *passive_actuated_joint_name = j->Attribute(
"passive_actuated_joint");
405 if (passive_actuated_joint_name)
407 has_simulated_passive_actuated_joint_ =
true;
409 joint_names_.push_back(passive_actuated_joint_name);
421 void PR2GripperTransmission::computeGapStates(
422 double MR,
double MR_dot,
double MT,
423 double &theta,
double &dtheta_dMR,
double &dt_dtheta,
double &dt_dMR,
double &gap_size,
double &gap_velocity,
double &gap_effort)
428 double u = (a_*a_+b_*b_-h_*h_
429 -
pow(L0_+MR*screw_reduction_/gear_ratio_,2))/(2.0*a_*b_);
430 u = u < -1.0 ? -1.0 : u > 1.0 ? 1.0 : u;
431 theta = theta0_ - phi0_ +
acos(u);
437 gap_size = t0_ + r_ * (
sin(theta) -
sin(theta0_) );
443 MR = MR >= 0.0 ? MR : 0.0;
445 u = (a_*a_+b_*b_-h_*h_
446 -
pow(L0_+MR*screw_reduction_/gear_ratio_,2))/(2.0*a_*b_);
447 u = u < -1.0 ? -1.0 : u > 1.0 ? 1.0 : u;
448 double tmp_theta = theta0_ - phi0_ + acos(u);
451 double arg = 1.0 -
pow(u,2);
452 arg = arg >
TOL ? arg :
TOL;
454 double du_dMR = -(L0_ * screw_reduction_)/(gear_ratio_*a_*b_)
455 -MR/(a_*b_)*
pow(screw_reduction_/gear_ratio_,2);
457 dtheta_dMR = -1.0/
sqrt(arg) * du_dMR;
459 dt_dtheta = r_ *
cos( tmp_theta );
460 dt_dMR = dt_dtheta * dtheta_dMR;
461 gap_velocity = MR_dot * dt_dMR;
470 gap_effort = MT / dt_dMR /
RAD2MR ;
478 void PR2GripperTransmission::inverseGapStates(
479 double gap_size,
double &MR,
double &dMR_dtheta,
double &dtheta_dt,
double &dMR_dt)
482 double sin_theta = (gap_size - t0_)/r_ +
sin(theta0_);
483 sin_theta = sin_theta > 1.0 ? 1.0 : sin_theta < -1.0 ? -1.0 : sin_theta;
484 double theta =
asin(sin_theta);
487 inverseGapStates1(theta,MR,dMR_dtheta,dtheta_dt,dMR_dt);
494 void PR2GripperTransmission::inverseGapStates1(
495 double theta,
double &MR,
double &dMR_dtheta,
double &dtheta_dt,
double &dMR_dt)
503 double arg = -2.0*a_*b_*
cos(theta-theta0_+phi0_)
507 MR = gear_ratio_/screw_reduction_ * (
sqrt(arg) - L0_ );
508 dMR_dtheta = gear_ratio_/(2.0 * screw_reduction_) /
sqrt(arg)
509 * 2.0 * a_ * b_ *
sin(theta + phi0_ - theta0_);
513 MR = gear_ratio_/screw_reduction_ * ( 0.0 - L0_ );
518 double gap_size = t0_ + r_ * (
sin(theta) -
sin(theta0_) );
524 double tmp_dMR_dtheta = fabs(dMR_dtheta);
526 double u = (gap_size - t0_)/r_ +
sin(theta0_);
527 double arg2 = 1.0 -
pow(u,2);
528 arg2 = arg2 >
TOL ? arg2 :
TOL;
529 dtheta_dt = 1.0 /
sqrt( arg2 ) / r_;
530 dMR_dt = tmp_dMR_dtheta * dtheta_dt;
537 void PR2GripperTransmission::propagatePosition(
538 std::vector<Actuator*>& as, std::vector<JointState*>& js)
544 if (has_simulated_passive_actuated_joint_) {
ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
545 else ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);
551 double MR = as[0]->state_.position_ / gap_mechanical_reduction_ *
RAD2MR ;
554 double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ *
RAD2MR ;
569 double MT = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_;
572 double theta, dtheta_dMR, dt_dtheta, dt_dMR;
574 double gap_size,gap_velocity,gap_effort;
577 computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
580 js[0]->position_ = gap_size*2.0;
581 js[0]->velocity_ = gap_velocity*2.0;
582 js[0]->measured_effort_ = gap_effort/2.0;
588 for (
size_t i = 1; i < passive_joints_.size()+1; ++i)
590 js[i]->position_ = theta - theta0_;
591 js[i]->velocity_ = dtheta_dMR * MR_dot;
592 js[i]->measured_effort_ = MT / dtheta_dMR /
RAD2MR;
593 js[i]->reference_position_ = MT / dtheta_dMR /
RAD2MR;
597 js[passive_joints_.size()+1]->position_ = 0.0;
598 js[passive_joints_.size()+1]->velocity_ = 0.0;
599 js[passive_joints_.size()+1]->measured_effort_ = 0.0;
600 js[passive_joints_.size()+1]->reference_position_ = 0.0;
601 js[passive_joints_.size()+1]->calibrated_ =
true;
603 if (has_simulated_passive_actuated_joint_)
606 js[passive_joints_.size()+2]->position_ = 0.0;
607 js[passive_joints_.size()+2]->velocity_ = 0.0;
608 js[passive_joints_.size()+2]->measured_effort_ = 0.0;
609 js[passive_joints_.size()+2]->reference_position_ = 0.0;
610 js[passive_joints_.size()+2]->calibrated_ =
true;
616 void PR2GripperTransmission::propagatePositionBackwards(
617 std::vector<JointState*>& js, std::vector<Actuator*>& as)
621 if (has_simulated_passive_actuated_joint_) {
ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
624 ROS_DEBUG(
"js [%zd], pjs [%zd]", js.size(), passive_joints_.size());
627 double MR,dMR_dtheta,dtheta_dt,dMR_dt;
632 double gap_size = js[0]->position_/2.0;
634 inverseGapStates(gap_size,MR,dMR_dtheta,dtheta_dt,dMR_dt);
635 double gap_rate = js[0]->velocity_/2.0;
636 joint_rate = gap_rate * dtheta_dt;
638 double gap_effort = js[0]->commanded_effort_;
643 as[0]->state_.position_ = MR * gap_mechanical_reduction_ /
RAD2MR ;
647 as[0]->state_.velocity_ = joint_rate * dMR_dtheta * gap_mechanical_reduction_ /
RAD2MR ;
652 as[0]->state_.last_measured_effort_ = 2.0*gap_effort / dMR_dt *
RAD2MR * gap_mechanical_reduction_;
655 if (!simulated_actuator_timestamp_initialized_)
664 simulated_actuator_timestamp_initialized_ =
true;
670 as[0]->state_.sample_timestamp_ =
ros::Time::now() - simulated_actuator_start_time_;
673 as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.
toSec();
676 this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
679 void PR2GripperTransmission::propagateEffort(
680 std::vector<JointState*>& js, std::vector<Actuator*>& as)
684 if (has_simulated_passive_actuated_joint_) {
ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
685 else {
ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);}
694 double gap_size = js[0]->position_/2.0;
697 double MR,dMR_dtheta,dtheta_dt,dMR_dt;
699 inverseGapStates(gap_size,MR,dMR_dtheta,dtheta_dt,dMR_dt);
701 double gap_effort = js[0]->commanded_effort_;
706 as[0]->command_.enable_ =
true;
707 as[0]->command_.effort_ = 2.0*gap_effort / dMR_dt *
RAD2MR * gap_mechanical_reduction_;
710 void PR2GripperTransmission::propagateEffortBackwards(
711 std::vector<Actuator*>& as, std::vector<JointState*>& js)
715 ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);
723 double MR = as[0]->state_.position_ / gap_mechanical_reduction_ *
RAD2MR ;
724 double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ *
RAD2MR ;
725 double MT = as[0]->command_.effort_ / gap_mechanical_reduction_;
728 double theta, dtheta_dMR, dt_dtheta, dt_dMR;
730 double gap_size,gap_velocity,gap_effort;
733 computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
741 js[passive_joints_.size()+1]->commanded_effort_ = gap_effort/simulated_reduction_;
PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::PR2GripperTransmission, pr2_mechanism_model::Transmission) bool PR2GripperTransmission
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
ROSCPP_DECL bool isStarted()
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
This class provides the controllers with an interface to the robot model.
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)