curobo.util.error_metrics module

rotation_error_quaternion(q_des, q)

rotation_error_matrix(r_des, r)

px = torch.tensor([1.0,0.0,0.0],device=r_des.device).T py = torch.tensor([0.0,1.0,0.0],device=r_des.device).T pz = torch.tensor([0.0,0.0,1.0],device=r_des.device).T print(px.shape, r.shape)

current_px = r * px current_py = r * py current_pz = r * pz

des_px = r_des * px des_py = r_des * py des_pz = r_des * pz

cost = torch.norm(current_px - des_px) + torch.norm(current_py - des_py) + torch.norm(current_pz - des_pz) return cost