43 #ifndef GRIPPER_TRANSMISSION_H 44 #define GRIPPER_TRANSMISSION_H 62 bool initXml(TiXmlElement *config);
65 std::vector<pr2_mechanism_model::JointState*>&);
67 std::vector<pr2_hardware_interface::Actuator*>&);
69 std::vector<pr2_hardware_interface::Actuator*>&);
71 std::vector<pr2_mechanism_model::JointState*>&);
94 double &theta,
double &dtheta_dMR,
double &dt_dtheta,
double &dt_dMR,
double &gap_size,
double &gap_velocity,
double &gap_effort);
95 void inverseGapStates(
double gap_size,
double &MR,
double &dMR_dtheta,
double &dtheta_dt,
double &dMR_dt);
96 void inverseGapStates1(
double theta,
double &MR,
double &dMR_dtheta,
double &dtheta_dt,
double &dMR_dt);
113 #define RAD2MR (1.0/(2.0*M_PI)) // convert radians to motor revolutions 114 #define TOL 0.00001 // limit for denominators void inverseGapStates1(double theta, double &MR, double &dMR_dtheta, double &dtheta_dt, double &dMR_dt)
int simulated_actuator_timestamp_initialized_
JointCalibrationSimulator joint_calibration_simulator_
double simulated_reduction_
ros::Time simulated_actuator_start_time_
bool use_simulated_actuated_joint_
std::vector< std::string > passive_joints_
void inverseGapStates(double gap_size, double &MR, double &dMR_dtheta, double &dtheta_dt, double &dMR_dt)
void propagatePosition(std::vector< pr2_hardware_interface::Actuator *> &, std::vector< pr2_mechanism_model::JointState *> &)
void propagateEffort(std::vector< pr2_mechanism_model::JointState *> &, std::vector< pr2_hardware_interface::Actuator *> &)
Uses commanded joint efforts to fill out commanded motor currents.
bool has_simulated_passive_actuated_joint_
double gap_mechanical_reduction_
bool initXml(TiXmlElement *config, Robot *robot)
Initializes the transmission from XML data.
virtual ~PR2GripperTransmission()
void propagatePositionBackwards(std::vector< pr2_mechanism_model::JointState *> &, std::vector< pr2_hardware_interface::Actuator *> &)
Uses the joint position to fill out the actuator's encoder.
void propagateEffortBackwards(std::vector< pr2_hardware_interface::Actuator *> &, std::vector< pr2_mechanism_model::JointState *> &)
Uses the actuator's commanded effort to fill out the torque on the joint.
void computeGapStates(double MR, double MR_dot, double MT, double &theta, double &dtheta_dMR, double &dt_dtheta, double &dt_dMR, double &gap_size, double &gap_velocity, double &gap_effort)
compute gap position, velocity and measured effort from actuator states
This class provides the controllers with an interface to the robot model.