Drake
ConstraintRelaxingIk Class Reference

A wrapper class around the IK planner. More...

#include <drake/manipulation/planner/constraint_relaxing_ik.h>

Classes

struct  IkCartesianWaypoint
 Cartesian waypoint. More...
 

Public Member Functions

 ConstraintRelaxingIk (const std::string &model_path, const std::string &end_effector_link_name, const Isometry3< double > &base_to_world)
 Constructor. More...
 
void SetEndEffector (const RigidBody< double > &end_effector_body)
 Sets end effector to end_effector_body. More...
 
void SetEndEffector (const std::string &link_name)
 Sets end effector to link_name. More...
 
const RigidBodyTree< double > & get_robot () const
 Returns constant reference to the robot model. More...
 
bool PlanSequentialTrajectory (const std::vector< IkCartesianWaypoint > &waypoints, const VectorX< double > &q_current, IKResults *ik_res)
 Generates IK solutions for each waypoint sequentially. More...
 
Does not allow copy, move, or assignment
 ConstraintRelaxingIk (const ConstraintRelaxingIk &)=delete
 
ConstraintRelaxingIkoperator= (const ConstraintRelaxingIk &)=delete
 
 ConstraintRelaxingIk (ConstraintRelaxingIk &&)=delete
 
ConstraintRelaxingIkoperator= (ConstraintRelaxingIk &&)=delete
 

Detailed Description

A wrapper class around the IK planner.

This class improves IK's usability by handling constraint relaxing and multiple initial guesses internally.

Constructor & Destructor Documentation

ConstraintRelaxingIk ( const std::string &  model_path,
const std::string &  end_effector_link_name,
const Isometry3< double > &  base_to_world 
)

Constructor.

Instantiates an internal RigidBodyTree from model_path.

Parameters
model_pathPath to the model file.
end_effector_link_nameLink name of the end effector.
base_to_worldX_WB, transformation from robot's base to the world frame.

Here is the call graph for this function:

Member Function Documentation

const RigidBodyTree<double>& get_robot ( ) const
inline

Returns constant reference to the robot model.

Here is the call graph for this function:

Here is the caller graph for this function:

ConstraintRelaxingIk& operator= ( const ConstraintRelaxingIk )
delete
ConstraintRelaxingIk& operator= ( ConstraintRelaxingIk &&  )
delete
bool PlanSequentialTrajectory ( const std::vector< IkCartesianWaypoint > &  waypoints,
const VectorX< double > &  q_current,
IKResults ik_res 
)

Generates IK solutions for each waypoint sequentially.

For waypoint wp_i, the IK tries to solve q_i that satisfies the end effector constraints in wp_i and minimizes the squared difference to q_{i-1}, where q_{i-1} is the solution to the previous wp_{i-1}. q_{i-1} = q_current when i = 0. This function internally does constraint relaxing and initial condition guessing if necessary.

Note that q_current is inserted at the beginning of ik_res.

Parameters
waypointsA sequence of desired waypoints.
q_currentThe initial generalized position.
[out]ik_resResults.
Returns
True if solved successfully.

Here is the call graph for this function:

Here is the caller graph for this function:

void SetEndEffector ( const RigidBody< double > &  end_effector_body)
inline

Sets end effector to end_effector_body.

Here is the call graph for this function:

Here is the caller graph for this function:

void SetEndEffector ( const std::string &  link_name)
inline

Sets end effector to link_name.


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