eanswer/DiffHand

Look up poses of objects

Closed this issue · 3 comments

jordanott commented

I would like to look up the pose of an object in the simulator. When I attempt this using get_joint_q it does not match up with external validation (Open3D). In the animations below I would expect the red mesh (right visual) to match up with the end location in the redmax replay but this is not the case

Redmax Open3D (Start location ✔️ End location ❌ )
render open3d_render

Questions

  1. Is this the correct function to use? I see there are other variants get_joint_q(..., world_frame=True), get_body_E0i. It's not clear which one to use
  2. Am I not using the result correctly? I was thinking the result from get_joint_q could be relative to the start location but after applying that it still didn't look right

This example uses an object from this dataset

import numpy as np
import open3d as o3d
import redmax_py as redmax
from scipy.spatial.transform import Rotation

def translation_euler_angles_to_matrix(translation, euler_angles):
    T = np.eye(4)
    T[:3, :3] = Rotation.from_euler("xyz", euler_angles).as_matrix()
    T[:3, -1] = translation

    return T

np.random.seed(12)
translation = np.array([0, 0, 4.0])

euler_angles = np.random.rand(3)
quaternion = Rotation.from_euler("xyz", euler_angles).as_quat()
# Convert from XYZW -> WXYZ
quaternion = np.roll(quaternion, 1)

translation_string = " ".join([str(x) for x in translation])
quaternion_string = " ".join([str(x) for x in quaternion])

xml_string = f"""
<redmax model="assemble">
<option integrator="BDF1" timestep="1e-3" gravity="0. 0. -9.8"/>
<ground pos="0 0 0" normal="0 0 1"/>
<default>
    <ground_contact kn="1e6" kt="1e3" mu="0.8" damping="5e1"/>
    <general_SDF_contact kn="1e5" kt="1e3" mu="0.0" damping="3e1" />
</default>
<robot>
    <link name="part">
        <joint name="part" type="free3d-euler" axis="0. 0. 0." pos="{translation_string}" quat="{quaternion_string}" frame="WORLD" damping="0"/>
        <body name="part" type="SDF" filename="assets/joint_assembly/00000/0.obj" pos="0 0 0" quat="1 0 0 0" scale="1 1 1" transform_type="OBJ_TO_JOINT" density="1" dx="0.1" mu="0"/>
    </link>
</robot>
<contact>
    <ground_contact body="part"/>
</contact>
</redmax>
"""

simulation = redmax.Simulation(xml_string, "")
simulation.reset()
simulation.forward(5000)

# Read translation and euler angles from sim
translation_euler_angles = simulation.get_joint_q("part")
simulation.replay()

# Transform the mesh to the requested start position
start_location_mesh = o3d.io.read_triangle_mesh(object_file)
start_pose = translation_euler_angles_to_matrix(translation, euler_angles)
start_location_mesh.transform(start_pose)

# Transform the mesh to the expected end position
end_location_mesh = o3d.io.read_triangle_mesh(object_file)
end_pose = translation_euler_angles_to_matrix(translation_euler_angles[:3], translation_euler_angles[3:])
end_location_mesh.transform(end_pose)

o3d.visualization.draw_geometries(
    [
        # End mesh is red, start mesh is green
        end_location_mesh.paint_uniform_color((1, 0, 0)),
        start_location_mesh.paint_uniform_color((0, 1, 0)),
        # Ground plane and coordinate frame for reference
        o3d.geometry.TriangleMesh.create_coordinate_frame(),
        o3d.geometry.TriangleMesh.create_box(width=10, height=10, depth=0.0001)
    ]
)
eanswer commented

Hi, get_joint_q and get_body_E0i functions are not implemented in this original repo and it is indeed introduced from the modified one in AssembleThemAll. So I would ask in that repo to not confuse users in this repo.

Here are my thoughts on what you encountered. First, the reduced joint coordinates are represented in joint local frame instead of world frame, which means the returned euler angles are relative to the rotated joint frame. So get_joint_q(..., world_frame=True) could give you the correct result in your case. Second, E_0i should be the correct transformation to use, since it is directly the transformation matrix from body frame (i) to world frame (0).

jordanott commented

Okay, thanks for the insight. I've tried 3 different ways, none seem to work

# Option 1
translation_euler_angles = simulation.get_joint_q("part", world_frame=False)
end_location_mesh.transform(start_pose @ end_pose)

# Option 2
translation_euler_angles = simulation.get_joint_q("part", world_frame=True)

# Option 3
end_pose = simulation.get_body_E0i("part")
eanswer commented

I close the issue here and let's move the discussion to AssembleThemAll repo #12.

  • 👍1