curobo.types.math module

class curobo.types.math.Pose(position=None, quaternion=None, rotation=None, batch=1, n_goalset=1, name='ee_link', normalize_rotation=True)

Bases: Sequence

Pose representation used in CuRobo. You can initialize a pose by calling pose = Pose(position, quaternion).

Parameters:
  • position (Tensor | None) –

  • quaternion (Tensor | None) –

  • rotation (Tensor | None) –

  • batch (int) –

  • n_goalset (int) –

  • name (str) –

  • normalize_rotation (bool) –

position: Tensor | None = None

Position is represented as x, y, z, in meters

quaternion: Tensor | None = None

Quaternion is represented as w, x, y, z.

rotation: Tensor | None = None

Rotation is represents orientation as a 3x3 rotation matrix

batch: int = 1

Batch size will be initialized from input.

n_goalset: int = 1

Goalset will be initialized from input when position shape is batch x n_goalset x 3

name: str = 'ee_link'

name of link that this pose represents.

normalize_rotation: bool = True

quaternion input will be normalized when this flag is enabled. This is recommended when a pose comes from an external source as some programs do not send normalized quaternions.

static from_matrix(matrix)
Parameters:

matrix (Union[np.ndarray, torch.Tensor]) –

get_rotation()
stack(other_pose)
Parameters:

other_pose (Pose) –

repeat(n)

Repeat pose

Parameters:

n ([type]) – [description]

unsqueeze(dim=-1)
repeat_seeds(num_seeds)
Parameters:

num_seeds (int) –

get_index(b, n=None)
Parameters:
  • b (int) –

  • n (int | None) –

Return type:

Pose

apply_kernel(kernel_mat)
classmethod from_list(pose, tensor_args=TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32), q_xyzw=False)
Parameters:
classmethod from_batch_list(pose, tensor_args, q_xyzw=False)
Parameters:
to_list(q_xyzw=False)
tolist(q_xyzw=False)
clone()
to(tensor_args)
Parameters:

tensor_args (TensorDeviceType) –

get_matrix(out_matrix=None)
Parameters:

out_matrix (Tensor | None) –

get_numpy_matrix()
inverse()

Inverse of pose

Returns:

inverse pose

Return type:

Pose

get_pose_vector()
copy_(pose)

Copies pose data from another memory buffer. This will create a new instance if buffers are not same shape

Parameters:

pose (Pose) – _description_

static cat(pose_list)
Parameters:

pose_list (List[Pose]) –

distance(other_pose, use_phi3=False)
Parameters:
  • other_pose (Pose) –

  • use_phi3 (bool) –

angular_distance(other_pose, use_phi3=False)

This function computes the angular distance phi_3.

See Huynh, Du Q. “Metrics for 3D rotations: Comparison and analysis.” Journal of Mathematical Imaging and Vision 35 (2009): 155-164.

Parameters:
  • goal_quat – _description_

  • current_quat – _description_

  • other_pose (Pose) –

  • use_phi3 (bool) –

Returns:

Angular distance in range [0,1]

linear_distance(other_pose)
Parameters:

other_pose (Pose) –

multiply(other_pose)
Parameters:

other_pose (Pose) –

transform_point(points, out_buffer=None, gp_out=None, gq_out=None, gpt_out=None)
Parameters:
  • points (Tensor) –

  • out_buffer (Tensor | None) –

  • gp_out (Tensor | None) –

  • gq_out (Tensor | None) –

  • gpt_out (Tensor | None) –

transform_points(points, out_buffer=None, gp_out=None, gq_out=None, gpt_out=None)
Parameters:
  • points (Tensor) –

  • out_buffer (Tensor | None) –

  • gp_out (Tensor | None) –

  • gq_out (Tensor | None) –

  • gpt_out (Tensor | None) –

batch_transform_points(points, out_buffer=None, gp_out=None, gq_out=None, gpt_out=None)
Parameters:
  • points (Tensor) –

  • out_buffer (Tensor | None) –

  • gp_out (Tensor | None) –

  • gq_out (Tensor | None) –

  • gpt_out (Tensor | None) –

property shape
compute_offset_pose(offset)
Parameters:

offset (Pose) –

Return type:

Pose

compute_local_pose(world_pose)
Parameters:

world_pose (Pose) –

Return type:

Pose

_abc_impl = <_abc._abc_data object>
curobo.types.math.quat_multiply(q1, q2, q_res)
class curobo.types.math.OrientationError(*args, **kwargs)

Bases: Function

static geodesic_distance(goal_quat, current_quat, quat_res)
static forward(ctx, goal_quat, current_quat, quat_res)

This function is to be overridden by all subclasses. There are two ways to define forward:

Usage 1 (Combined forward and ctx):

@staticmethod
def forward(ctx: Any, *args: Any, **kwargs: Any) -> Any:
    pass
  • It must accept a context ctx as the first argument, followed by any number of arguments (tensors or other types).

  • See combining-forward-context for more details

Usage 2 (Separate forward and ctx):

@staticmethod
def forward(*args: Any, **kwargs: Any) -> Any:
    pass

@staticmethod
def setup_context(ctx: Any, inputs: Tuple[Any, ...], output: Any) -> None:
    pass
  • The forward no longer accepts a ctx argument.

  • Instead, you must also override the torch.autograd.Function.setup_context staticmethod to handle setting up the ctx object. output is the output of the forward, inputs are a Tuple of inputs to the forward.

  • See extending-autograd for more details

The context can be used to store arbitrary data that can be then retrieved during the backward pass. Tensors should not be stored directly on ctx (though this is not currently enforced for backward compatibility). Instead, tensors should be saved either with ctx.save_for_backward if they are intended to be used in backward (equivalently, vjp) or ctx.save_for_forward if they are intended to be used for in jvp.

static backward(ctx, grad_out)

Defines a formula for differentiating the operation with backward mode automatic differentiation (alias to the vjp function).

This function is to be overridden by all subclasses.

It must accept a context ctx as the first argument, followed by as many outputs as the forward returned (None will be passed in for non tensor outputs of the forward function), and it should return as many tensors, as there were inputs to forward. Each argument is the gradient w.r.t the given output, and each returned value should be the gradient w.r.t. the corresponding input. If an input is not a Tensor or is a Tensor not requiring grads, you can just pass None as a gradient for that input.

The context can be used to retrieve tensors saved during the forward pass. It also has an attribute ctx.needs_input_grad as a tuple of booleans representing whether each input needs gradient. E.g., backward will have ctx.needs_input_grad[0] = True if the first input to forward needs gradient computed w.r.t. the output.

_backward_cls

alias of OrientationErrorBackward