Error planning to goalset with constraints -- QuatToMatrix assumes 1 batch dim

[peter-mitrano-bg]

1. cuRobo installation mode: isaac sim
2. python version: 3.10.13
3. Isaac Sim version (if using): 2023.1.0

---

I tested added a pose cost metric to goalset planning, but it fails because the QuatToMatrix function assumes only one batch dimension. To reproduce, add these lines and run demo_motion_gen_goalset.

  from curobo.rollout.cost.pose_cost import PoseCostMetric
  m_config.pose_cost_metric = PoseCostMetric(hold_partial_pose=True,
                                             hold_vec_weight=tensor_args.to_device([1, 1, 0, 0, 0, 0]))

Stack trace:

Traceback (most recent call last):
  File "/home/berkshiregrey.com/peter.mitrano/Documents/p2/curobo/examples/motion_gen_example.py", line 438, in <module>
    demo_motion_gen_goalset()
  File "/home/berkshiregrey.com/peter.mitrano/Documents/p2/curobo/examples/motion_gen_example.py", line 321, in demo_motion_gen_goalset
    result = motion_gen.plan_goalset(start_state, goal_pose, m_config)
  File "/home/berkshiregrey.com/peter.mitrano/Documents/p2/curobo/src/curobo/wrap/reacher/motion_gen.py", line 1423, in plan_goalset
    result = self._plan_attempts(
  File "/home/berkshiregrey.com/peter.mitrano/Documents/p2/curobo/src/curobo/wrap/reacher/motion_gen.py", line 1163, in _plan_attempts
    valid_query = self.update_pose_cost_metric(
  File "/home/berkshiregrey.com/peter.mitrano/Documents/p2/curobo/src/curobo/wrap/reacher/motion_gen.py", line 2617, in update_pose_cost_metric
    projected_pose = goal_pose.compute_local_pose(start_pose)
  File "/home/berkshiregrey.com/peter.mitrano/Documents/p2/curobo/src/curobo/types/math.py", line 452, in compute_local_pose
    return self.inverse().multiply(world_pose)
  File "/home/berkshiregrey.com/peter.mitrano/.local/share/ov/pkg/isaac_sim-2023.1.0-hotfix.1/kit/python/lib/python3.10/contextlib.py", line 79, in inner
    return func(*args, **kwds)
  File "/home/berkshiregrey.com/peter.mitrano/Documents/p2/curobo/src/curobo/types/math.py", line 385, in multiply
    mat_mul = self.get_matrix() @ other_pose.get_matrix()
  File "/home/berkshiregrey.com/peter.mitrano/.local/share/ov/pkg/isaac_sim-2023.1.0-hotfix.1/kit/python/lib/python3.10/contextlib.py", line 79, in inner
    return func(*args, **kwds)
  File "/home/berkshiregrey.com/peter.mitrano/Documents/p2/curobo/src/curobo/types/math.py", line 287, in get_matrix
    full_mat = pose_to_matrix(self.position, self.quaternion, out_matrix)
  File "/home/berkshiregrey.com/peter.mitrano/Documents/p2/curobo/src/curobo/geom/transform.py", line 166, in pose_to_matrix
    out_matrix[..., :3, :3] = quaternion_to_matrix(quaternion)
  File "/home/berkshiregrey.com/peter.mitrano/Documents/p2/curobo/src/curobo/geom/transform.py", line 113, in quaternion_to_matrix
    out_mat = QuatToMatrix.apply(quaternions, out_mat, adj_quaternion)
  File "/home/berkshiregrey.com/peter.mitrano/.local/share/ov/pkg/isaac_sim-2023.1.0-hotfix.1/extscache/omni.pip.torch-2_0_1-2.0.2+105.1.lx64/torch-2-0-1/torch/autograd/function.py", line 506, in apply
    return super().apply(*args, **kwargs)  # type: ignore[misc]
  File "/home/berkshiregrey.com/peter.mitrano/Documents/p2/curobo/src/curobo/geom/transform.py", line 1058, in forward
    b, _ = quaternion.shape
ValueError: too many values to unpack (expected 2)
There was an error running python

Conceptually, I would expect this type of planning query to be valid. I would like to maintain the current pose (in certain dimensions) while using a goalset to expand the possible solutions instead of just using one goal pose. As long as the hold_vec_weight and the goalset are comptaible. Perhaps this could be addressed by checking the batch shape in the update_pose_cost_metric function?

            if self.project_pose_to_goal_frame:
                if len(goal_pose.shape) > 2:
                    log_info("Skipping error checking on pose cost metric because a goalset is being used")
                else:
                    # project start pose to goal frame:
                    projected_pose = goal_pose.compute_local_pose(start_pose)
                    ...

With this change, it seems to plan as expected! But of course this means no helpful error message. A better test perhaps would be to check if any of the projected goal poses are valid, and if none of them are, then print the warnings?

[peter-mitrano-bg]

As a side note, it could be nice to make the Pose type re-shapeable. I'm not sure if it is already, but I didn't see how one would do that. Having this would make implementing the more sophisticated error checking easier I think.