yunshengtian/Assemble-Them-All

Look up poses of objects

Closed this issue · 7 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])
object_file = "assets/joint_assembly/00000/0.obj"

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="{object_file}" 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

Move this discussion here from DiffHand Repo #7.

eanswer commented

For the three options you tried, what are the behaviors of the failure, do option 2 and option 3 produce the same open3D results?

# 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")
yunshengtian commented

Hi @jordanott , thanks for raising the question. I think this is a bug that I forgot to fix in this repo. Please try the following API get_joint_qm instead of get_joint_q or get_body_E0i:

Add this to simulation/redmax/Simulation.cpp (e.g., under the function get_joint_q):

VectorX Simulation::get_joint_qm(const string name) {
    Joint* joint = get_joint(name);
    if (joint->_ndof == 3) {
        return joint->_E_0j.topRightCorner(3, 1);
    } else if (joint->_ndof == 6) {
        Vector6 qm = Vector6::Zero();
        Matrix3 R2 = joint->_E_0j.topLeftCorner(3, 3);
        Vector3 p2 = joint->_E_0j.topRightCorner(3, 1);
        qm.head(3) = p2;
        qm.tail(3) = math::mat2euler(R2);
        return qm;
    } else {
        throw_error("get_joint_qm: joint ndof not supported: " + std::to_string(joint->_ndof));
    }
}

Add this to simulation/redmax/Simulation.h (e.g., under the function get_joint_q):

VectorX get_joint_qm(const string name);

Add this to simulation/redmax/python_interface.cpp (e.g., under the code for get_joint_q):

.def("get_joint_qm", &Simulation::get_joint_qm,
                "get qm of the joint",
                py::arg("name"))

After the above code snippets are added, please run python setup.py install again under the simulation folder. Then, just replace get_joint_q with get_joint_qm in your code and you should get the correct result.

There are some mistakes in converting from local coordinates to world coordinates (though doesn't affect the main algorithm much). I will check if I have fixed other stuff locally as well at some point later and let you know once I push the updated code. Stay tuned!

jordanott commented

Thanks @yunshengtian!

There's a couple build issues, maybe just missing an import?

/home/path/projects/Assemble-Them-All/simulation/redmax/Simulation.cpp: In member function ‘redmax::VectorX redmax::Simulation::get_joint_qm(std::string)’:
/home/path/projects/Assemble-Them-All/simulation/redmax/Simulation.cpp:176:28: error: ‘mat2euler’ is not a member of ‘redmax::math’
  176 |         qm.tail(3) = math::mat2euler(R2);
      |                            ^~~~~~~~~
...
[ 98%] Building CXX object redmax/CMakeFiles/redmax_py.dir/VirtualObject/VirtualObjectSphere.cpp.o
/home/path/projects/Assemble-Them-All/simulation/redmax/Simulation.cpp:179:20: warning: control reaches end of non-void function [-Wreturn-type]
  179 |         throw_error("get_joint_qm: joint ndof not supported: " + std::to_string(joint->_ndof));
      |         ~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
yunshengtian commented

You'll also need to add this to simulation/redmax/Util.h:

    inline Vector3 mat2euler(Matrix3 mat) {
        dtype r11 = mat(0, 0), r12 = mat(0, 1), r13 = mat(0, 2);
        dtype r21 = mat(1, 0), r22 = mat(1, 1), r23 = mat(1, 2);
        dtype r31 = mat(2, 0), r32 = mat(2, 1), r33 = mat(2, 2);

        dtype roll = std::atan2(r32, r33);
        dtype pitch = std::atan2(-r31, std::sqrt(r32 * r32 + r33 * r33));
        dtype yaw = std::atan2(r21, r11);

        return Vector3(roll, pitch, yaw);
    }
  • 👍1
jordanott commented

These changes fixed this issue and I'm now able to access the correct transform, thanks for your help!

yunshengtian commented

Glad to hear! Thanks for providing the script to locate the error. Will close the issue now.