MTC
|
#include <compute_ik.h>
Public Member Functions | |
ComputeIK (const std::string &name="IK", Stage::pointer &&child=Stage::pointer()) | |
void | reset () override |
reset stage, clearing all solutions, interfaces, inherited properties. | |
void | init (const core::RobotModelConstPtr &robot_model) override |
void | onNewSolution (const SolutionBase &s) override |
called by a (direct) child when a new solution becomes available | |
bool | canCompute () const override |
void | compute () override |
void | setEndEffector (const std::string &eef) |
void | setGroup (const std::string &group) |
void | setIKFrame (const geometry_msgs::PoseStamped &pose) |
setters for IK frame | |
void | setIKFrame (const Eigen::Isometry3d &pose, const std::string &link) |
template<typename T > | |
void | setIKFrame (const T &p, const std::string &link) |
void | setIKFrame (const std::string &link) |
void | setTargetPose (const geometry_msgs::PoseStamped &pose) |
void | setTargetPose (const Eigen::Isometry3d &pose, const std::string &frame="") |
template<typename T > | |
void | setTargetPose (const T &p, const std::string &frame="") |
void | setMaxIKSolutions (uint32_t n) |
void | setIgnoreCollisions (bool flag) |
void | setMinSolutionDistance (double distance) |
![]() | |
WrapperBase (const std::string &name="wrapper", Stage::pointer &&child=Stage::pointer()) | |
void | insert (Stage::pointer &&stage, int before=-1) override |
insertion is only allowed if children() is empty | |
Stage * | wrapped () |
access the single wrapped child, NULL if still empty | |
const Stage * | wrapped () const |
![]() | |
ParallelContainerBase (const std::string &name="parallel container") | |
![]() | |
void | setPruning (bool pruning) |
Explicitly enable/disable pruning. | |
bool | pruning () const |
size_t | numChildren () const |
Stage * | findChild (const std::string &name) const |
Stage * | operator[] (int index) const |
bool | traverseChildren (const StageCallback &processor) const |
traverse direct children of this container, calling the callback for each of them | |
bool | traverseRecursively (const StageCallback &processor) const |
traverse all children of this container recursively | |
void | add (Stage::pointer &&stage) |
virtual Stage::pointer | remove (int pos) |
virtual Stage::pointer | remove (Stage *child) |
virtual void | clear () |
void | init (const moveit::core::RobotModelConstPtr &robot_model) override |
bool | explainFailure (std::ostream &os) const override |
![]() | |
const ContainerBase * | parent () const |
const std::string & | name () const |
void | setName (const std::string &name) |
uint32_t | introspectionId () const |
Introspection * | introspection () const |
void | setTimeout (double timeout) |
double | timeout () const |
timeout of stage per computation | |
void | setMarkerNS (const std::string &marker_ns) |
const std::string & | markerNS () |
marker namespace of solution markers | |
void | setTrajectoryExecutionInfo (TrajectoryExecutionInfo trajectory_execution_info) |
Set and get info to use when executing the stage's trajectory. | |
TrajectoryExecutionInfo | trajectoryExecutionInfo () const |
void | forwardProperties (const InterfaceState &source, InterfaceState &dest) |
forwarding of properties between interface states | |
std::set< std::string > | forwardedProperties () const |
void | setForwardedProperties (const std::set< std::string > &names) |
SolutionCallbackList::const_iterator | addSolutionCallback (SolutionCallback &&cb) |
add function to be called for every newly found solution or failure | |
void | removeSolutionCallback (SolutionCallbackList::const_iterator which) |
remove function callback | |
void | setCostTerm (const CostTermConstPtr &term) |
template<typename T , typename = std::enable_if_t<std::is_constructible<LambdaCostTerm, T>::value>> | |
void | setCostTerm (T term) |
const ordered< SolutionBaseConstPtr > & | solutions () const |
const std::list< SolutionBaseConstPtr > & | failures () const |
size_t | numFailures () const |
void | silentFailure () |
Call to increase number of failures w/o storing a (failure) trajectory. | |
bool | storeFailures () const |
Should we generate failure solutions? Note: Always report a failure! | |
PropertyMap & | properties () |
Get the stage's property map. | |
const PropertyMap & | properties () const |
void | setProperty (const std::string &name, const boost::any &value) |
Set a previously declared property to a new value. | |
void | setProperty (const std::string &name, const char *value) |
overload: const char* values are stored as std::string | |
void | reportPropertyError (const Property::error &e) |
Analyze source of error and report accordingly. | |
double | getTotalComputeTime () const |
Protected Attributes | |
ordered< const SolutionBase * > | upstream_solutions_ |
![]() | |
StagePrivate * | pimpl_ |
Additional Inherited Members | |
![]() | |
using | pointer = std::unique_ptr< ContainerBase > |
using | StageCallback = std::function< bool(const Stage &, unsigned int)> |
![]() | |
enum | PropertyInitializerSource : uint8_t { DEFAULT = 0 , MANUAL = 1 , PARENT = 2 , INTERFACE = 4 } |
using | pointer = std::unique_ptr< Stage > |
using | SolutionCallback = std::function< void(const SolutionBase &)> |
using | SolutionCallbackList = std::list< SolutionCallback > |
![]() | |
WrapperBase (WrapperBasePrivate *impl, Stage::pointer &&child=Stage::pointer()) | |
![]() | |
ParallelContainerBase (ParallelContainerBasePrivate *impl) | |
void | liftSolution (const SolutionBase &solution) |
lift unmodified child solution (useful for simple filtering) | |
void | liftSolution (const SolutionBase &solution, double cost) |
lift child solution to external interface, adapting the costs | |
void | liftSolution (const SolutionBase &solution, double cost, std::string comment) |
lift child solution to external interface, adapting the costs and comment | |
void | spawn (InterfaceState &&state, SubTrajectory &&trajectory) |
spawn a new solution with given state as start and end | |
void | sendForward (const InterfaceState &from, InterfaceState &&to, SubTrajectory &&trajectory) |
propagate a solution forwards | |
void | sendBackward (InterfaceState &&from, const InterfaceState &to, SubTrajectory &&trajectory) |
propagate a solution backwards | |
![]() | |
ContainerBase (ContainerBasePrivate *impl) | |
![]() | |
Stage (StagePrivate *impl) | |
Stage can only be instantiated through derived classes. | |
Stage (const Stage &)=delete | |
Stage cannot be copied. | |
Wrapper for any pose generator stage to compute IK poses for a Cartesian pose.
The wrapper reads a target_pose from the interface state of solutions provided by the wrapped stage. This Cartesian pose (PoseStamped msg) is used as a goal pose for inverse kinematics.
Usually, the end effector's parent link or the group's tip link is used as the IK frame, which should be moved to the goal frame. However, any other IK frame can be defined (which is linked to the tip of the group).
Properties of the internally received InterfaceState can be forwarded to the newly generated, externally exposed InterfaceState.
|
inline |
setters for target pose property
This property should almost always be set in the InterfaceState sent by the child. If possible, avoid setting it manually.