Skip to content

Commit b5e56f0

Browse files
move pos/quat update higher because it also needs to happen for zero-dof joints
Signed-off-by: Alain Denzler <adenzler@nvidia.com>
1 parent fa59345 commit b5e56f0

File tree

1 file changed

+8
-7
lines changed

1 file changed

+8
-7
lines changed

newton/_src/solvers/mujoco/kernels.py

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1016,6 +1016,14 @@ def update_joint_transforms_kernel(
10161016

10171017
child_xform = joint_X_c[tid]
10181018
parent_xform = joint_X_p[tid]
1019+
1020+
# update body pos and quat from parent joint transform
1021+
child = joint_child[joint_in_world] # Newton body id
1022+
body_id = body_mapping[child] # MuJoCo body id
1023+
tf = parent_xform * wp.transform_inverse(child_xform)
1024+
body_pos[worldid, body_id] = tf.p
1025+
body_quat[worldid, body_id] = wp.quat(tf.q.w, tf.q.x, tf.q.y, tf.q.z)
1026+
10191027
lin_axis_count = joint_dof_dim[tid, 0]
10201028
ang_axis_count = joint_dof_dim[tid, 1]
10211029

@@ -1042,13 +1050,6 @@ def update_joint_transforms_kernel(
10421050
joint_axis[worldid, ai] = wp.quat_rotate(child_xform.q, axis)
10431051
joint_pos[worldid, ai] = child_xform.p
10441052

1045-
# update body pos and quat from parent joint transform
1046-
child = joint_child[joint_in_world] # Newton body id
1047-
body_id = body_mapping[child] # MuJoCo body id
1048-
tf = parent_xform * wp.transform_inverse(child_xform)
1049-
body_pos[worldid, body_id] = tf.p
1050-
body_quat[worldid, body_id] = wp.quat(tf.q.w, tf.q.x, tf.q.y, tf.q.z)
1051-
10521053

10531054
@wp.kernel(enable_backward=False)
10541055
def update_shape_mappings_kernel(

0 commit comments

Comments
 (0)