Public Member Functions | Protected Member Functions | Protected Attributes
descartes_moveit::MoveitStateAdapter Class Reference

MoveitStateAdapter adapts the MoveIt RobotState to the Descartes RobotModel interface. More...

#include <moveit_state_adapter.h>

Inheritance diagram for descartes_moveit::MoveitStateAdapter:
Inheritance graph
[legend]

List of all members.

Public Member Functions

virtual bool getAllIK (const Eigen::Affine3d &pose, std::vector< std::vector< double > > &joint_poses) const
virtual int getDOF () const
virtual bool getFK (const std::vector< double > &joint_pose, Eigen::Affine3d &pose) const
virtual bool getIK (const Eigen::Affine3d &pose, const std::vector< double > &seed_state, std::vector< double > &joint_pose) const
const std::vector< std::vector
< double > > & 
getSeedStates () const
 Retrieves the initial seed states used by iterative inverse kinematic solvers.
moveit::core::RobotStatePtr getState ()
 Returns the underlying moveit state object so it can be used to generate seeds.
virtual bool initialize (const std::string &robot_description, const std::string &group_name, const std::string &world_frame, const std::string &tcp_frame)
virtual bool initialize (robot_model::RobotModelConstPtr robot_model, const std::string &group_name, const std::string &world_frame, const std::string &tcp_frame)
virtual bool isValid (const std::vector< double > &joint_pose) const
virtual bool isValid (const Eigen::Affine3d &pose) const
virtual bool isValidMove (const std::vector< double > &from_joint_pose, const std::vector< double > &to_joint_pose, double dt) const
 MoveitStateAdapter ()
void setSeedStates (const std::vector< std::vector< double > > &seeds)
 Set the initial states used for iterative inverse kineamtics.
void setState (const moveit::core::RobotState &state)
 Copies the internal state of 'state' into this model. Useful for initializing the value of joints that are not part of the active move group. Should be called after 'initialize()'.
virtual ~MoveitStateAdapter ()

Protected Member Functions

bool getIK (const Eigen::Affine3d &pose, std::vector< double > &joint_pose) const
bool isInCollision (const std::vector< double > &joint_pose) const

Protected Attributes

std::string group_name_
 Planning group name.
const
moveit::core::JointModelGroup
joint_group_
planning_scene::PlanningScenePtr planning_scene_
robot_model_loader::RobotModelLoaderPtr robot_model_loader_
robot_model::RobotModelConstPtr robot_model_ptr_
moveit::core::RobotStatePtr robot_state_
std::vector< std::vector
< double > > 
seed_states_
 Vector of starting configurations for the numerical solver.
std::string tool_frame_
 Tool frame name.
std::vector< double > velocity_limits_
std::string world_frame_
 Work object/reference frame name.
descartes_core::Frame world_to_root_
 convenient transformation frame

Detailed Description

MoveitStateAdapter adapts the MoveIt RobotState to the Descartes RobotModel interface.

Definition at line 34 of file moveit_state_adapter.h.


Constructor & Destructor Documentation

Definition at line 66 of file moveit_state_adapter.cpp.

Definition at line 39 of file moveit_state_adapter.h.


Member Function Documentation

bool descartes_moveit::MoveitStateAdapter::getAllIK ( const Eigen::Affine3d &  pose,
std::vector< std::vector< double > > &  joint_poses 
) const [virtual]

Implements descartes_core::RobotModel.

Definition at line 301 of file moveit_state_adapter.cpp.

bool descartes_moveit::MoveitStateAdapter::getFK ( const std::vector< double > &  joint_pose,
Eigen::Affine3d &  pose 
) const [virtual]
bool descartes_moveit::MoveitStateAdapter::getIK ( const Eigen::Affine3d &  pose,
const std::vector< double > &  seed_state,
std::vector< double > &  joint_pose 
) const [virtual]
bool descartes_moveit::MoveitStateAdapter::getIK ( const Eigen::Affine3d &  pose,
std::vector< double > &  joint_pose 
) const [protected]

Gets IK solution (assumes robot state is pre-seeded)

Parameters:
poseAffine pose of TOOL in WOBJ frame
joint_poseSolution (if function successful).
Returns:

Definition at line 144 of file moveit_state_adapter.cpp.

const std::vector<std::vector<double> >& descartes_moveit::MoveitStateAdapter::getSeedStates ( ) const [inline]

Retrieves the initial seed states used by iterative inverse kinematic solvers.

Definition at line 77 of file moveit_state_adapter.h.

moveit::core::RobotStatePtr descartes_moveit::MoveitStateAdapter::getState ( ) [inline]

Returns the underlying moveit state object so it can be used to generate seeds.

Definition at line 85 of file moveit_state_adapter.h.

bool descartes_moveit::MoveitStateAdapter::initialize ( const std::string &  robot_description,
const std::string &  group_name,
const std::string &  world_frame,
const std::string &  tcp_frame 
) [virtual]

Implements descartes_core::RobotModel.

Reimplemented in descartes_moveit::IkFastMoveitStateAdapter.

Definition at line 70 of file moveit_state_adapter.cpp.

bool descartes_moveit::MoveitStateAdapter::initialize ( robot_model::RobotModelConstPtr  robot_model,
const std::string &  group_name,
const std::string &  world_frame,
const std::string &  tcp_frame 
) [virtual]

Definition at line 79 of file moveit_state_adapter.cpp.

bool descartes_moveit::MoveitStateAdapter::isInCollision ( const std::vector< double > &  joint_pose) const [protected]

TODO: Checks for collisions at this joint pose. The setCollisionCheck(true) must have been called previously in order to enable collision checks, otherwise it will return false.

Parameters:
joint_posethe joint values at which check for collisions will be made

Definition at line 237 of file moveit_state_adapter.cpp.

bool descartes_moveit::MoveitStateAdapter::isValid ( const std::vector< double > &  joint_pose) const [virtual]

Implements descartes_core::RobotModel.

Definition at line 274 of file moveit_state_adapter.cpp.

bool descartes_moveit::MoveitStateAdapter::isValid ( const Eigen::Affine3d &  pose) const [virtual]

Implements descartes_core::RobotModel.

Definition at line 294 of file moveit_state_adapter.cpp.

bool descartes_moveit::MoveitStateAdapter::isValidMove ( const std::vector< double > &  from_joint_pose,
const std::vector< double > &  to_joint_pose,
double  dt 
) const [virtual]

Implements descartes_core::RobotModel.

Definition at line 306 of file moveit_state_adapter.cpp.

void descartes_moveit::MoveitStateAdapter::setSeedStates ( const std::vector< std::vector< double > > &  seeds) [inline]

Set the initial states used for iterative inverse kineamtics.

Parameters:
seedsVector of vector of doubles representing joint positions. Be sure that it's sized correctly for the DOF.

Definition at line 69 of file moveit_state_adapter.h.

Copies the internal state of 'state' into this model. Useful for initializing the value of joints that are not part of the active move group. Should be called after 'initialize()'.

Reimplemented in descartes_moveit::IkFastMoveitStateAdapter.

Definition at line 327 of file moveit_state_adapter.cpp.


Member Data Documentation

Planning group name.

Definition at line 137 of file moveit_state_adapter.h.

Definition at line 127 of file moveit_state_adapter.h.

planning_scene::PlanningScenePtr descartes_moveit::MoveitStateAdapter::planning_scene_ [protected]

Definition at line 121 of file moveit_state_adapter.h.

robot_model_loader::RobotModelLoaderPtr descartes_moveit::MoveitStateAdapter::robot_model_loader_ [protected]

Definition at line 125 of file moveit_state_adapter.h.

robot_model::RobotModelConstPtr descartes_moveit::MoveitStateAdapter::robot_model_ptr_ [protected]

Definition at line 123 of file moveit_state_adapter.h.

moveit::core::RobotStatePtr descartes_moveit::MoveitStateAdapter::robot_state_ [mutable, protected]

Definition at line 119 of file moveit_state_adapter.h.

std::vector<std::vector<double> > descartes_moveit::MoveitStateAdapter::seed_states_ [protected]

Vector of starting configurations for the numerical solver.

Definition at line 132 of file moveit_state_adapter.h.

Tool frame name.

Definition at line 142 of file moveit_state_adapter.h.

Maximum joint velocities (rad/s) for each joint in the chain. Used for checking in `isValidMove()`

Definition at line 117 of file moveit_state_adapter.h.

Work object/reference frame name.

Definition at line 147 of file moveit_state_adapter.h.

convenient transformation frame

Definition at line 152 of file moveit_state_adapter.h.


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


descartes_moveit
Author(s): Shaun Edwards
autogenerated on Thu Jun 6 2019 21:36:08