Cartesian path planner for moveit. More...
#include <cartesian_planner.h>
Public Member Functions | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | CartesianPlanner (const std::string &name, const std::string &group) |
CartesianPlanner Constructor. | |
CartesianPlanner (const CartesianPlanner &other) | |
CartesianPlanner Copy Constructor. | |
void | clear () override |
Clear planner data. | |
bool | initializeSolver () |
This will initialize the solver if it has not all ready. | |
void | resetSolverConfiguration () |
Reset the planners IK solver configuration to it default settings. | |
void | setSolverConfiguration (const ConstrainedIKConfiguration &config) |
Set the planners IK solver configuration. | |
bool | solve (planning_interface::MotionPlanResponse &res) override |
Inverse Kinematic Solve. | |
bool | solve (planning_interface::MotionPlanDetailedResponse &res) override |
Inverse Kinematic Solve. | |
bool | terminate () override |
Terminate the active planner solve. | |
Private Member Functions | |
std::vector< Eigen::Affine3d, Eigen::aligned_allocator < Eigen::Affine3d > > | interpolateCartesian (const Eigen::Affine3d &start, const Eigen::Affine3d &stop, double ds, double dt) const |
Preform position and orientation interpolation between start and stop. | |
Private Attributes | |
boost::mutex | mutex_ |
std::string | robot_description_ |
robot_model::RobotModelConstPtr | robot_model_ |
boost::shared_ptr< Constrained_IK > | solver_ |
boost::atomic< bool > | terminate_ |
Cartesian path planner for moveit.
This class is used to represent a cartesian path planner for moveit. It finds a straight line path between the start and goal position. This planner does not have the inherent ability to avoid collision. It does check if the path created is collision free before it returns a trajectory. If a collision is found it returns an empty trajectory and moveit error.
Definition at line 52 of file cartesian_planner.h.
constrained_ik::CartesianPlanner::CartesianPlanner | ( | const std::string & | name, |
const std::string & | group | ||
) |
CartesianPlanner Constructor.
name | of planner |
group | of the planner |
Definition at line 41 of file cartesian_planner.cpp.
constrained_ik::CartesianPlanner::CartesianPlanner | ( | const CartesianPlanner & | other | ) | [inline] |
CartesianPlanner Copy Constructor.
other | cartesian planner |
Definition at line 68 of file cartesian_planner.h.
This will initialize the solver if it has not all ready.
Definition at line 51 of file cartesian_planner.cpp.
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > constrained_ik::CartesianPlanner::interpolateCartesian | ( | const Eigen::Affine3d & | start, |
const Eigen::Affine3d & | stop, | ||
double | ds, | ||
double | dt | ||
) | const [private] |
Preform position and orientation interpolation between start and stop.
start | begining pose of trajectory |
stop | end pose of trajectory |
ds | max cartesian translation interpolation step |
dt | max cartesian orientation interpolation step |
Definition at line 243 of file cartesian_planner.cpp.
void constrained_ik::CartesianPlanner::setSolverConfiguration | ( | const ConstrainedIKConfiguration & | config | ) |
Set the planners IK solver configuration.
config | Constrained IK solver configuration |
Definition at line 64 of file cartesian_planner.cpp.
bool constrained_ik::CartesianPlanner::solve | ( | planning_interface::MotionPlanResponse & | res | ) | [override, virtual] |
Inverse Kinematic Solve.
res | planner response |
Implements constrained_ik::CLIKPlanningContext.
Definition at line 88 of file cartesian_planner.cpp.
bool constrained_ik::CartesianPlanner::solve | ( | planning_interface::MotionPlanDetailedResponse & | res | ) | [override, virtual] |
Inverse Kinematic Solve.
res | planner detailed response |
Implements constrained_ik::CLIKPlanningContext.
Definition at line 74 of file cartesian_planner.cpp.
bool constrained_ik::CartesianPlanner::terminate | ( | ) | [inline, override, virtual] |
Terminate the active planner solve.
Implements constrained_ik::CLIKPlanningContext.
Definition at line 80 of file cartesian_planner.h.
boost::mutex constrained_ik::CartesianPlanner::mutex_ [private] |
Mutex
Definition at line 131 of file cartesian_planner.h.
std::string constrained_ik::CartesianPlanner::robot_description_ [private] |
robot description value from ros param server
Definition at line 128 of file cartesian_planner.h.
robot_model::RobotModelConstPtr constrained_ik::CartesianPlanner::robot_model_ [private] |
Robot model object
Definition at line 129 of file cartesian_planner.h.
boost::shared_ptr<Constrained_IK> constrained_ik::CartesianPlanner::solver_ [private] |
Constrained IK Solver
Definition at line 130 of file cartesian_planner.h.
boost::atomic<bool> constrained_ik::CartesianPlanner::terminate_ [private] |
Termination flag
Definition at line 127 of file cartesian_planner.h.