Robowflex  v0.1
Making MoveIt Easy
robowflex::Robot::IKQuery Struct Reference

Robot IK Query options. IK queries in Robowflex consist of: a) A position specified by some geometric region (a robowflex::Geometry) at a pose. b) An orientation specified by some base orientation with allowable deviations specified by tolerances on the XYZ Euler axes. It is recommended to use the provided constructors to specify a query, or to use the addRequest() function. Multiple target tips can be specified, but note that not all kinematics solvers support multi-tip IK. Additionally, a robowflex::Scene can be specified to do collision-aware IK. More...

#include <robot.h>

Public Types

using Metric = std::function< double(const robot_state::RobotState &state, const SceneConstPtr &scene, const kinematic_constraints::ConstraintEvaluationResult &result)>
 Metric for evaluating states within an IK query. More...
 

Public Member Functions

 IKQuery (const std::string &group)
 Constructor. Empty for fine control. More...
 
Directional Offset Constructors
 IKQuery (const std::string &group, const std::string &tip, const robot_state::RobotState &start, const Eigen::Vector3d &direction, double distance)
 Constructor. Initialize an IK query based on offsets from an initial robot state. More...
 
 IKQuery (const std::string &group, const std::string &tip, const robot_state::RobotState &start, const Eigen::Vector3d &position_offset, const Eigen::Quaterniond &rotation_offset=Eigen::Quaterniond::Identity())
 Constructor. Initialize an IK query based on offsets from an initial robot state. More...
 
 IKQuery (const std::string &group, const std::string &tip, const robot_state::RobotState &start, const RobotPose &offset)
 Constructor. Initialize an IK query based on an offset from an initial robot state. Only for single-tip systems. More...
 
Single Target Constructors
 IKQuery (const std::string &group, const RobotPose &pose, double radius=constants::ik_tolerance, const Eigen::Vector3d &tolerance=constants::ik_vec_tolerance)
 Constructor. Initialize a basic IK query to reach the desired pose. More...
 
 IKQuery (const std::string &group, const Eigen::Vector3d &position, const Eigen::Quaterniond &orientation, double radius=constants::ik_tolerance, const Eigen::Vector3d &tolerance=constants::ik_vec_tolerance)
 Constructor. Initialize a basic IK query to reach the desired position and orientation. More...
 
 IKQuery (const std::string &group, const GeometryConstPtr &region, const RobotPose &pose, const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerance=constants::ik_vec_tolerance, const ScenePtr &scene=nullptr, bool verbose=false)
 Constructor. Initialize an IK query to reach somewhere in the provided region (at a pose) and orientation. More...
 
 IKQuery (const std::string &group, const RobotPose &offset, const ScenePtr &scene, const std::string &object, const Eigen::Vector3d &tolerances=constants::ik_vec_tolerance, bool verbose=false)
 
 IKQuery (const std::string &group, const moveit_msgs::PositionConstraint &pc, const moveit_msgs::OrientationConstraint &oc)
 Constructor. Initialize an IK query from MoveIt message constraints. More...
 
Multiple Target Constructors
 IKQuery (const std::string &group, const RobotPoseVector &poses, const std::vector< std::string > &input_tips, double radius=constants::ik_tolerance, const Eigen::Vector3d &tolerance=constants::ik_vec_tolerance, const ScenePtr &scene=nullptr, bool verbose=false)
 Constructor. Initialize a basic multi-target IK query so that each of the tips reach their desired poses. More...
 
 IKQuery (const std::string &group, const std::vector< std::string > &input_tips, const std::vector< GeometryConstPtr > &regions, const RobotPoseVector &poses, const std::vector< Eigen::Quaterniond > &orientations, const EigenSTL::vector_Vector3d &tolerances, const ScenePtr &scene=nullptr, bool verbose=false)
 
Other Setters
void addRequest (const std::string &tip, const GeometryConstPtr &region, const RobotPose &pose, const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerance=constants::ik_vec_tolerance)
 Add a request for a tip. More...
 
void setScene (const ScenePtr &scene, bool verbose=false)
 Set a scene to use for collision checking for this IK request. More...
 
void addMetric (const Metric &metric_function)
 Add a metric to this IK query. More...
 
void addDistanceMetric (double weight=1.)
 Add a metric to the query to evaluate distance to constraint. More...
 
void addCenteringMetric (double weight=1.)
 Add a metric to the query to evaluate how "centered" the joints of the robot are (from their 0 position). More...
 
void addClearanceMetric (double weight=1.)
 Add a metric to the query to evaluate clearance from provided scene. More...
 
Sampling Region
bool sampleRegion (RobotPose &pose, std::size_t index) const
 Sample desired end-effector pose for region at index. More...
 
bool sampleRegions (RobotPoseVector &poses) const
 Sample desired end-effector pose for each region. More...
 
Other Functions
void getMessage (const std::string &base_frame, moveit_msgs::Constraints &msg) const
 Get this IK query as a constraint message. More...
 
kinematic_constraints::KinematicConstraintSetPtr getAsConstraints (const Robot &robot) const
 Get this IK query as a kinematic constraint set. More...
 
double getMetricValue (const robot_state::RobotState &state, const kinematic_constraints::ConstraintEvaluationResult &result) const
 Get the value of the metrics assigned to this query for a given state and result. More...
 

Public Attributes

 ROBOWFLEX_EIGEN
 
Query Targets
std::string group
 Target joint group to do IK for. More...
 
std::vector< std::stringtips
 List of end-effectors. More...
 
std::vector< GeometryConstPtrregions
 Regions to sample target positions from. More...
 
RobotPoseVector region_poses
 Poses of regions. More...
 
std::vector< Eigen::Quaterniond > orientations
 Target orientations. More...
 
EigenSTL::vector_Vector3d tolerances
 XYZ Euler orientation tolerances. More...
 
Additional Solver Options
SceneConstPtr scene
 If provided, use this scene for collision checking. More...
 
bool verbose {false}
 Verbose output of collision checking. More...
 
bool random_restart {true}
 Randomly reset joint states. More...
 
kinematics::KinematicsQueryOptions options
 Other query options. More...
 
std::size_t attempts {constants::ik_attempts}
 IK attempts (samples within regions). More...
 
double timeout {0.}
 Timeout for each query. More...
 
bool validate {false}
 
double valid_distance {0.}
 
std::vector< Metricmetrics
 

Detailed Description

Robot IK Query options. IK queries in Robowflex consist of: a) A position specified by some geometric region (a robowflex::Geometry) at a pose. b) An orientation specified by some base orientation with allowable deviations specified by tolerances on the XYZ Euler axes. It is recommended to use the provided constructors to specify a query, or to use the addRequest() function. Multiple target tips can be specified, but note that not all kinematics solvers support multi-tip IK. Additionally, a robowflex::Scene can be specified to do collision-aware IK.

Definition at line 374 of file robowflex_library/include/robowflex_library/robot.h.

Member Typedef Documentation

◆ Metric

using robowflex::Robot::IKQuery::Metric = std::function<double(const robot_state::RobotState &state, const SceneConstPtr &scene, const kinematic_constraints::ConstraintEvaluationResult &result)>

Metric for evaluating states within an IK query.

Parameters
[in]stateResulting state from query.
[in]sceneInput scene for query, if available. Nullptr otherwise.
[in]resultResult of evaluating validity of constraint.
Returns
Value of the state.

Definition at line 384 of file robowflex_library/include/robowflex_library/robot.h.

Constructor & Destructor Documentation

◆ IKQuery() [1/11]

Robot::IKQuery::IKQuery ( const std::string group)

Constructor. Empty for fine control.

Parameters
[in]groupGroup to set.

Definition at line 607 of file robowflex_library/src/robot.cpp.

607  : group(group)
608 {
609 }
std::string group
Target joint group to do IK for.

◆ IKQuery() [2/11]

Robot::IKQuery::IKQuery ( const std::string group,
const std::string tip,
const robot_state::RobotState &  start,
const Eigen::Vector3d &  direction,
double  distance 
)

Constructor. Initialize an IK query based on offsets from an initial robot state.

Parameters
[in]groupGroup to set.
[in]tipTip frame to apply offset to.
[in]startInitial robot state to compute offset for.
[in]directionVector direction of end-effector motion. Will be used as unit vector.
[in]distanceDistance to travel in that direction.

Definition at line 611 of file robowflex_library/src/robot.cpp.

614  : IKQuery(group, tip, start, distance * direction.normalized())
615 {
616 }
IKQuery(const std::string &group)
Constructor. Empty for fine control.

◆ IKQuery() [3/11]

Robot::IKQuery::IKQuery ( const std::string group,
const std::string tip,
const robot_state::RobotState &  start,
const Eigen::Vector3d &  position_offset,
const Eigen::Quaterniond &  rotation_offset = Eigen::Quaterniond::Identity() 
)

Constructor. Initialize an IK query based on offsets from an initial robot state.

Parameters
[in]groupGroup to set.
[in]tipTip frame to apply offset to.
[in]startInitial robot state to compute offset for.
[in]position_offsetPosition offset to apply from current tip position.
[in]rotation_offsetRotational offset to apply from current tip position.

Definition at line 618 of file robowflex_library/src/robot.cpp.

621  : IKQuery(group, tip, start, TF::createPoseQ(position_offset, rotation_offset))
622 {
623 }
RobotPose createPoseQ(double x, double y, double z, double W, double X, double Y, double Z)
Creates a robot pose from a linear component and a Quaternion.
Definition: tf.cpp:47

◆ IKQuery() [4/11]

Robot::IKQuery::IKQuery ( const std::string group,
const std::string tip,
const robot_state::RobotState &  start,
const RobotPose offset 
)

Constructor. Initialize an IK query based on an offset from an initial robot state. Only for single-tip systems.

Parameters
[in]groupGroup to set.
[in]tipTip frame to apply offset to.
[in]startInitial robot state to compute offset for.
[in]offsetOffset to apply from current tip position.

Definition at line 625 of file robowflex_library/src/robot.cpp.

627  : IKQuery(group, start.getGlobalLinkTransform(tip) * offset)
628 {
629 }

◆ IKQuery() [5/11]

Robot::IKQuery::IKQuery ( const std::string group,
const RobotPose pose,
double  radius = constants::ik_tolerance,
const Eigen::Vector3d &  tolerance = constants::ik_vec_tolerance 
)

Constructor. Initialize a basic IK query to reach the desired pose.

Parameters
[in]groupGroup to set.
[in]poseDesired pose of end-effector.
[in]radiusRadius tolerance around position.
[in]tolerancesTolerance about orientation.

Definition at line 646 of file robowflex_library/src/robot.cpp.

648  : IKQuery(group, //
649  pose.translation(), //
650  Eigen::Quaterniond{pose.rotation()}, //
651  radius, //
652  tolerance)
653 {
654 }

◆ IKQuery() [6/11]

Robot::IKQuery::IKQuery ( const std::string group,
const Eigen::Vector3d &  position,
const Eigen::Quaterniond &  orientation,
double  radius = constants::ik_tolerance,
const Eigen::Vector3d &  tolerance = constants::ik_vec_tolerance 
)

Constructor. Initialize a basic IK query to reach the desired position and orientation.

Parameters
[in]groupGroup to set.
[in]positionPosition to achieve.
[in]orientationMean orientation.
[in]radiusRadius tolerance around position.
[in]tolerancesTolerance about orientation.

Definition at line 656 of file robowflex_library/src/robot.cpp.

659  : IKQuery(group, //
660  Geometry::makeSphere(radius), //
661  TF::createPoseXYZ(position), //
662  orientation, //
663  tolerance)
664 {
665 }
static GeometryPtr makeSphere(double radius)
Create a sphere.
Definition: geometry.cpp:43
RobotPose createPoseXYZ(double x, double y, double z)
Creates a robot pose from a linear component and zero orientation.
Definition: tf.cpp:14

◆ IKQuery() [7/11]

Robot::IKQuery::IKQuery ( const std::string group,
const GeometryConstPtr region,
const RobotPose pose,
const Eigen::Quaterniond &  orientation,
const Eigen::Vector3d &  tolerance = constants::ik_vec_tolerance,
const ScenePtr scene = nullptr,
bool  verbose = false 
)

Constructor. Initialize an IK query to reach somewhere in the provided region (at a pose) and orientation.

Parameters
[in]groupGroup to set.
[in]regionRegion of points for position.
[in]posePose of the region.
[in]orientationMean orientation.
[in]tolerancesTolerance about orientation.
[in]sceneOptional scene to do collision checking against.
[in]verboseIf true, will give verbose collision checking output.

Definition at line 667 of file robowflex_library/src/robot.cpp.

671 {
672  addRequest("", region, pose, orientation, tolerance);
673 }
Functions for loading and animating scenes in Blender.
void addRequest(const std::string &tip, const GeometryConstPtr &region, const RobotPose &pose, const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerance=constants::ik_vec_tolerance)
Add a request for a tip.
bool verbose
Verbose output of collision checking.
SceneConstPtr scene
If provided, use this scene for collision checking.

◆ IKQuery() [8/11]

Robot::IKQuery::IKQuery ( const std::string group,
const RobotPose offset,
const ScenePtr scene,
const std::string object,
const Eigen::Vector3d &  tolerances = constants::ik_vec_tolerance,
bool  verbose = false 
)

Constructor. Initialize a basic IK query to grasp an object.

Parameters
[in]groupGroup to set.
[in]offsetOffset of end-effector from object frame.
[in]sceneScene that object is in.
[in]objectName of object to grasp.
[in]tolerancesPosition tolerances on the XYZ axes for the grasp.
[in]verboseIf true, will give verbose collision checking output.

Definition at line 631 of file robowflex_library/src/robot.cpp.

638 {
639  const auto &pose = scene->getObjectGraspPose(object, offset);
640  addRequest("", //
642  TF::createPoseXYZ(pose.translation()), //
643  Eigen::Quaterniond(pose.rotation()));
644 }
static GeometryPtr makeBox(double x, double y, double z)
Create a box.
Definition: geometry.cpp:48
EigenSTL::vector_Vector3d tolerances
XYZ Euler orientation tolerances.

◆ IKQuery() [9/11]

Robot::IKQuery::IKQuery ( const std::string group,
const moveit_msgs::PositionConstraint &  pc,
const moveit_msgs::OrientationConstraint &  oc 
)

Constructor. Initialize an IK query from MoveIt message constraints.

Parameters
[in]groupGroup to set.
[in]pcPosition constraint.
[in]ocOrientation constraint.

Definition at line 675 of file robowflex_library/src/robot.cpp.

677  : group(group)
678 {
679  if (pc.link_name != oc.link_name)
680  throw Exception(
681  1, log::format("Link name mismatch in constraints, `%1%` != `%2%`", pc.link_name, oc.link_name));
682 
683  if (not TF::isVecZero(TF::vectorMsgToEigen(pc.target_point_offset)))
684  throw Exception(1, "target_point_offset in position constraint not supported.");
685 
686  const auto &cr = pc.constraint_region;
687 
688  if (not cr.meshes.empty())
689  throw Exception(1, "Cannot specify mesh regions!");
690 
691  if (cr.primitives.size() > 1)
692  throw Exception(1, "Cannot specify more than one primitive!");
693 
694  const auto &region = Geometry::makeSolidPrimitive(cr.primitives[0]);
695  const auto &pose = TF::poseMsgToEigen(cr.primitive_poses[0]);
696 
697  const auto &rotation = TF::quaternionMsgToEigen(oc.orientation);
698  Eigen::Vector3d tolerance{oc.absolute_x_axis_tolerance, //
699  oc.absolute_y_axis_tolerance, //
700  oc.absolute_z_axis_tolerance};
701 
702  addRequest(pc.link_name, region, pose, rotation, tolerance);
703 }
Exception that contains a message and an error code.
Definition: util.h:15
static GeometryPtr makeSolidPrimitive(const shape_msgs::SolidPrimitive &msg)
Create a solid primitive.
Definition: geometry.cpp:68
Eigen::Vector3d vectorMsgToEigen(const geometry_msgs::Vector3 &msg)
Converts a vector message to an Eigen::Vector3d.
Definition: tf.cpp:94
RobotPose poseMsgToEigen(const geometry_msgs::Pose &msg)
Converts a pose message to RobotPose.
Definition: tf.cpp:114
Eigen::Quaterniond quaternionMsgToEigen(const geometry_msgs::Quaternion &msg)
Converts a quaternion message to Eigen::Quaterniond.
Definition: tf.cpp:135
bool isVecZero(const Eigen::Ref< const Eigen::VectorXd > &v, double tolerance=constants::eps)
Checks if a vector is close to zero.
Definition: tf.cpp:321
std::string format(const std::string &fmt, Args &&... args)
Recursion base case, return string form of formatted arguments.
Definition: log.h:60

◆ IKQuery() [10/11]

Robot::IKQuery::IKQuery ( const std::string group,
const RobotPoseVector poses,
const std::vector< std::string > &  input_tips,
double  radius = constants::ik_tolerance,
const Eigen::Vector3d &  tolerance = constants::ik_vec_tolerance,
const ScenePtr scene = nullptr,
bool  verbose = false 
)

Constructor. Initialize a basic multi-target IK query so that each of the tips reach their desired poses.

Parameters
[in]groupGroup to set.
[in]posesDesired poses of end-effector tips.
[in]input_tipsEnd-effector tips to target.
[in]radiusRadius tolerance around position.
[in]tolerancesTolerance about orientation.
[in]sceneOptional scene to do collision checking against.
[in]verboseIf true, will give verbose collision checking output.

Definition at line 705 of file robowflex_library/src/robot.cpp.

709 {
710  if (poses.size() != input_tips.size())
711  throw Exception(1, "Invalid multi-target IK query. poses != tips.");
712 
713  for (std::size_t i = 0; i < poses.size(); ++i)
714  addRequest(input_tips[i], //
715  Geometry::makeSphere(radius), //
716  TF::createPoseXYZ(poses[i].translation()), //
717  Eigen::Quaterniond{poses[i].rotation()}, //
718  tolerance);
719 }
T size(T... args)

◆ IKQuery() [11/11]

Robot::IKQuery::IKQuery ( const std::string group,
const std::vector< std::string > &  input_tips,
const std::vector< GeometryConstPtr > &  regions,
const RobotPoseVector poses,
const std::vector< Eigen::Quaterniond > &  orientations,
const EigenSTL::vector_Vector3d &  tolerances,
const ScenePtr scene = nullptr,
bool  verbose = false 
)

Constructor. Initialize a multi-target IK query so that each of the tips reach somewhere in the provided region (at a pose) and orientation.

Parameters
[in]groupGroup to set.
[in]input_tipsEnd-effector tips to target.
[in]regionsRegion of points for position for each tip.
[in]posesPose of the region for each tip.
[in]orientationsMean orientation for each tip.
[in]tolerancesTolerance about orientation for each tip.
[in]sceneOptional scene to do collision checking against.
[in]verboseIf true, will give verbose collision checking output.

Definition at line 721 of file robowflex_library/src/robot.cpp.

726 {
727  if (poses.size() != input_tips.size() //
728  or poses.size() != regions.size() //
729  or poses.size() != orientations.size() //
730  or poses.size() != tolerances.size())
731  throw Exception(1, "Invalid multi-target IK query. Vectors are of different length.");
732 
733  for (std::size_t i = 0; i < poses.size(); ++i)
734  addRequest(input_tips[i], regions[i], poses[i], orientations[i], tolerances[i]);
735 }
std::vector< Eigen::Quaterniond > orientations
Target orientations.
std::vector< GeometryConstPtr > regions
Regions to sample target positions from.

Member Function Documentation

◆ addCenteringMetric()

void Robot::IKQuery::addCenteringMetric ( double  weight = 1.)

Add a metric to the query to evaluate how "centered" the joints of the robot are (from their 0 position).

Parameters
[in]weightMultiplicative weight to metric value.

Definition at line 766 of file robowflex_library/src/robot.cpp.

767 {
768  addMetric([&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene,
770  const auto &jmg = state.getJointModelGroup(group);
771  const auto &min = state.getMinDistanceToPositionBounds(jmg);
772  double extent = min.second->getMaximumExtent() / 2.;
773  return weight * (extent - min.first) / extent;
774  });
775 }
A const shared pointer wrapper for robowflex::Scene.
T min(T... args)
void addMetric(const Metric &metric_function)
Add a metric to this IK query.

◆ addClearanceMetric()

void Robot::IKQuery::addClearanceMetric ( double  weight = 1.)

Add a metric to the query to evaluate clearance from provided scene.

Parameters
[in]weightMultiplicative weight to metric value.

Definition at line 777 of file robowflex_library/src/robot.cpp.

778 {
779  addMetric([&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene,
781  if (scene)
782  {
783  double v = scene->distanceToCollision(state);
784  return weight * v;
785  }
786 
787  return 0.;
788  });
789 }

◆ addDistanceMetric()

void Robot::IKQuery::addDistanceMetric ( double  weight = 1.)

Add a metric to the query to evaluate distance to constraint.

Parameters
[in]weightMultiplicative weight to metric value.

Definition at line 758 of file robowflex_library/src/robot.cpp.

759 {
760  addMetric([weight](const robot_state::RobotState &state, const SceneConstPtr &scene,
762  return weight * result.distance;
763  });
764 }

◆ addMetric()

void Robot::IKQuery::addMetric ( const Metric metric_function)

Add a metric to this IK query.

Parameters
[in]metric_functionMetric to add to query.

Definition at line 753 of file robowflex_library/src/robot.cpp.

754 {
755  metrics.emplace_back(metric_function);
756 }

◆ addRequest()

void Robot::IKQuery::addRequest ( const std::string tip,
const GeometryConstPtr region,
const RobotPose pose,
const Eigen::Quaterniond &  orientation,
const Eigen::Vector3d &  tolerance = constants::ik_vec_tolerance 
)

Add a request for a tip.

Parameters
[in]tipTip for the request.
[in]regionRegion of points for position.
[in]posePose of the region.
[in]orientationMean orientation.
[in]tolerancesTolerance about orientation.

Definition at line 737 of file robowflex_library/src/robot.cpp.

739 {
740  tips.emplace_back(tip);
741  regions.emplace_back(region);
743  orientations.emplace_back(orientation);
744  tolerances.emplace_back(tolerance);
745 }
T emplace_back(T... args)
std::vector< std::string > tips
List of end-effectors.

◆ getAsConstraints()

kinematic_constraints::KinematicConstraintSetPtr Robot::IKQuery::getAsConstraints ( const Robot robot) const

Get this IK query as a kinematic constraint set.

Parameters
[in]robotRobot this IK query is for.
Returns
The IK query as a set of kinematic constraints.

Definition at line 834 of file robowflex_library/src/robot.cpp.

835 {
836  moveit_msgs::Constraints msg;
837  const auto &root = robot.getModelConst()->getRootLink()->getName();
838  getMessage(root, msg);
839 
840  auto constraints = std::make_shared<kinematic_constraints::KinematicConstraintSet>(robot.getModelConst());
841  moveit::core::Transforms none(root);
842 
843  constraints->add(msg, none);
844 
845  return constraints;
846 }
Functions for loading and animating robots in Blender.
void getMessage(const std::string &base_frame, moveit_msgs::Constraints &msg) const
Get this IK query as a constraint message.

◆ getMessage()

void Robot::IKQuery::getMessage ( const std::string base_frame,
moveit_msgs::Constraints &  msg 
) const

Get this IK query as a constraint message.

Parameters
[in]base_frameBase frame of the IK solver used. Can be obtained with Robot::getSolverBaseFrame().
[out]msgMessage to fill.

Definition at line 816 of file robowflex_library/src/robot.cpp.

817 {
818  const std::size_t n = regions.size();
819 
820  msg.name = "IKQuery";
821 
822  for (std::size_t i = 0; i < n; ++i)
823  {
824  auto pos = TF::getPositionConstraint(tips[i], base_frame, region_poses[i], regions[i]);
825  auto orn = TF::getOrientationConstraint(tips[i], base_frame, orientations[i], tolerances[i]);
826  pos.weight = 1. / n;
827  orn.weight = 1. / n / 2.;
828 
829  msg.position_constraints.emplace_back(pos);
830  msg.orientation_constraints.emplace_back(orn);
831  }
832 }
moveit_msgs::OrientationConstraint getOrientationConstraint(const std::string &ee_name, const std::string &base_name, const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances)
Get an orientation constraint message.
Definition: tf.cpp:189
moveit_msgs::PositionConstraint getPositionConstraint(const std::string &ee_name, const std::string &base_name, const RobotPose &pose, const GeometryConstPtr &geometry)
Get a position constraint message.
Definition: tf.cpp:169

◆ getMetricValue()

double Robot::IKQuery::getMetricValue ( const robot_state::RobotState &  state,
const kinematic_constraints::ConstraintEvaluationResult result 
) const

Get the value of the metrics assigned to this query for a given state and result.

Parameters
[in]stateThe state to evaluate.
[in]resultThe result of evaluating this state against the kinematic constraint set.
Returns
The value of the metric functions, summed.

Definition at line 848 of file robowflex_library/src/robot.cpp.

850 {
851  double v = 0.;
852  for (const auto &metric : metrics)
853  v += metric(state, scene, result);
854 
855  return v;
856 }

◆ sampleRegion()

bool Robot::IKQuery::sampleRegion ( RobotPose pose,
std::size_t  index 
) const

Sample desired end-effector pose for region at index.

Parameters
[out]poseThe sampled pose.
[in]indexThe index of the region to sample.
Returns
True on success, false on failure.

Definition at line 791 of file robowflex_library/src/robot.cpp.

792 {
793  const auto &point = regions[index]->sample();
794  if (point.first)
795  {
796  pose = region_poses[index];
797  pose.translate(point.second);
798  pose.rotate(TF::sampleOrientation(orientations[index], tolerances[index]));
799  }
800 
801  return point.first;
802 }
Eigen::Quaterniond sampleOrientation(const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances)
Sample an orientation from a given orientation with XYZ Euler angle tolerances.
Definition: tf.cpp:207

◆ sampleRegions()

bool Robot::IKQuery::sampleRegions ( RobotPoseVector poses) const

Sample desired end-effector pose for each region.

Parameters
[out]posesThe sampled poses.
Returns
True on success, false on failure.

Definition at line 804 of file robowflex_library/src/robot.cpp.

805 {
806  const std::size_t n = regions.size();
807  poses.resize(n);
808 
809  bool sampled = true;
810  for (std::size_t j = 0; j < n and sampled; ++j)
811  sampled &= sampleRegion(poses[j], j);
812 
813  return sampled;
814 }
T resize(T... args)
bool sampleRegion(RobotPose &pose, std::size_t index) const
Sample desired end-effector pose for region at index.

◆ setScene()

void Robot::IKQuery::setScene ( const ScenePtr scene,
bool  verbose = false 
)

Set a scene to use for collision checking for this IK request.

Parameters
[in]sceneScene to check collision against.
[in]verboseIf true, will output verbose collision checking output.

Definition at line 747 of file robowflex_library/src/robot.cpp.

748 {
749  scene = scene_in;
750  verbose = verbose_in;
751 }

Member Data Documentation

◆ attempts

std::size_t robowflex::Robot::IKQuery::attempts {constants::ik_attempts}

IK attempts (samples within regions).

Definition at line 407 of file robowflex_library/include/robowflex_library/robot.h.

◆ group

std::string robowflex::Robot::IKQuery::group

Target joint group to do IK for.

Definition at line 391 of file robowflex_library/include/robowflex_library/robot.h.

◆ metrics

std::vector<Metric> robowflex::Robot::IKQuery::metrics

Metrics used to evaluate configurations. If metrics are added, solver will use all allotted resources to search for the best configuration. If there are multiple metrics, metric values will be added together.

Definition at line 415 of file robowflex_library/include/robowflex_library/robot.h.

◆ options

kinematics::KinematicsQueryOptions robowflex::Robot::IKQuery::options

Other query options.

Definition at line 406 of file robowflex_library/include/robowflex_library/robot.h.

◆ orientations

std::vector<Eigen::Quaterniond> robowflex::Robot::IKQuery::orientations

Target orientations.

Definition at line 395 of file robowflex_library/include/robowflex_library/robot.h.

◆ random_restart

bool robowflex::Robot::IKQuery::random_restart {true}

Randomly reset joint states.

Definition at line 405 of file robowflex_library/include/robowflex_library/robot.h.

◆ region_poses

RobotPoseVector robowflex::Robot::IKQuery::region_poses

Poses of regions.

Definition at line 394 of file robowflex_library/include/robowflex_library/robot.h.

◆ regions

std::vector<GeometryConstPtr> robowflex::Robot::IKQuery::regions

Regions to sample target positions from.

Definition at line 393 of file robowflex_library/include/robowflex_library/robot.h.

◆ ROBOWFLEX_EIGEN

robowflex::Robot::IKQuery::ROBOWFLEX_EIGEN

◆ scene

SceneConstPtr robowflex::Robot::IKQuery::scene

If provided, use this scene for collision checking.

Definition at line 403 of file robowflex_library/include/robowflex_library/robot.h.

◆ timeout

double robowflex::Robot::IKQuery::timeout {0.}

Timeout for each query.

Definition at line 408 of file robowflex_library/include/robowflex_library/robot.h.

◆ tips

std::vector<std::string> robowflex::Robot::IKQuery::tips

List of end-effectors.

Definition at line 392 of file robowflex_library/include/robowflex_library/robot.h.

◆ tolerances

EigenSTL::vector_Vector3d robowflex::Robot::IKQuery::tolerances

XYZ Euler orientation tolerances.

Definition at line 396 of file robowflex_library/include/robowflex_library/robot.h.

◆ valid_distance

double robowflex::Robot::IKQuery::valid_distance {0.}

If positive and validate = true, will return success if result is within distance of kinematic constraint set.

Definition at line 412 of file robowflex_library/include/robowflex_library/robot.h.

◆ validate

bool robowflex::Robot::IKQuery::validate {false}

If true, double check if result is valid and use this rather than validity reported by the solver.

Definition at line 410 of file robowflex_library/include/robowflex_library/robot.h.

◆ verbose

bool robowflex::Robot::IKQuery::verbose {false}

Verbose output of collision checking.

Definition at line 404 of file robowflex_library/include/robowflex_library/robot.h.


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