Public Member Functions | Protected Attributes
JointCommandWrist Class Reference

This class provides the joint command interface for the wrist. More...

#include <JointCommandWrist.h>

Inheritance diagram for JointCommandWrist:
Inheritance graph
[legend]

List of all members.

Public Member Functions

r2_msgs::JointCapability getCapability ()
r2_msgs::JointCommand getCommandedState ()
sensor_msgs::JointState getCompleteMeasuredState ()
sensor_msgs::JointState getSimpleMeasuredState ()
 JointCommandWrist (const std::string &mechanism, IoFunctions ioFunctions)
void loadCoeffs ()
void setCommand (r2_msgs::JointCommand msg, r2_msgs::JointControlData control)
void setFaultState ()
void updateMeasuredState (r2_msgs::JointControlData msg)
virtual ~JointCommandWrist ()

Protected Attributes

Eigen::Vector2d ang
std::string angleRosJointName
Eigen::Vector2d angVel
double busVoltage
bool calCalled
bool calFailureReported
r2_msgs::JointControlCalibrationMode calibrationState
unsigned int completeMessageSize
Eigen::Vector2d desiredAngle
std::string desiredMotComLittleCommandElement
std::string desiredMotComThumbCommandElement
double desiredPitch
double desiredPitchVel
double desiredQ1
double desiredQ2
Eigen::Vector2d desiredSlider
double desiredYaw
double desiredYawVel
Eigen::Vector2d encoder
std::string encoderLittlePositionStatusElement
std::string encoderRosJointName
std::string encoderThumbPositionStatusElement
Eigen::Vector2d encoderVel
Eigen::Vector2d filteredHalls
Eigen::Vector2d filteredSlider
bool fwdKinFault
bool hallFilterInitialized
Eigen::Vector2d halls
std::string hallsPitchStatusElement
std::string hallsRosJointName
Eigen::Vector2d hallsVel
std::string hallsYawStatusElement
std::vector< HallsToAnglehta
Eigen::Vector2d manualCalAng
Eigen::Vector2d manualCalSliderPos
double maxSensorErrorFault
double maxSliderDiff
double maxSliderDiffFault
std::string motorRosJointName
std::string nameCalibrationMode
std::string nameCoeffState
std::string nameLittlesideSliderLimit
std::string nameManualCalLittleSlider
std::string nameManualCalPitch
std::string nameManualCalThumbSlider
std::string nameManualCalYaw
std::string namePitchCalcValue
std::string namePitchHallsValue
std::string namePitchLimit
std::string namePitchMaxValue
std::string namePitchMinValue
std::string nameSensorError
std::string nameSliderDiffError
std::string nameThumbsideSliderLimit
std::string nameUseHalls
std::string nameYawCalcValue
std::string nameYawHallsValue
std::string nameYawLimit
std::string nameYawMaxValue
std::string nameYawMinValue
double positionAlpha
std::vector< double > positionVals
bool prevPosInitialized
std::vector< double > prevPositionVals
std::auto_ptr
< MultiLoopController > 
q1Controller
std::auto_ptr
< MultiLoopController > 
q2Controller
bool sliderFilterInitialized
double sliderMaxPosition
double sliderMaxPositionFault
double sliderMinPosition
double sliderMinPositionFault
std::string sliderRosJointName
Eigen::Vector2d sliderVel
Eigen::Vector2d sliderVelCommand
double smoothedPitch
double smoothedYaw
std::auto_ptr< EmbeddedSmoothersmootherPitch
std::auto_ptr< EmbeddedSmoothersmootherYaw
std::vector< std::string >
::iterator 
strVecIt
double timestep
bool useHalls
std::vector< double > velocityVals
std::auto_ptr< WristMechanismwrist
std::auto_ptr< WristMechanismwristFault

Detailed Description

This class provides the joint command interface for the wrist.

Definition at line 24 of file JointCommandWrist.h.


Constructor & Destructor Documentation

JointCommandWrist::JointCommandWrist ( const std::string &  mechanism,
IoFunctions  ioFunctions 
)

Definition at line 8 of file JointCommandWrist.cpp.

Definition at line 217 of file JointCommandWrist.cpp.


Member Function Documentation

r2_msgs::JointCapability JointCommandWrist::getCapability ( ) [virtual]

create a function to find the joint velocity limits (forward rate kinematics- Jacobian) RDEV-2305

Implements JointCommandInterface.

Definition at line 1107 of file JointCommandWrist.cpp.

r2_msgs::JointCommand JointCommandWrist::getCommandedState ( ) [virtual]

Implements JointCommandInterface.

Definition at line 952 of file JointCommandWrist.cpp.

sensor_msgs::JointState JointCommandWrist::getCompleteMeasuredState ( ) [virtual]

Implements JointCommandInterface.

Definition at line 846 of file JointCommandWrist.cpp.

sensor_msgs::JointState JointCommandWrist::getSimpleMeasuredState ( ) [virtual]

Implements JointCommandInterface.

Definition at line 833 of file JointCommandWrist.cpp.

void JointCommandWrist::loadCoeffs ( ) [virtual]

Initialize live coeff names

Initialize messages

Related to TODO in RDEV-2305

Parse parameter file

Status Elements

Command Elements

Multi-loop parameters

Capability Elements

limits

calibration

Safety Elements

Manual Cal elements Initialize params

live coeff

set defaults

Implements JointCommandInterface.

Definition at line 222 of file JointCommandWrist.cpp.

void JointCommandWrist::setCommand ( r2_msgs::JointCommand  msg,
r2_msgs::JointControlData  control 
) [virtual]

confirm msg has required parts

get the desiredQ1 and desiredQ2 from the current ControlCommandMode we are in

if we've errored out, there is something wrong with the kinematics here, send to park

if we've errored out, there is something wrong with the kinematics here, send to park

set the motcoms

Implements JointCommandInterface.

Definition at line 958 of file JointCommandWrist.cpp.

WristPosLimMonitor

WristSliderLimMonitor

WristSensorErrorMonitor

WristSliderDiffMonitor

Definition at line 856 of file JointCommandWrist.cpp.

void JointCommandWrist::updateMeasuredState ( r2_msgs::JointControlData  msg) [virtual]

check forward kinematics is good

only output this message once until not faulted again.

Implements JointCommandInterface.

Definition at line 582 of file JointCommandWrist.cpp.


Member Data Documentation

Eigen::Vector2d JointCommandWrist::ang [protected]

Definition at line 73 of file JointCommandWrist.h.

std::string JointCommandWrist::angleRosJointName [protected]

Definition at line 46 of file JointCommandWrist.h.

Eigen::Vector2d JointCommandWrist::angVel [protected]

Definition at line 74 of file JointCommandWrist.h.

double JointCommandWrist::busVoltage [protected]

Definition at line 70 of file JointCommandWrist.h.

bool JointCommandWrist::calCalled [protected]

Definition at line 136 of file JointCommandWrist.h.

Definition at line 135 of file JointCommandWrist.h.

r2_msgs::JointControlCalibrationMode JointCommandWrist::calibrationState [protected]

Definition at line 137 of file JointCommandWrist.h.

unsigned int JointCommandWrist::completeMessageSize [protected]

Definition at line 148 of file JointCommandWrist.h.

Eigen::Vector2d JointCommandWrist::desiredAngle [protected]

Definition at line 65 of file JointCommandWrist.h.

Definition at line 67 of file JointCommandWrist.h.

Definition at line 68 of file JointCommandWrist.h.

double JointCommandWrist::desiredPitch [protected]

Definition at line 59 of file JointCommandWrist.h.

Definition at line 61 of file JointCommandWrist.h.

double JointCommandWrist::desiredQ1 [protected]

Definition at line 57 of file JointCommandWrist.h.

double JointCommandWrist::desiredQ2 [protected]

Definition at line 58 of file JointCommandWrist.h.

Eigen::Vector2d JointCommandWrist::desiredSlider [protected]

Definition at line 64 of file JointCommandWrist.h.

double JointCommandWrist::desiredYaw [protected]

Definition at line 60 of file JointCommandWrist.h.

Definition at line 62 of file JointCommandWrist.h.

Eigen::Vector2d JointCommandWrist::encoder [protected]

Definition at line 75 of file JointCommandWrist.h.

Definition at line 52 of file JointCommandWrist.h.

std::string JointCommandWrist::encoderRosJointName [protected]

Definition at line 48 of file JointCommandWrist.h.

Definition at line 53 of file JointCommandWrist.h.

Eigen::Vector2d JointCommandWrist::encoderVel [protected]

Definition at line 76 of file JointCommandWrist.h.

Eigen::Vector2d JointCommandWrist::filteredHalls [protected]

Definition at line 91 of file JointCommandWrist.h.

Eigen::Vector2d JointCommandWrist::filteredSlider [protected]

Definition at line 90 of file JointCommandWrist.h.

Definition at line 131 of file JointCommandWrist.h.

Definition at line 96 of file JointCommandWrist.h.

Eigen::Vector2d JointCommandWrist::halls [protected]

Definition at line 77 of file JointCommandWrist.h.

Definition at line 54 of file JointCommandWrist.h.

std::string JointCommandWrist::hallsRosJointName [protected]

Definition at line 50 of file JointCommandWrist.h.

Eigen::Vector2d JointCommandWrist::hallsVel [protected]

Definition at line 78 of file JointCommandWrist.h.

Definition at line 55 of file JointCommandWrist.h.

std::vector<HallsToAngle> JointCommandWrist::hta [protected]

Definition at line 129 of file JointCommandWrist.h.

Eigen::Vector2d JointCommandWrist::manualCalAng [protected]

Definition at line 100 of file JointCommandWrist.h.

Eigen::Vector2d JointCommandWrist::manualCalSliderPos [protected]

Definition at line 99 of file JointCommandWrist.h.

Definition at line 87 of file JointCommandWrist.h.

Definition at line 82 of file JointCommandWrist.h.

Definition at line 86 of file JointCommandWrist.h.

std::string JointCommandWrist::motorRosJointName [protected]

Definition at line 47 of file JointCommandWrist.h.

std::string JointCommandWrist::nameCalibrationMode [protected]

Definition at line 108 of file JointCommandWrist.h.

std::string JointCommandWrist::nameCoeffState [protected]

Definition at line 109 of file JointCommandWrist.h.

Definition at line 104 of file JointCommandWrist.h.

Definition at line 123 of file JointCommandWrist.h.

std::string JointCommandWrist::nameManualCalPitch [protected]

Definition at line 124 of file JointCommandWrist.h.

Definition at line 122 of file JointCommandWrist.h.

std::string JointCommandWrist::nameManualCalYaw [protected]

Definition at line 125 of file JointCommandWrist.h.

std::string JointCommandWrist::namePitchCalcValue [protected]

Definition at line 116 of file JointCommandWrist.h.

std::string JointCommandWrist::namePitchHallsValue [protected]

Definition at line 115 of file JointCommandWrist.h.

std::string JointCommandWrist::namePitchLimit [protected]

Definition at line 102 of file JointCommandWrist.h.

std::string JointCommandWrist::namePitchMaxValue [protected]

Definition at line 111 of file JointCommandWrist.h.

std::string JointCommandWrist::namePitchMinValue [protected]

Definition at line 112 of file JointCommandWrist.h.

std::string JointCommandWrist::nameSensorError [protected]

Definition at line 106 of file JointCommandWrist.h.

std::string JointCommandWrist::nameSliderDiffError [protected]

Definition at line 107 of file JointCommandWrist.h.

Definition at line 105 of file JointCommandWrist.h.

std::string JointCommandWrist::nameUseHalls [protected]

Definition at line 121 of file JointCommandWrist.h.

std::string JointCommandWrist::nameYawCalcValue [protected]

Definition at line 118 of file JointCommandWrist.h.

std::string JointCommandWrist::nameYawHallsValue [protected]

Definition at line 117 of file JointCommandWrist.h.

std::string JointCommandWrist::nameYawLimit [protected]

Definition at line 103 of file JointCommandWrist.h.

std::string JointCommandWrist::nameYawMaxValue [protected]

Definition at line 113 of file JointCommandWrist.h.

std::string JointCommandWrist::nameYawMinValue [protected]

Definition at line 114 of file JointCommandWrist.h.

Definition at line 94 of file JointCommandWrist.h.

std::vector<double> JointCommandWrist::positionVals [protected]

Definition at line 139 of file JointCommandWrist.h.

Definition at line 97 of file JointCommandWrist.h.

std::vector<double> JointCommandWrist::prevPositionVals [protected]

Definition at line 92 of file JointCommandWrist.h.

std::auto_ptr<MultiLoopController> JointCommandWrist::q1Controller [protected]

Definition at line 43 of file JointCommandWrist.h.

std::auto_ptr<MultiLoopController> JointCommandWrist::q2Controller [protected]

Definition at line 44 of file JointCommandWrist.h.

Definition at line 95 of file JointCommandWrist.h.

Definition at line 80 of file JointCommandWrist.h.

Definition at line 84 of file JointCommandWrist.h.

Definition at line 81 of file JointCommandWrist.h.

Definition at line 85 of file JointCommandWrist.h.

std::string JointCommandWrist::sliderRosJointName [protected]

Definition at line 49 of file JointCommandWrist.h.

Eigen::Vector2d JointCommandWrist::sliderVel [protected]

Definition at line 72 of file JointCommandWrist.h.

Eigen::Vector2d JointCommandWrist::sliderVelCommand [protected]

Definition at line 146 of file JointCommandWrist.h.

Definition at line 144 of file JointCommandWrist.h.

double JointCommandWrist::smoothedYaw [protected]

Definition at line 145 of file JointCommandWrist.h.

Definition at line 142 of file JointCommandWrist.h.

Definition at line 143 of file JointCommandWrist.h.

std::vector<std::string>::iterator JointCommandWrist::strVecIt [protected]

Definition at line 134 of file JointCommandWrist.h.

double JointCommandWrist::timestep [protected]

Definition at line 93 of file JointCommandWrist.h.

bool JointCommandWrist::useHalls [protected]

Definition at line 127 of file JointCommandWrist.h.

std::vector<double> JointCommandWrist::velocityVals [protected]

Definition at line 140 of file JointCommandWrist.h.

std::auto_ptr<WristMechanism> JointCommandWrist::wrist [protected]

Definition at line 41 of file JointCommandWrist.h.

std::auto_ptr<WristMechanism> JointCommandWrist::wristFault [protected]

Definition at line 42 of file JointCommandWrist.h.


The documentation for this class was generated from the following files:


robodyn_mechanisms
Author(s):
autogenerated on Thu Jun 6 2019 21:22:49