Tesseract 0.28.4
Loading...
Searching...
No Matches
tesseract::kinematics::RTPInvKin Class Reference

Robot With Tool Positioner Inverse kinematic implementation. More...

#include <rtp_inv_kin.h>

Inheritance diagram for tesseract::kinematics::RTPInvKin:

Public Types

using Ptr = std::shared_ptr<RTPInvKin>
using ConstPtr = std::shared_ptr<const RTPInvKin>
using UPtr = std::unique_ptr<RTPInvKin>
using ConstUPtr = std::unique_ptr<const RTPInvKin>
Public Types inherited from tesseract::kinematics::InverseKinematics
using Ptr = std::shared_ptr<InverseKinematics>
using ConstPtr = std::shared_ptr<const InverseKinematics>
using UPtr = std::unique_ptr<InverseKinematics>
using ConstUPtr = std::unique_ptr<const InverseKinematics>

Public Member Functions

 RTPInvKin (const RTPInvKin &other)
RTPInvKin & operator= (const RTPInvKin &other)
 RTPInvKin (RTPInvKin &&)=default
RTPInvKin & operator= (RTPInvKin &&)=default
 RTPInvKin (const tesseract::scene_graph::SceneGraph &scene_graph, const tesseract::scene_graph::SceneState &scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, std::unique_ptr< ForwardKinematics > tool_positioner, const Eigen::VectorXd &tool_sample_resolution, std::string solver_name=DEFAULT_RTP_INV_KIN_SOLVER_NAME)
 Construct RTP inverse kinematics using full tool-joint limits as the sample range.
 RTPInvKin (const tesseract::scene_graph::SceneGraph &scene_graph, const tesseract::scene_graph::SceneState &scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, std::unique_ptr< ForwardKinematics > tool_positioner, const Eigen::MatrixX2d &tool_sample_range, const Eigen::VectorXd &tool_sample_resolution, std::string solver_name=DEFAULT_RTP_INV_KIN_SOLVER_NAME)
 As above, but with an explicit sample range per tool joint instead of the full tool-joint limits. tool_sample_range must have one row per tool joint.
 RTPInvKin (const tesseract::scene_graph::SceneGraph &scene_graph, const tesseract::scene_graph::SceneState &scene_state, InverseKinematics::UPtr manipulator, std::unique_ptr< ForwardKinematics > tool_positioner, const Eigen::VectorXd &tool_sample_resolution, std::string solver_name=DEFAULT_RTP_INV_KIN_SOLVER_NAME)
 As the full-limits ctor, but auto-deriving manipulator_reach as computeChainReachUpperBound() over the manipulator's base->tip chain. The reach is not derived when the reach filter is disabled (see the class description).
 RTPInvKin (const tesseract::scene_graph::SceneGraph &scene_graph, const tesseract::scene_graph::SceneState &scene_state, InverseKinematics::UPtr manipulator, std::unique_ptr< ForwardKinematics > tool_positioner, const Eigen::MatrixX2d &tool_sample_range, const Eigen::VectorXd &tool_sample_resolution, std::string solver_name=DEFAULT_RTP_INV_KIN_SOLVER_NAME)
 As above, auto-deriving manipulator_reach and taking an explicit tool sample range.
void calcInvKin (IKSolutions &solutions, const tesseract::common::LinkIdTransformMap &tip_link_poses, const Eigen::Ref< const Eigen::VectorXd > &seed) const override final
 See InverseKinematics::calcInvKin().
std::vector< tesseract::common::JointId > getJointIds () const override final
 Get list of joint ids for kinematic object.
Eigen::Index numJoints () const override final
 Number of joints in robot.
tesseract::common::LinkId getBaseLinkId () const override final
 Get the robot base link id.
tesseract::common::LinkId getWorkingFrame () const override final
 Get the inverse kinematics working frame.
std::vector< tesseract::common::LinkId > getTipLinkIds () const override final
 Get the ids of the tip links of the kinematics group.
std::string getSolverName () const override final
 Get the name of the solver. Recommend using the name of the class.
InverseKinematics::UPtr clone () const override final
 Clone the forward kinematics object.
Public Member Functions inherited from tesseract::kinematics::InverseKinematics
 InverseKinematics (const InverseKinematics &)=default
InverseKinematics & operator= (const InverseKinematics &)=default
 InverseKinematics (InverseKinematics &&)=default
InverseKinematics & operator= (InverseKinematics &&)=default
IKSolutions calcInvKin (const tesseract::common::LinkIdTransformMap &tip_link_poses, const Eigen::Ref< const Eigen::VectorXd > &seed) const
 Calculates joint solutions given a pose for each tip link.

Detailed Description

Robot With Tool Positioner Inverse kinematic implementation.

Wraps a manipulator inverse-kinematics solver and a tool-side forward-kinematics chain. The tool chain's joints are sampled on a LinSpaced grid; at each sample the tool-tip target is mapped back to a manipulator-tip target and the inner IK is solved.

The tool chain's contribution at each grid sample depends only on the grid, so it is evaluated once during construction and stored (sample_to_manip_tip_). calcInvKin() therefore never calls the tool forward kinematics, and the class does not retain it after construction.

Base link and working frame are the manipulator's own. Tip link reported from getTipLinkIds() is the tool chain tip (i.e. the frame the target pose is for). Joint order in the returned solution vector: manipulator joints, then tool joints.

Samples whose manipulator-tip target lies beyond the manipulator reach, measured from its base link, are skipped. That filter is disabled when a non-fixed joint separates the manipulator's working frame from its base link, since the base origin then moves in the target frame.

A rotational (revolute or continuous) tool joint is sampled over at most one full turn: a sample a full turn past another gives the same tool pose.

Note
The manipulator and the tool positioner must each have exactly one tip link; the constructor rejects anything else. The static-offset model needs a single manipulator-tip→ tool-base transform, and the tool tip is the frame target poses are interpreted in.

Constructor & Destructor Documentation

◆ RTPInvKin() [1/2]

tesseract::kinematics::RTPInvKin::RTPInvKin ( const tesseract::scene_graph::SceneGraph & scene_graph,
const tesseract::scene_graph::SceneState & scene_state,
InverseKinematics::UPtr manipulator,
double manipulator_reach,
std::unique_ptr< ForwardKinematics > tool_positioner,
const Eigen::VectorXd & tool_sample_resolution,
std::string solver_name = DEFAULT_RTP_INV_KIN_SOLVER_NAME )

Construct RTP inverse kinematics using full tool-joint limits as the sample range.

All four constructors share one contract, enforced in a single place; they differ only in whether manipulator_reach is supplied or derived, and whether the tool sample range is supplied or taken from the tool joints' full limits.

Exceptions
std::runtime_errorif any input is null or invalid: null manipulator or tool positioner, empty solver name, invalid scene graph root, a manipulator or tool positioner with other than exactly one tip link, a manipulator reach that is not greater than zero, a tool sample range or resolution whose size disagrees with the tool joint count, a malformed sample range/resolution pair (see buildSampleGrid()), a combined sample grid above the supported size, a tool-positioner base joined to the manipulator tip by any non-fixed joint, a scene_state missing a transform for the manipulator tip or tool-positioner base, or tool-positioner forward kinematics that do not return its tip link.

◆ RTPInvKin() [2/2]

tesseract::kinematics::RTPInvKin::RTPInvKin ( const tesseract::scene_graph::SceneGraph & scene_graph,
const tesseract::scene_graph::SceneState & scene_state,
InverseKinematics::UPtr manipulator,
std::unique_ptr< ForwardKinematics > tool_positioner,
const Eigen::VectorXd & tool_sample_resolution,
std::string solver_name = DEFAULT_RTP_INV_KIN_SOLVER_NAME )

As the full-limits ctor, but auto-deriving manipulator_reach as computeChainReachUpperBound() over the manipulator's base->tip chain. The reach is not derived when the reach filter is disabled (see the class description).

Exceptions
std::runtime_erroradditionally on anything propagated from computeChainReachUpperBound() (e.g. a mimic joint, or a joint without limits, on the manipulator chain).

Member Function Documentation

◆ calcInvKin()

void tesseract::kinematics::RTPInvKin::calcInvKin ( IKSolutions & solutions,
const tesseract::common::LinkIdTransformMap & tip_link_poses,
const Eigen::Ref< const Eigen::VectorXd > & seed ) const
finaloverridevirtual

See InverseKinematics::calcInvKin().

Exceptions
std::out_of_rangeif tip_link_poses has no entry for getTipLinkIds()[0]. That is a precondition of the interface, not a runtime failure, so it is asserted in debug builds and left to the map lookup otherwise.

Implements tesseract::kinematics::InverseKinematics.

◆ getJointIds()

std::vector< tesseract::common::JointId > tesseract::kinematics::RTPInvKin::getJointIds ( ) const
finaloverridevirtual

Get list of joint ids for kinematic object.

Returns
A vector of joint ids

Implements tesseract::kinematics::InverseKinematics.

◆ numJoints()

Eigen::Index tesseract::kinematics::RTPInvKin::numJoints ( ) const
finaloverridevirtual

Number of joints in robot.

Returns
Number of joints in robot

Implements tesseract::kinematics::InverseKinematics.

◆ getBaseLinkId()

tesseract::common::LinkId tesseract::kinematics::RTPInvKin::getBaseLinkId ( ) const
finaloverridevirtual

Get the robot base link id.

Implements tesseract::kinematics::InverseKinematics.

◆ getWorkingFrame()

tesseract::common::LinkId tesseract::kinematics::RTPInvKin::getWorkingFrame ( ) const
finaloverridevirtual

Get the inverse kinematics working frame.

This is the frame of reference in which all poses given to the calcInvKin function should be defined

Implements tesseract::kinematics::InverseKinematics.

◆ getTipLinkIds()

std::vector< tesseract::common::LinkId > tesseract::kinematics::RTPInvKin::getTipLinkIds ( ) const
finaloverridevirtual

Get the ids of the tip links of the kinematics group.

In the case of a kinematic chain, this returns one tip link; in the case of a kinematic tree this returns the tip link for each branch of the tree.

Implements tesseract::kinematics::InverseKinematics.

◆ getSolverName()

std::string tesseract::kinematics::RTPInvKin::getSolverName ( ) const
finaloverridevirtual

Get the name of the solver. Recommend using the name of the class.

Implements tesseract::kinematics::InverseKinematics.

◆ clone()

InverseKinematics::UPtr tesseract::kinematics::RTPInvKin::clone ( ) const
finaloverridevirtual

Clone the forward kinematics object.

Implements tesseract::kinematics::InverseKinematics.