32 #ifndef MECHANISM_MODEL_CHAIN_H 33 #define MECHANISM_MODEL_CHAIN_H 36 #include <kdl/chain.hpp> 37 #include <kdl/jntarray.hpp> 38 #include <kdl/jntarrayvel.hpp> 39 #include <kdl/jntarrayacc.hpp> 56 bool init(
RobotState *robot_state,
const std::string &root,
const std::string &tip);
66 assert((
int)v.size() == (int)
joints_.size());
67 for (
size_t i = 0; i <
joints_.size(); ++i)
79 assert((
int)v.size() == (int)
joints_.size());
80 for (
size_t i = 0; i <
joints_.size(); ++i)
101 assert((
int)v.size() == (int)
joints_.size());
102 for (
size_t i = 0; i <
joints_.size(); ++i)
103 joints_[i]->commanded_effort_ += v[i];
void getPositions(Vec &v)
gets the joint positions of the chain as any type with size() and []
std::vector< JointState * > joints_
void getPositions(std::vector< double > &)
get the joint positions of the chain as a std vector
void setEfforts(KDL::JntArray &)
set the commanded joint efforts of the chain as a std vector
void getVelocities(Vec &v)
gets the joint velocities of the chain as any type with size() and []
void getVelocities(std::vector< double > &)
get the joint velocities of the chain as a std vector
void addEfforts(const Vec &v)
Adds efforts from any type that implements size() and [] lookup.
bool allCalibrated()
returns true if all the joints in the chain are calibrated
pr2_mechanism_model::RobotState * robot_state_
void getEfforts(std::vector< double > &)
get the measured joint efforts of the chain as a std vector
This class provides the controllers with an interface to the robot state.
JointState * getJoint(unsigned int actuated_joint_i)
get a joint state of an actuated joint of the chain.
bool init(RobotState *robot_state, const std::string &root, const std::string &tip)
initialize the chain object
void toKDL(KDL::Chain &chain)
get a kdl chain object that respresents the chain from root to tip
void addEfforts(KDL::JntArray &)
set the commanded joint efforts of the chain as a kdl jnt array
int size() const
Returns the number of actuated joints in the chain.