#include <pr2_gripper_transmission.h>
Public Member Functions | |
bool | initXml (TiXmlElement *config) |
Initializes the transmission from XML data. More... | |
bool | initXml (TiXmlElement *config, Robot *robot) |
Initializes the transmission from XML data. More... | |
PR2GripperTransmission () | |
void | propagateEffort (std::vector< pr2_mechanism_model::JointState * > &, std::vector< pr2_hardware_interface::Actuator * > &) |
Uses commanded joint efforts to fill out commanded motor currents. More... | |
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. More... | |
void | propagatePosition (std::vector< pr2_hardware_interface::Actuator * > &, std::vector< pr2_mechanism_model::JointState * > &) |
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. More... | |
virtual | ~PR2GripperTransmission () |
Public Member Functions inherited from pr2_mechanism_model::Transmission | |
Transmission () | |
Constructor. More... | |
virtual | ~Transmission () |
Destructor. More... | |
Public Attributes | |
std::string | gap_joint_ |
double | gap_mechanical_reduction_ |
bool | has_simulated_passive_actuated_joint_ |
std::vector< std::string > | passive_joints_ |
double | simulated_reduction_ |
bool | use_simulated_actuated_joint_ |
Public Attributes inherited from pr2_mechanism_model::Transmission | |
std::vector< std::string > | actuator_names_ |
std::vector< std::string > | joint_names_ |
std::string | name_ |
the name of the transmission More... | |
Private Member Functions | |
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 More... | |
void | inverseGapStates (double gap_size, double &MR, double &dMR_dtheta, double &dtheta_dt, double &dMR_dt) |
void | inverseGapStates1 (double theta, double &MR, double &dMR_dtheta, double &dtheta_dt, double &dMR_dt) |
Private Attributes | |
double | a_ |
double | b_ |
double | gear_ratio_ |
double | h_ |
JointCalibrationSimulator | joint_calibration_simulator_ |
double | L0_ |
double | phi0_ |
double | r_ |
double | screw_reduction_ |
ros::Time | simulated_actuator_start_time_ |
int | simulated_actuator_timestamp_initialized_ |
double | t0_ |
double | theta0_ |
Definition at line 55 of file pr2_gripper_transmission.h.
|
inline |
Definition at line 58 of file pr2_gripper_transmission.h.
|
inlinevirtual |
Definition at line 59 of file pr2_gripper_transmission.h.
|
private |
compute gap position, velocity and measured effort from actuator states
given actuator states (motor revolustion, joint torques), compute gap properties.
Definition at line 421 of file pr2_gripper_transmission.cpp.
|
virtual |
Initializes the transmission from XML data.
Reimplemented from pr2_mechanism_model::Transmission.
Definition at line 128 of file pr2_gripper_transmission.cpp.
|
virtual |
Initializes the transmission from XML data.
Implements pr2_mechanism_model::Transmission.
|
private |
inverse of computeGapStates() need theta as input computes MR, dMR_dtheta, dtheta_dt, dMR_dt
Definition at line 478 of file pr2_gripper_transmission.cpp.
|
private |
inverse of computeGapStates() need theta as input computes MR, dMR_dtheta, dtheta_dt, dMR_dt
Definition at line 494 of file pr2_gripper_transmission.cpp.
|
virtual |
Uses commanded joint efforts to fill out commanded motor currents.
Newtons
actuator commanded effort = gap_dffort / dMR_dt / (2*pi) * gap_mechanical_reduction_
Implements pr2_mechanism_model::Transmission.
Definition at line 679 of file pr2_gripper_transmission.cpp.
|
virtual |
Uses the actuator's commanded effort to fill out the torque on the joint.
taken from propagatePosition()
internal theta state, gripper closed it is theta0_. same as finger joint angles + theta0_.
information on the fictitious joint: gap_joint
Implements pr2_mechanism_model::Transmission.
Definition at line 710 of file pr2_gripper_transmission.cpp.
|
virtual |
assign joint position, velocity, effort from actuator state all passive joints are assigned by single actuator state through mimic?
motor revolutions = encoder value * gap_mechanical_reduction_ * RAD2MR motor revolutions = motor angle(rad) / (2*pi) = theta / (2*pi)
motor revolustions per second = motor angle rate (rad per second) / (2*pi)
old MT definition - obsolete
but we convert it to Nm*(MR/rad) motor torque = actuator_state->last_meausured_effort motor torque = I * theta_ddot MT = I * MR_ddot (my definition) = I * theta_ddot / (2*pi) = motot torque / (2*pi)
gripper motor torque: received from hardware side in newton-meters
internal theta state, gripper closed it is theta0_. same as finger joint angles + theta0_.
information on the fictitious joint: gap_joint
Implements pr2_mechanism_model::Transmission.
Definition at line 537 of file pr2_gripper_transmission.cpp.
|
virtual |
Uses the joint position to fill out the actuator's encoder.
should be exact inverse of propagatePosition() call
state velocity = MR_dot * gap_mechanical_reduction_ / rad2mr = theta_dot * dMR_dtheta * gap_mechanical_reduction_ / rad2mr
motor torque = inverse of getting gap effort from motor torque = gap_effort * dt_dMR / (2*pi) * gap_mechanical_reduction_ = gap_effort / dMR_dt * RAD2MR * gap_mechanical_reduction_
Implements pr2_mechanism_model::Transmission.
Definition at line 616 of file pr2_gripper_transmission.cpp.
|
private |
Definition at line 109 of file pr2_gripper_transmission.h.
|
private |
Definition at line 110 of file pr2_gripper_transmission.h.
std::string pr2_mechanism_model::PR2GripperTransmission::gap_joint_ |
Definition at line 72 of file pr2_gripper_transmission.h.
double pr2_mechanism_model::PR2GripperTransmission::gap_mechanical_reduction_ |
Definition at line 77 of file pr2_gripper_transmission.h.
|
private |
Definition at line 103 of file pr2_gripper_transmission.h.
|
private |
Definition at line 108 of file pr2_gripper_transmission.h.
bool pr2_mechanism_model::PR2GripperTransmission::has_simulated_passive_actuated_joint_ |
Definition at line 82 of file pr2_gripper_transmission.h.
|
private |
Definition at line 119 of file pr2_gripper_transmission.h.
|
private |
Definition at line 107 of file pr2_gripper_transmission.h.
std::vector<std::string> pr2_mechanism_model::PR2GripperTransmission::passive_joints_ |
Definition at line 89 of file pr2_gripper_transmission.h.
|
private |
Definition at line 105 of file pr2_gripper_transmission.h.
|
private |
Definition at line 111 of file pr2_gripper_transmission.h.
|
private |
Definition at line 102 of file pr2_gripper_transmission.h.
|
private |
Definition at line 117 of file pr2_gripper_transmission.h.
|
private |
Definition at line 116 of file pr2_gripper_transmission.h.
double pr2_mechanism_model::PR2GripperTransmission::simulated_reduction_ |
Definition at line 80 of file pr2_gripper_transmission.h.
|
private |
Definition at line 106 of file pr2_gripper_transmission.h.
|
private |
Definition at line 104 of file pr2_gripper_transmission.h.
bool pr2_mechanism_model::PR2GripperTransmission::use_simulated_actuated_joint_ |
Definition at line 81 of file pr2_gripper_transmission.h.