Fix dimforge#416 - update ball joint motor pos

2023-12-26 22:47:33 -08:00
parent b96e61853b
commit 2ed34bacf4
2 changed files with 5 additions and 2 deletions
--- a/src/dynamics/joint/multibody_joint/multibody_joint.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs
@@ -115,6 +115,9 @@ impl MultibodyJoint {
                let angvel = Vector3::from_row_slice(&vels[curr_free_dof..curr_free_dof + 3]);
                let disp = UnitQuaternion::new_eps(angvel * dt, 0.0);
                self.joint_rot = disp * self.joint_rot;
+                self.coords[3] += angvel[0] * dt;
+                self.coords[4] += angvel[1] * dt;
+                self.coords[5] += angvel[2] * dt;
            }
            _ => unreachable!(),
        }