diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index ba68391f..243e9636 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -363,6 +363,7 @@ def rnea( joint_positions: npt.ArrayLike, base_velocity: npt.ArrayLike, joint_velocities: npt.ArrayLike, + joint_accelerations: npt.ArrayLike, g: npt.ArrayLike, ) -> npt.ArrayLike: """Implementation of reduced Recursive Newton-Euler algorithm @@ -374,6 +375,7 @@ def rnea( joint_positions (T): The joints position base_velocity (T): The base velocity in mixed representation joint_velocities (T): The joints velocity + joint_accelerations (T): The joints acceleration g (T): The 6D gravity acceleration Returns: @@ -434,12 +436,15 @@ def rnea( q_dot = ( joint_velocities[joint_i.idx] if joint_i.idx is not None else 0.0 ) + q_dot_dot = ( + joint_accelerations[joint_i.idx] if joint_i.idx is not None else 0.0 + ) X_p[i] = joint_i.spatial_transform(q) Phi[i] = joint_i.motion_subspace() pi = self.model.tree.get_idx_from_name(link_pi.name) # pi = self.tree.links.index(link_pi) v[i] = X_p[i] @ v[pi] + Phi[i] * q_dot - a[i] = X_p[i] @ a[pi] + self.math.spatial_skew(v[i]) @ Phi[i] * q_dot + a[i] = X_p[i] @ a[pi] + self.math.spatial_skew(v[i]) @ Phi[i] * q_dot + Phi[i]* q_dot_dot f[i] = Ic[i] @ a[i] + self.math.spatial_skew_star(v[i]) @ Ic[i] @ v[i]