From 0755d6dac7f67f650570ac9c534abef29735c424 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Tue, 2 Jul 2019 12:26:51 +0200 Subject: [PATCH] Fix #3: issue with get_link_world_velocities --- pyrobolearn/robots/robot.py | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py index 18e5a2e..e1787f2 100644 --- a/pyrobolearn/robots/robot.py +++ b/pyrobolearn/robots/robot.py @@ -1402,19 +1402,27 @@ class Robot(ControllableBody): if multiple links: np.array[Nx6], np.array[N,6]: linear and angular velocity of each link """ + # if one link, compute the linear and angular velocity of that link if isinstance(link_ids, int): + if link_ids == -1: + return self.get_base_velocity(concatenate=True) lin_vel, ang_vel = self.sim.get_link_state(self.id, link_ids, compute_velocity=True)[6:8] - return np.array(lin_vel + ang_vel) + return np.concatenate((lin_vel, ang_vel)) + + # if multiple links, compute the linear and angular velocity of each link if link_ids is None: link_ids = self.joints - vel = [] + velocities = [] for link in link_ids: lin_vel, ang_vel = self.sim.get_link_state(self.id, link, compute_velocity=True)[6:8] - vel.append(lin_vel + ang_vel) - vel = np.array(vel) + velocities.append(np.concatenate((lin_vel, ang_vel))) + velocities = np.asarray(velocities) + + # if we need to flatten the velocities (N, 6) --> (N*6,) if flatten: - return vel.reshape(-1) # 1d array - return vel # 2D array + return velocities.reshape(-1) # 1d array + + return velocities # 2D array def get_link_linear_velocities(self, link_ids=None, wrt_link_id=None, flatten=True): r"""