From c9556b40f4515c41d6753b6f246f6e2ee1fb052f Mon Sep 17 00:00:00 2001 From: Giaco02 <167908830+Giaco02@users.noreply.github.com> Date: Sun, 1 Sep 2024 18:58:42 +0200 Subject: [PATCH 1/2] Update rbd_algorithms.py I incorporated joint acceleration into the reduced RNEA --- src/adam/core/rbd_algorithms.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index ba68391f..0632870c 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -439,7 +439,7 @@ def rnea( 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] From 2d0d757568a04faec3c7436d1068bf8b97e29dfd Mon Sep 17 00:00:00 2001 From: Giaco02 <167908830+Giaco02@users.noreply.github.com> Date: Mon, 2 Sep 2024 15:41:19 +0200 Subject: [PATCH 2/2] Update rbd_algorithms.py added joint accelerations to rnea (i uploaded the wrong version in the first pr) --- src/adam/core/rbd_algorithms.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 0632870c..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,6 +436,9 @@ 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)