A floating joint. More...
#include <floating_joint_model.h>
Public Member Functions | |
virtual void | computeJointStateValues (const Eigen::Affine3d &transf, std::vector< double > &joint_values) const |
Given the transform generated by joint, compute the corresponding joint values. | |
virtual void | computeTransform (const std::vector< double > &joint_values, Eigen::Affine3d &transf) const |
Given the joint values for a joint, compute the corresponding transform. | |
virtual double | distance (const std::vector< double > &values1, const std::vector< double > &values2) const |
Compute the distance between two joint states of the same model (represented by the variable values) | |
double | distanceRotation (const std::vector< double > &values1, const std::vector< double > &values2) const |
double | distanceTranslation (const std::vector< double > &values1, const std::vector< double > &values2) const |
virtual void | enforceBounds (std::vector< double > &values, const Bounds &other_bounds) const |
Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous joints are made between -Pi and Pi. | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | FloatingJointModel (const std::string &name) |
double | getAngularDistanceWeight () const |
virtual double | getMaximumExtent (const Bounds &other_bounds) const |
Get the extent of the state space (the maximum value distance() can ever report) | |
virtual unsigned int | getStateSpaceDimension () const |
Get the dimension of the state space that corresponds to this joint. | |
virtual void | getVariableDefaultValues (std::vector< double > &values, const Bounds &other_bounds) const |
Provide a default value for the joint given the joint variable bounds. Most joints will use the default implementation provided in this base class, but the quaternion for example needs a different implementation. The vector is NOT cleared; elements are only added with push_back. | |
virtual void | getVariableRandomValues (random_numbers::RandomNumberGenerator &rng, std::vector< double > &values, const Bounds &other_bounds) const |
Provide random values for the joint variables (within specified bounds). The vector is NOT cleared; elements are only added with push_back. | |
virtual void | getVariableRandomValuesNearBy (random_numbers::RandomNumberGenerator &rng, std::vector< double > &values, const Bounds &other_bounds, const std::vector< double > &near, const double distance) const |
Provide random values for the joint variables (within specified bounds). The vector is NOT cleared; elements are only added with push_back. | |
virtual void | interpolate (const std::vector< double > &from, const std::vector< double > &to, const double t, std::vector< double > &state) const |
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state. The memory location of state is not required to be different from the memory of either from or to. | |
bool | normalizeRotation (std::vector< double > &values) const |
virtual bool | satisfiesBounds (const std::vector< double > &values, const Bounds &other_bounds, double margin) const |
Check if the set of values for the variables of this joint are within bounds, up to some margin. | |
void | setAngularDistanceWeight (double weight) |
virtual void | updateTransform (const std::vector< double > &joint_values, Eigen::Affine3d &transf) const |
Update a transform so that it corresponds to the new joint values assuming that transf was previously set to identity and that only calls to updateTransform() were issued afterwards. | |
Private Attributes | |
double | angular_distance_weight_ |
Friends | |
class | RobotModel |
A floating joint.
Definition at line 46 of file floating_joint_model.h.
robot_model::FloatingJointModel::FloatingJointModel | ( | const std::string & | name | ) |
Definition at line 42 of file floating_joint_model.cpp.
void robot_model::FloatingJointModel::computeJointStateValues | ( | const Eigen::Affine3d & | transform, |
std::vector< double > & | joint_values | ||
) | const [virtual] |
Given the transform generated by joint, compute the corresponding joint values.
Implements robot_model::JointModel.
Definition at line 210 of file floating_joint_model.cpp.
void robot_model::FloatingJointModel::computeTransform | ( | const std::vector< double > & | joint_values, |
Eigen::Affine3d & | transf | ||
) | const [virtual] |
Given the joint values for a joint, compute the corresponding transform.
Implements robot_model::JointModel.
Definition at line 199 of file floating_joint_model.cpp.
double robot_model::FloatingJointModel::distance | ( | const std::vector< double > & | values1, |
const std::vector< double > & | values2 | ||
) | const [virtual] |
Compute the distance between two joint states of the same model (represented by the variable values)
Implements robot_model::JointModel.
Definition at line 72 of file floating_joint_model.cpp.
double robot_model::FloatingJointModel::distanceRotation | ( | const std::vector< double > & | values1, |
const std::vector< double > & | values2 | ||
) | const |
Definition at line 96 of file floating_joint_model.cpp.
double robot_model::FloatingJointModel::distanceTranslation | ( | const std::vector< double > & | values1, |
const std::vector< double > & | values2 | ||
) | const |
Definition at line 86 of file floating_joint_model.cpp.
void robot_model::FloatingJointModel::enforceBounds | ( | std::vector< double > & | values, |
const Bounds & | other_bounds | ||
) | const [virtual] |
Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous joints are made between -Pi and Pi.
Implements robot_model::JointModel.
Definition at line 185 of file floating_joint_model.cpp.
double robot_model::FloatingJointModel::getAngularDistanceWeight | ( | ) | const [inline] |
Definition at line 68 of file floating_joint_model.h.
double robot_model::FloatingJointModel::getMaximumExtent | ( | const Bounds & | other_bounds | ) | const [virtual] |
Get the extent of the state space (the maximum value distance() can ever report)
Implements robot_model::JointModel.
Definition at line 64 of file floating_joint_model.cpp.
unsigned int robot_model::FloatingJointModel::getStateSpaceDimension | ( | ) | const [virtual] |
Get the dimension of the state space that corresponds to this joint.
Implements robot_model::JointModel.
Definition at line 180 of file floating_joint_model.cpp.
void robot_model::FloatingJointModel::getVariableDefaultValues | ( | std::vector< double > & | values, |
const Bounds & | other_bounds | ||
) | const [virtual] |
Provide a default value for the joint given the joint variable bounds. Most joints will use the default implementation provided in this base class, but the quaternion for example needs a different implementation. The vector is NOT cleared; elements are only added with push_back.
Implements robot_model::JointModel.
Definition at line 223 of file floating_joint_model.cpp.
void robot_model::FloatingJointModel::getVariableRandomValues | ( | random_numbers::RandomNumberGenerator & | rng, |
std::vector< double > & | values, | ||
const Bounds & | other_bounds | ||
) | const [virtual] |
Provide random values for the joint variables (within specified bounds). The vector is NOT cleared; elements are only added with push_back.
Implements robot_model::JointModel.
Definition at line 241 of file floating_joint_model.cpp.
void robot_model::FloatingJointModel::getVariableRandomValuesNearBy | ( | random_numbers::RandomNumberGenerator & | rng, |
std::vector< double > & | values, | ||
const Bounds & | other_bounds, | ||
const std::vector< double > & | near, | ||
const double | distance | ||
) | const [virtual] |
Provide random values for the joint variables (within specified bounds). The vector is NOT cleared; elements are only added with push_back.
Implements robot_model::JointModel.
Definition at line 266 of file floating_joint_model.cpp.
void robot_model::FloatingJointModel::interpolate | ( | const std::vector< double > & | from, |
const std::vector< double > & | to, | ||
const double | t, | ||
std::vector< double > & | state | ||
) | const [virtual] |
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state. The memory location of state is not required to be different from the memory of either from or to.
Implements robot_model::JointModel.
Definition at line 106 of file floating_joint_model.cpp.
bool robot_model::FloatingJointModel::normalizeRotation | ( | std::vector< double > & | values | ) | const |
Normalize the quaternion (warn if norm is 0, and set to identity); Return true if any change was made
Definition at line 152 of file floating_joint_model.cpp.
bool robot_model::FloatingJointModel::satisfiesBounds | ( | const std::vector< double > & | values, |
const Bounds & | other_bounds, | ||
double | margin | ||
) | const [virtual] |
Check if the set of values for the variables of this joint are within bounds, up to some margin.
Implements robot_model::JointModel.
Definition at line 136 of file floating_joint_model.cpp.
void robot_model::FloatingJointModel::setAngularDistanceWeight | ( | double | weight | ) | [inline] |
Definition at line 73 of file floating_joint_model.h.
void robot_model::FloatingJointModel::updateTransform | ( | const std::vector< double > & | joint_values, |
Eigen::Affine3d & | transf | ||
) | const [virtual] |
Update a transform so that it corresponds to the new joint values assuming that transf was previously set to identity and that only calls to updateTransform() were issued afterwards.
Implements robot_model::JointModel.
Definition at line 204 of file floating_joint_model.cpp.
friend class RobotModel [friend] |
Reimplemented from robot_model::JointModel.
Definition at line 48 of file floating_joint_model.h.
double robot_model::FloatingJointModel::angular_distance_weight_ [private] |
Definition at line 88 of file floating_joint_model.h.