Joint Trajectory Point used to describe a joint goal for a robot trajectory. More...
#include <joint_trajectory_pt.h>
Public Member Functions | |
virtual descartes_core::TrajectoryPtPtr | copy () const |
virtual bool | isValid (const descartes_core::RobotModel &model) const |
JointTrajectoryPt (const descartes_core::TimingConstraint &timing=descartes_core::TimingConstraint()) | |
All frames initialized to Identity, joint values left empty. | |
JointTrajectoryPt (const std::vector< TolerancedJointValue > &joints, const descartes_core::Frame &tool, const descartes_core::Frame &wobj, const descartes_core::TimingConstraint &timing=descartes_core::TimingConstraint()) | |
Full joint trajectory point constructor. | |
JointTrajectoryPt (const std::vector< TolerancedJointValue > &joints, const descartes_core::TimingConstraint &timing=descartes_core::TimingConstraint()) | |
Full joint trajectory point constructor. | |
JointTrajectoryPt (const std::vector< double > &joints, const descartes_core::TimingConstraint &timing=descartes_core::TimingConstraint()) | |
Full joint trajectory point constructor. | |
const std::vector< double > & | lower () const |
const std::vector< double > & | nominal () const |
virtual bool | setDiscretization (const std::vector< double > &discretization) |
Set discretization. Each joint can have a different discretization. | |
void | setJoints (const std::vector< TolerancedJointValue > &joints) |
void | setTool (const descartes_core::Frame &tool) |
void | setWobj (const descartes_core::Frame &wobj) |
const std::vector< double > & | upper () const |
virtual | ~JointTrajectoryPt () |
Getters for Cartesian pose(s) | |
virtual bool | getClosestCartPose (const std::vector< double > &seed_state, const descartes_core::RobotModel &model, Eigen::Affine3d &pose) const |
virtual bool | getNominalCartPose (const std::vector< double > &seed_state, const descartes_core::RobotModel &model, Eigen::Affine3d &pose) const |
virtual void | getCartesianPoses (const descartes_core::RobotModel &model, EigenSTL::vector_Affine3d &poses) const |
Getters for joint pose(s) | |
virtual bool | getClosestJointPose (const std::vector< double > &seed_state, const descartes_core::RobotModel &model, std::vector< double > &joint_pose) const |
virtual bool | getNominalJointPose (const std::vector< double > &seed_state, const descartes_core::RobotModel &model, std::vector< double > &joint_pose) const |
virtual void | getJointPoses (const descartes_core::RobotModel &model, std::vector< std::vector< double > > &joint_poses) const |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | |
Protected Attributes | |
std::vector< double > | discretization_ |
How finely to discretize each joint. | |
std::vector< double > | lower_ |
std::vector< double > | nominal_ |
std::vector< double > | upper_ |
JointTrajectoryPt transforms. Used in get*CartPose() methods and for interpolation. | |
descartes_core::Frame | tool_ |
Transform from robot wrist to active tool pt. | |
descartes_core::Frame | wobj_ |
Transform from world to active workobject pt. |
Joint Trajectory Point used to describe a joint goal for a robot trajectory.
Background: The TOOL is something held by the robot. It is located relative to robot wrist/tool plate. The WOBJ is something that exists in the world/global environment that is not held by robot.
For a JointTrajectoryPt, the transform from wrist to tool, and base to workobject, are defined by fixed frames. These transforms are important when calculating interpolation. The joint position is specified as a nominal with upper/lower tolerances.
The get*Pose() methods of JointTrajectoryPt try to set joint positions of a robot such that tool_ is coincident with wobj_.
Definition at line 67 of file joint_trajectory_pt.h.
descartes_trajectory::JointTrajectoryPt::JointTrajectoryPt | ( | const descartes_core::TimingConstraint & | timing = descartes_core::TimingConstraint() | ) |
All frames initialized to Identity, joint values left empty.
Definition at line 54 of file src/joint_trajectory_pt.cpp.
descartes_trajectory::JointTrajectoryPt::JointTrajectoryPt | ( | const std::vector< TolerancedJointValue > & | joints, |
const descartes_core::Frame & | tool, | ||
const descartes_core::Frame & | wobj, | ||
const descartes_core::TimingConstraint & | timing = descartes_core::TimingConstraint() |
||
) |
Full joint trajectory point constructor.
joints | Fixed joint position with tolerance |
tool | Transform from robot wrist to active tool pt. |
wobj | Transform from world to active workobject pt. |
Definition at line 60 of file src/joint_trajectory_pt.cpp.
descartes_trajectory::JointTrajectoryPt::JointTrajectoryPt | ( | const std::vector< TolerancedJointValue > & | joints, |
const descartes_core::TimingConstraint & | timing = descartes_core::TimingConstraint() |
||
) |
Full joint trajectory point constructor.
joints | Fixed joint position with tolerance |
Definition at line 70 of file src/joint_trajectory_pt.cpp.
descartes_trajectory::JointTrajectoryPt::JointTrajectoryPt | ( | const std::vector< double > & | joints, |
const descartes_core::TimingConstraint & | timing = descartes_core::TimingConstraint() |
||
) |
Full joint trajectory point constructor.
joints | Fixed joint position |
Definition at line 79 of file src/joint_trajectory_pt.cpp.
virtual descartes_trajectory::JointTrajectoryPt::~JointTrajectoryPt | ( | ) | [inline, virtual] |
Definition at line 104 of file joint_trajectory_pt.h.
virtual descartes_core::TrajectoryPtPtr descartes_trajectory::JointTrajectoryPt::copy | ( | ) | const [inline, virtual] |
Implements descartes_core::TrajectoryPt.
Definition at line 150 of file joint_trajectory_pt.h.
void descartes_trajectory::JointTrajectoryPt::getCartesianPoses | ( | const descartes_core::RobotModel & | model, |
EigenSTL::vector_Affine3d & | poses | ||
) | const [virtual] |
Implements descartes_core::TrajectoryPt.
Definition at line 102 of file src/joint_trajectory_pt.cpp.
bool descartes_trajectory::JointTrajectoryPt::getClosestCartPose | ( | const std::vector< double > & | seed_state, |
const descartes_core::RobotModel & | model, | ||
Eigen::Affine3d & | pose | ||
) | const [virtual] |
Implements descartes_core::TrajectoryPt.
Definition at line 90 of file src/joint_trajectory_pt.cpp.
bool descartes_trajectory::JointTrajectoryPt::getClosestJointPose | ( | const std::vector< double > & | seed_state, |
const descartes_core::RobotModel & | model, | ||
std::vector< double > & | joint_pose | ||
) | const [virtual] |
Implements descartes_core::TrajectoryPt.
Definition at line 107 of file src/joint_trajectory_pt.cpp.
void descartes_trajectory::JointTrajectoryPt::getJointPoses | ( | const descartes_core::RobotModel & | model, |
std::vector< std::vector< double > > & | joint_poses | ||
) | const [virtual] |
Implements descartes_core::TrajectoryPt.
Definition at line 129 of file src/joint_trajectory_pt.cpp.
bool descartes_trajectory::JointTrajectoryPt::getNominalCartPose | ( | const std::vector< double > & | seed_state, |
const descartes_core::RobotModel & | model, | ||
Eigen::Affine3d & | pose | ||
) | const [virtual] |
Implements descartes_core::TrajectoryPt.
Definition at line 96 of file src/joint_trajectory_pt.cpp.
bool descartes_trajectory::JointTrajectoryPt::getNominalJointPose | ( | const std::vector< double > & | seed_state, |
const descartes_core::RobotModel & | model, | ||
std::vector< double > & | joint_pose | ||
) | const [virtual] |
Implements descartes_core::TrajectoryPt.
Definition at line 121 of file src/joint_trajectory_pt.cpp.
bool descartes_trajectory::JointTrajectoryPt::isValid | ( | const descartes_core::RobotModel & | model | ) | const [virtual] |
Implements descartes_core::TrajectoryPt.
Definition at line 137 of file src/joint_trajectory_pt.cpp.
const std::vector<double>& descartes_trajectory::JointTrajectoryPt::lower | ( | ) | const [inline] |
Definition at line 183 of file joint_trajectory_pt.h.
const std::vector<double>& descartes_trajectory::JointTrajectoryPt::nominal | ( | ) | const [inline] |
Definition at line 171 of file joint_trajectory_pt.h.
bool descartes_trajectory::JointTrajectoryPt::setDiscretization | ( | const std::vector< double > & | discretization | ) | [virtual] |
Set discretization. Each joint can have a different discretization.
discretization | Vector of discretization values. If length=1, set all elements of discretization_ are set to value. |
Implements descartes_core::TrajectoryPt.
Definition at line 142 of file src/joint_trajectory_pt.cpp.
void descartes_trajectory::JointTrajectoryPt::setJoints | ( | const std::vector< TolerancedJointValue > & | joints | ) |
Definition at line 171 of file src/joint_trajectory_pt.cpp.
void descartes_trajectory::JointTrajectoryPt::setTool | ( | const descartes_core::Frame & | tool | ) | [inline] |
Definition at line 158 of file joint_trajectory_pt.h.
void descartes_trajectory::JointTrajectoryPt::setWobj | ( | const descartes_core::Frame & | wobj | ) | [inline] |
Definition at line 164 of file joint_trajectory_pt.h.
const std::vector<double>& descartes_trajectory::JointTrajectoryPt::upper | ( | ) | const [inline] |
Definition at line 177 of file joint_trajectory_pt.h.
std::vector<double> descartes_trajectory::JointTrajectoryPt::discretization_ [protected] |
How finely to discretize each joint.
Definition at line 192 of file joint_trajectory_pt.h.
Definition at line 70 of file joint_trajectory_pt.h.
std::vector<double> descartes_trajectory::JointTrajectoryPt::lower_ [protected] |
Definition at line 190 of file joint_trajectory_pt.h.
std::vector<double> descartes_trajectory::JointTrajectoryPt::nominal_ [protected] |
Definition at line 189 of file joint_trajectory_pt.h.
Transform from robot wrist to active tool pt.
Definition at line 197 of file joint_trajectory_pt.h.
std::vector<double> descartes_trajectory::JointTrajectoryPt::upper_ [protected] |
Definition at line 191 of file joint_trajectory_pt.h.
Transform from world to active workobject pt.
Definition at line 198 of file joint_trajectory_pt.h.