Closed jordanott closed 7 months ago
Move this discussion here from DiffHand Repo #7.
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")
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!
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));
| ~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
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);
}
These changes fixed this issue and I'm now able to access the correct transform, thanks for your help!
Glad to hear! Thanks for providing the script to locate the error. Will close the issue now.
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 caseQuestions
get_joint_q(..., world_frame=True)
,get_body_E0i
. It's not clear which one to useget_joint_q
could be relative to the start location but after applying that it still didn't look rightThis example uses an object from this dataset