Add base velocity as input

ami-iit · Jan 4, 2024 · 80c7757 · 80c7757
1 parent 3c2de13
commit 80c7757
Show file tree

Hide file tree

Showing 5 changed files with 14 additions and 0 deletions.
diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py
@@ -415,6 +415,7 @@ def CoM_position(
     def forward_dynamics(
         self,
         base_transform: Union[cs.SX, cs.DM],
+        base_velocity: Union[cs.SX, cs.DM],
         joint_positions: Union[cs.SX, cs.DM],
         joint_velocities: Union[cs.SX, cs.DM],
         joint_torques: Union[cs.SX, cs.DM],
@@ -423,6 +424,7 @@ def forward_dynamics(
 
         Args:
             base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame
+            base_velocity (Union[cs.SX, cs.DM]): The base velocity in mixed representation
             joint_positions (Union[cs.SX, cs.DM]): The joints position
             joint_velocities (Union[cs.SX, cs.DM]): The joints velocity
             joint_torques (Union[cs.SX, cs.DM]): The joints torque
@@ -433,6 +435,7 @@ def forward_dynamics(
         """
         return self.rbdalgos.aba(
             base_transform,
+            base_velocity,
             joint_positions,
             joint_velocities,
             joint_torques,

diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py
@@ -460,6 +460,7 @@ def rnea(
     def aba(
         self,
         base_transform: npt.ArrayLike,
+        base_velocity: npt.ArrayLike,
         joint_positions: npt.ArrayLike,
         joint_velocities: npt.ArrayLike,
         tau: npt.ArrayLike,
@@ -469,6 +470,7 @@ def aba(
 
         Args:
             base_transform (T): The homogenous transform from base to world frame
+            base_velocity (T): The base velocity in mixed representation
             joint_positions (T): The joints position
             joint_velocities (T): The joints velocity
             tau (T): The generalized force variables

diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py
@@ -243,6 +243,7 @@ def get_total_mass(self) -> float:
     def forward_dynamics(
         self,
         base_transform: jnp.array,
+        base_velocity: jnp.array,
         joint_positions: jnp.array,
         joint_velocities: jnp.array,
         joint_torques: jnp.array,
@@ -251,6 +252,7 @@ def forward_dynamics(
 
         Args:
             base_transform (jnp.array): The homogenous transform from base to world frame
+            base_velocity (jnp.array): The base velocity in mixed representation
             joint_positions (jnp.array): The joints position
             joint_velocities (jnp.array): The joints velocity
             joint_torques (jnp.array): The joints torques
@@ -261,6 +263,7 @@ def forward_dynamics(
         """
         base_acceleration, joint_accelerations = self.rbdalgos.aba(
             base_transform,
+            base_velocity,
             joint_positions,
             joint_velocities,
             joint_torques,

diff --git a/src/adam/numpy/computations.py b/src/adam/numpy/computations.py
@@ -248,6 +248,7 @@ def get_total_mass(self) -> float:
     def forward_dynamics(
         self,
         base_transform: np.ndarray,
+        base_velocity: np.ndarray,
         joint_positions: np.ndarray,
         joint_velocities: np.ndarray,
         joint_torques: np.ndarray,
@@ -256,6 +257,7 @@ def forward_dynamics(
 
         Args:
             base_transform (np.ndarray): The homogenous transform from base to world frame
+            base_velocity (np.ndarray): The base velocity in mixed representation
             joint_positions (np.ndarray): The joints position
             joint_velocities (np.ndarray): The joint velocities
             joint_torques (np.ndarray): The joint torques
@@ -266,6 +268,7 @@ def forward_dynamics(
         """
         return self.rbdalgos.aba(
             base_transform,
+            base_velocity,
             joint_positions,
             joint_velocities,
             joint_torques,

diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py
@@ -254,6 +254,7 @@ def get_total_mass(self) -> float:
     def forward_dynamic(
         self,
         base_transform: torch.Tensor,
+        base_velocity: torch.Tensor,
         joint_positions: torch.Tensor,
         joint_velocities: torch.Tensor,
         joint_torques: torch.Tensor,
@@ -262,6 +263,7 @@ def forward_dynamic(
 
         Args:
             base_transform (torch.tensor): The homogenous transform from base to world frame
+            base_velocity (torch.tensor): The base velocity in mixed representation
             joint_positions (torch.tensor): The joints positions
             joint_velocities (torch.tensor): The joints velocities
             joint_torques (torch.tensor): The joints torques
@@ -272,6 +274,7 @@ def forward_dynamic(
         """
         return self.rbdalgos.aba(
             base_transform,
+            base_velocity,
             joint_positions,
             joint_velocities,
             joint_torques,