Merge pull request #291 from robotology/feature/rbd_average_velocity

Expose average velocity methods from KinDynComputations
robotology-legacy · Feb 9, 2021 · 8ba6e71 · 8ba6e71
2 parents 33cb234 + 6d96cc9
commit 8ba6e71
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 5 deletions.
diff --git a/python/gym_ignition/rbd/idyntree/helpers.py b/python/gym_ignition/rbd/idyntree/helpers.py
@@ -61,7 +61,8 @@ def get_model_loader(model_file: str, considered_joints: List[str] = None):
     def get_kindyncomputations(
             model_file: str,
             considered_joints: List[str] = None,
-            velocity_representation: FrameVelocityRepresentation = None):
+            velocity_representation: FrameVelocityRepresentation =
+                FrameVelocityRepresentation.MIXED_REPRESENTATION):
 
         # Get the model loader
         model_loader = iDynTreeHelpers.get_model_loader(model_file, considered_joints)
@@ -73,9 +74,6 @@ def get_kindyncomputations(
         if not ok_load:
             raise RuntimeError("Failed to load model")
 
-        if velocity_representation is None:
-            velocity_representation = FrameVelocityRepresentation.MIXED_REPRESENTATION
-
         # Configure the velocity representation
         velocity_representation_idyntree = velocity_representation.to_idyntree()
         ok_repr = kindyn.setFrameVelocityRepresentation(velocity_representation_idyntree)

diff --git a/python/gym_ignition/rbd/idyntree/kindyncomputations.py b/python/gym_ignition/rbd/idyntree/kindyncomputations.py
@@ -17,7 +17,10 @@ def __init__(self,
                  model_file: str,
                  considered_joints: List[str] = None,
                  world_gravity: np.ndarray = np.array([0, 0, -9.806]),
-                 velocity_representation: FrameVelocityRepresentation = None) -> None:
+                 velocity_representation: FrameVelocityRepresentation =
+                    FrameVelocityRepresentation.MIXED_REPRESENTATION) -> None:
+
+        self.velocity_representation = velocity_representation
 
         self.kindyn = iDynTreeHelpers.get_kindyncomputations(model_file,
                                                              considered_joints,
@@ -351,6 +354,16 @@ def get_com_velocity(self) -> np.ndarray:
         else:
             raise RuntimeError("INERTIAL_FIXED_REPRESENTATION not yet supported")
 
+    def get_average_velocity(self) -> np.ndarray:
+
+        twist: idt.Twist = self.kindyn.getAverageVelocity()
+        return twist.toNumPy()
+
+    def get_centroidal_average_velocity(self) -> np.ndarray:
+
+        twist: idt.Twist = self.kindyn.getCentroidalAverageVelocity()
+        return twist.toNumPy()
+
     def get_frame_jacobian(self, frame_name: str) -> np.ndarray:
 
         if self.kindyn.getFrameIndex(frame_name) < 0:
@@ -390,6 +403,15 @@ def get_average_velocity_jacobian(self) -> np.ndarray:
 
         return J_avg_vel.toNumPy()
 
+    def get_centroidal_average_velocity_jacobian(self) -> np.ndarray:
+
+        J_cen_avg_vel = idt.MatrixDynSize()
+
+        if not self.kindyn.getCentroidalAverageVelocityJacobian(J_cen_avg_vel):
+            raise RuntimeError("Failed to get the average velocity jacobian")
+
+        return J_cen_avg_vel.toNumPy()
+
     def get_frame_bias_acc(self, frame_name: str) -> np.ndarray:
 
         if self.kindyn.getFrameIndex(frame_name) < 0: