Bias Which Joints Move with Per-Joint Costs

Required Version

This feature will not be released until MoveIt Pro version 10.1.

On many robots, not all joints are equally cheap to move. A machine with hydraulic base joints and electric wrist joints wears, spends energy, and moves slowly on the hydraulic side, so a plan that reaches the goal mostly with the electric joints is preferable even when a raw shortest path would swing the hydraulic ones. Per-joint costs let you express this preference: each joint gets a cost multiplier, and the planners and IK solvers then prefer motion on the cheaper joints.

The joint_costs port

Behaviors that plan or rank joint motion accept an optional joint_costs input port:

• PlanToJointGoal — biases the planner search toward paths that move expensive joints less.
• SetupMTCPlanToPose, SetupMTCPlanToRobotState, and SetupMTCConnectWithProRRT — bias the planner search, rank the resulting MTC solutions by cost-weighted path length, and (for pose goals) rank IK candidates with the same costs.
• SetupMTCBatchPoseIK — ranks the IK candidates for each target pose so candidates that move expensive joints less are preferred.
• SetupMTCCartesianSequence — ranks the per-waypoint IK candidates; the Cartesian paths between waypoints are not affected.
• ComputeInverseKinematics — with solve_mode set to optimize_distance, ranks IK solutions by cost-weighted distance. Setting joint_costs with solve_mode: first_found is an error: switch solve_mode to optimize_distance to use costs.

Format

Provide one cost per active joint of the planning group (excluding fixed and mimic joints), in the group's active-joint order, as a semicolon-separated list:

joint_costs: "8;8;8;1;1;1"

This example makes the first three joints of a six-joint group eight times as expensive to move as the last three. The rules:

• Every cost must be at least 1.0, the neutral baseline. A cost of 5.0 reads as "five times as expensive as normal". A sub-baseline or non-finite cost fails the Behavior with an error naming the offending joint.
• The vector length must match the number of active joints in the planning group; the Behavior fails with an error stating the expected count otherwise.
• Leave the port empty (the default) for unbiased planning — existing Objectives behave exactly as before.

What the costs affect

With costs set, every layer of the pipeline weighs joints the same way (the planner search uses a cost-weighted Euclidean segment metric; solution and IK-candidate ranking use a cost-weighted per-joint sum):

• The planner search spends its motion on cheap joints when a detour is needed, instead of distributing it evenly.
• When several candidate plans reach the goal, the cost-weighted plan wins the arbitration even if a raw-shorter plan exists that moves expensive joints more.
• IK candidate ranking prefers solutions that move expensive joints less.

Step size, collision-check resolution, and joint limits are never affected by costs — a biased plan is validated exactly as strictly as an unbiased one.

Inverse kinematics drives most of the effect

For pose goals, most of the biasing comes from IK-candidate ranking: the selected IK solution is what decides which joints move and where they end up. The planner then computes a path to that joint-space goal — joint costs still help there, favoring routes that spare the expensive joints, but their effect on the final motion is smaller.

Give the planner alternatives to choose from

The cost-weighted arbitration picks the best of the candidate plans the search finds. With the default single planning attempt there is often only one candidate, so set planning_seed_attempts (in the optimization_params port of the ProRRT Behaviors) to a value greater than one when using costs — multiple attempts explore different routes, and the weighted ranking then selects the one that spares the expensive joints.