Is it possible to plan_pose() or plan_grasp() with some target frame pose while adding a constraint or cost term on another link's pose(origin x and z axis)?

[xibeisiber]

Is it possible to plan_pose() or plan_grasp() with some target frame pose while adding a constraint or cost term on another link's pose(origin x and z axis)?

An example case is: planning torso+arm joints together, the arm end-effector should be guaranteed to reach the target, and I hope the torso won't lean forward or backward too much by adding a cost on the torso end effector's origin x (close to 0) and z axis(close to (0,0,1)).

But it seems it's not possible to let the optimizer be aware of this cost:
"cuRobo's ToolPoseCost (in both cost and constraint managers) is evaluated via Python compute_costs() which feeds into metric reporting — not the optimization gradient. The actual optimization gradient comes from compiled Warp kernels that the Python config API cannot extend."

Is this true?

[RenSolvyn]

No, that quote is not correct, on both halves. ToolPoseCost does feed the optimizer, and per-link per-axis weighting is exactly what the public config API is for. Your example is close to a supported use case.

On the gradient claim. ToolPoseCost is not metrics-only. Looking at curobo/_src/cost/cost_tool_pose.py, it allocates _out_position_gradient and _out_rotation_gradient, passes use_grad_input into the Warp kernel, and the kernel in curobo/_src/cost/wp_tool_pose.py writes those gradient buffers per link. If you would rather execute that path than trust a reading, benchmark/cost_gradient_benchmark.py in the repo is a standalone CLI that builds a ToolPoseCost over the robot's tool frames and benchmarks its gradients directly. Whatever produced that quote appears to have confused the convergence and metrics reporting path with the cost path.

On extending via the Python config API. You do not need to extend a kernel. The kernel already accepts a per-link, per-axis weight vector: wp_tool_pose.py indexes terminal_pose_axes_weight_factor[link_idx * 6 + 0..5], so the weighting is [num_links, 6]. The public surface for it is ToolPoseCriteria:

from curobo.types import ToolPoseCriteria

Its terminal_pose_axes_weight_factor is documented in curobo/_src/cost/tool_pose_criteria.py as scaling each axis in the order (x, y, z, roll, pitch, yaw), and __post_init__ enforces exactly 6 entries. There is a matching non_terminal_pose_axes_weight_factor for the non-terminal timesteps.

Mapping your torso case. Multi-link goals with different criteria per link are already used in-repo. curobo/motion_retargeter.py does exactly your shape of problem on a Unitree G1, with separate criteria for pelvis, left_hand and right_hand:

tool_pose_criteria={
    "pelvis": ToolPoseCriteria.track_position_and_orientation(
        xyz=[1.0, 1.0, 1.0], rpy=[0.5, 0.5, 0.5],
    ),
    "left_hand": ToolPoseCriteria.track_position_and_orientation(
        xyz=[1.0, 1.0, 1.0], rpy=[0.3, 0.3, 0.3],
    ),
}

For "arm reaches the target, torso stays upright", you want full weight on the arm frame and a weight vector on the torso frame that is nonzero only on the axes you care about. Your stated axes are torso origin x, and torso z axis pointing up, which is roll and pitch (yaw left free):

tool_pose_criteria={
    "arm_ee_link":  ToolPoseCriteria.track_position_and_orientation(
        xyz=[1.0, 1.0, 1.0], rpy=[1.0, 1.0, 1.0],
    ),
    "torso_link":   ToolPoseCriteria.track_position_and_orientation(
        xyz=[1.0, 0.0, 0.0],   # x only
        rpy=[1.0, 1.0, 0.0],   # roll and pitch only, yaw free
    ),
}

track_position_and_orientation is a convenience constructor that expands xyz/rpy into the 6-vector and scales the non-terminal vector by non_terminal_scale (default 0.1). If you want different non-terminal behaviour, construct ToolPoseCriteria directly with both 6-vectors. If you need to change criteria after building the solver, solver_ik.py exposes update_tool_pose_criteria(Dict[str, ToolPoseCriteria]).

Two things to be aware of, because they change how you set this up:

1. This weights error against a goal pose you supply for the torso frame, it does not constrain in absolute terms. So pass a nominal upright torso pose as that link's goal and zero the axes you do not care about, rather than expecting "keep x near 0" to be inferred.
2. It is a soft cost, not a hard constraint. If your target is only reachable with the torso leaning, the optimizer will trade off. Raise the torso weight, or tighten terminal_pose_convergence_tolerance, if you need the torso term to dominate.

If you are on a pre-restructure release (v0.7.x). The same capability exists under older names: pass the extra link goal through plan_single(..., link_poses=...), and shape per-axis behaviour with PoseCostMetric(hold_partial_pose=True, hold_vec_weight=...) applied via MotionGen.update_pose_cost_metric. One trap when moving between the two eras: the old hold_vec_weight is rotation-first, [rx, ry, rz, x, y, z]. You can see that in the in-repo create_grasp_approach_metric (in src/curobo/rollout/cost/pose_cost.py), which constrains an approach by zeroing hold_vec_weight[3 + linear_axis], i.e. a linear axis lives at index 3 to 5. The new ToolPoseCriteria order is position-first. Same idea, opposite ordering.

Caveat on my end: I verified the above by reading current main plus the v0.7.6 tag, not by running them, so treat the file paths as accurate for those two points and re-check names if you are pinned elsewhere in between.