pose_ik Namespace
Definition
Classes Index
| struct | PoseTarget |
Typedefs Index
| using | IKValidationFunction = std::function< bool(const Eigen::VectorXd &solution)> |
Functions Index
| bool | emptyValidationFunction (const Eigen::VectorXd &) |
| tl::expected< Eigen::VectorXd, std::string > | solveIK (moveit_pro::base::RobotState &robot_state, const moveit_pro::base::JointModelGroup &group, const std::vector< PoseTarget > &targets, const Eigen::VectorXd &seed, const rclcpp::Duration &timeout, const IKValidationFunction &validation_fn, const Params ¶ms) |
|
Computes pose inverse kinematics for the given group and set of targets. More... | |
| tl::expected< void, std::string > | checkPreconditions (const moveit_pro::base::JointModelGroup &group, const std::vector< PoseTarget > &targets, const Eigen::VectorXd &seed) |
| bool | unwindAndCheckLimits (const moveit_pro::base::JointModelGroup &group, std::span< const moveit_pro::base::LinkModel *const > tip_links, Eigen::VectorXd &joint_values) |
| bool | unwindAndCheckLimits (const moveit_pro::base::JointModelGroup &group, Eigen::VectorXd &joint_values) |
Variables Index
| constexpr auto | kSolveModeFirstFound = "first_found" |
| constexpr auto | kSolveModeOptimizeDistance = "optimize_distance" |
| constexpr double | kMaxJointCost = 50.0 |
Typedefs
IKValidationFunction
|
Definition at line 57 of file pose_ik.hpp.
Functions
checkPreconditions()
|
Definition at line 472 of file pose_ik.cpp.
emptyValidationFunction()
| inline |
Definition at line 60 of file pose_ik.hpp.
solveIK()
|
Computes pose inverse kinematics for the given group and set of targets.
This implements a Jacobian-based Newton-Raphson IK solver. The random sequence used internally is deterministic: given the same inputs, the solver will always find the same solution.
Groups may contain mimic joints. The solver searches over the group's active joints only — seed and solution are laid out as RobotState::setJointGroupActivePositions() expects — and mimic joints follow their driving joint, both in the forward kinematics and in the Jacobian, so the returned solution places the tips correctly when the mimic joints are applied. A mimic joint's own position limits need no check here: models built through RobotModel::create or the robot model loader reject a mimic mapping that could exceed them (see RobotModel::validateMimicJointLimits).
- Parameters
-
robot_state A robot state, used to compute FK. Scratch: its final contents are unspecified (it holds whatever configuration gradient descent set last, which can differ from the returned solution by whole turns on continuous joints). Callers must use the returned vector, not this state.
group The joint model group to compute IK for.
targets The target poses to compute IK for.
seed A joint configuration to be used as a first seed, before starting taking random configurations.
timeout Search timeout in seconds.
validation_fn Applied to a candidate solution; returning false discards it and the search resumes. It is only ever invoked with a configuration that has already converged on targets, so a candidate it refuses is one that reached them and was rejected. Callers can rely on that to tell a target that was unreachable from one that was reachable and unacceptable — a distinction the returned error message does not carry. It follows that an unreachable target never invokes it at all.
params Other IK parameters like tolerance, etc. params.joint_costs optionally holds one cost multiplier per active joint of group, in JointModelGroup::getActiveJointModelNames() order. A joint's cost scales how expensive it is to move away from seed, so raising it biases the solver toward solutions that move that joint less. Every entry must be finite and at least 1.0, the unbiased baseline; an empty list means "no bias", and kMaxJointCost is the accepted maximum — see that constant for why. Prefer the smallest cost that produces the bias you want. Costs only take effect when params.solve_mode is optimize_distance — see the error note below.
- Returns
a joint-space solution that brings tip_link to root_pose_tip, or an error message if the preconditions are not met. Continuous revolute joints in the solution are aligned to seed's winding: each is shifted by whole 2*pi turns to land within half a turn of the seed value, so a solution stays consistent with the physical robot state the solve was seeded from even when it was found from a random restart. The one exception is a continuous joint that drives mimic joints, which is returned as solved: shifting the driver can change the pose of the links downstream of the mimic. Position bounds set on a continuous joint are dropped by the model, so it never enters the position-limit pass; a bounded revolute joint does, and is unwound into range only under the mimic rule documented on unwindAndCheckLimits below. On such groups a solution's winding can still come back a whole number of turns from the seed. Supplying non-empty params.joint_costs with params.solve_mode == "first_found" is an error rather than a no-op: that mode returns the first valid solution without ranking candidates, so the costs would otherwise be silently discarded. For the same reason, non-empty costs combined with a params.optimization_distance_gain that is not finite and strictly positive are also an error: zero flattens every candidate's cost, and a negative gain reverses the bias. That gain remains valid at zero when no costs are supplied, which is what the parameter's own validation allows.
Definition at line 530 of file pose_ik.cpp.
unwindAndCheckLimits()
|
Definition at line 453 of file pose_ik.cpp.
unwindAndCheckLimits()
|
Definition at line 467 of file pose_ik.cpp.
Variables
kMaxJointCost
| constexpr |
Definition at line 53 of file pose_ik.hpp.
kSolveModeFirstFound
| constexpr |
Definition at line 29 of file pose_ik.hpp.
kSolveModeOptimizeDistance
| constexpr |
Definition at line 30 of file pose_ik.hpp.
The documentation for this namespace was generated from the following files:
Generated via doxygen2docusaurus 2.2.2 by Doxygen 1.9.8.