From 62bae6117f3c017826689a64f8c52a2c736fa323 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Mon, 1 Jul 2019 19:08:31 +0200 Subject: [PATCH] fix errors in examples when using Python 3 --- examples/dynamics/control/attractor_point.py | 4 +-- examples/kinematics/forward/fk.py | 2 +- ...ic_manipulability_tracking_with_balance.py | 24 +++++++-------- ...om_manipulability_tracking_with_balance.py | 30 +++++++++---------- pyrobolearn/robots/robot.py | 8 ++--- pyrobolearn/simulators/bullet.py | 30 +++++++++++++++---- pyrobolearn/utils/manifold_utils.py | 2 +- 7 files changed, 59 insertions(+), 41 deletions(-) diff --git a/examples/dynamics/control/attractor_point.py b/examples/dynamics/control/attractor_point.py index 47e8c8a..794672d 100644 --- a/examples/dynamics/control/attractor_point.py +++ b/examples/dynamics/control/attractor_point.py @@ -58,9 +58,9 @@ for _ in count(): F = K.dot(x_des - x) - D.dot(dx) # add torques resulting from them - # torques += Jlin.T.dot(F) + torques += Jlin.T.dot(F) # torques += Jlin.T.dot(- D.dot(dx)) # active compliance - # torques = Jlin.T.dot(F) # + # torques = Jlin.T.dot(F) # impedance control robot.set_joint_torques(torques=torques) diff --git a/examples/kinematics/forward/fk.py b/examples/kinematics/forward/fk.py index fa813c0..5f3bce8 100644 --- a/examples/kinematics/forward/fk.py +++ b/examples/kinematics/forward/fk.py @@ -43,7 +43,7 @@ sphere = Body(sim, body_id=sphere) for t in count(): # if no more joint positions, get out of the loop - if t > len(positions): + if t >= len(positions): break # set joint positions diff --git a/examples/manipulability/com_dynamic_manipulability_tracking_with_balance.py b/examples/manipulability/com_dynamic_manipulability_tracking_with_balance.py index a4f029c..9154ca7 100644 --- a/examples/manipulability/com_dynamic_manipulability_tracking_with_balance.py +++ b/examples/manipulability/com_dynamic_manipulability_tracking_with_balance.py @@ -185,13 +185,13 @@ CoMr = robot.get_center_of_mass_position() # Desired CoM print("CoMr: {}".format(CoMr)) if robot.name == 'centauro': - xref_l1f = robot.get_link_frame_world_positions(left_foot1_id) # Desired position for left foot - xref_r1f = robot.get_link_frame_world_positions(right_foot1_id) # Desired position for right foot - xref_l2f = robot.get_link_frame_world_positions(left_foot2_id) # Desired position for left foot - xref_r2f = robot.get_link_frame_world_positions(right_foot2_id) # Desired position for right foot + xref_l1f = robot.get_link_world_frame_positions(left_foot1_id) # Desired position for left foot + xref_r1f = robot.get_link_world_frame_positions(right_foot1_id) # Desired position for right foot + xref_l2f = robot.get_link_world_frame_positions(left_foot2_id) # Desired position for left foot + xref_r2f = robot.get_link_world_frame_positions(right_foot2_id) # Desired position for right foot else: - xref_lf = robot.get_link_frame_world_positions(left_foot_id) # Desired position for left foot - xref_rf = robot.get_link_frame_world_positions(right_foot_id) # Desired position for right foot + xref_lf = robot.get_link_world_frame_positions(left_foot_id) # Desired position for left foot + xref_rf = robot.get_link_world_frame_positions(right_foot_id) # Desired position for right foot # Display initial and desired manipulability ellipsoid @@ -220,13 +220,13 @@ for i in range(num_samples): robot.draw_com_position(0.03) if robot.name == 'centauro': - xt_l1f = robot.get_link_frame_world_positions(left_foot1_id) # Current position for left foot - xt_r1f = robot.get_link_frame_world_positions(right_foot1_id) # Current position for right foot - xt_l2f = robot.get_link_frame_world_positions(left_foot2_id) # Current position for left foot - xt_r2f = robot.get_link_frame_world_positions(right_foot2_id) # Current position for right foot + xt_l1f = robot.get_link_world_frame_positions(left_foot1_id) # Current position for left foot + xt_r1f = robot.get_link_world_frame_positions(right_foot1_id) # Current position for right foot + xt_l2f = robot.get_link_world_frame_positions(left_foot2_id) # Current position for left foot + xt_r2f = robot.get_link_world_frame_positions(right_foot2_id) # Current position for right foot else: - xt_lf = robot.get_link_frame_world_positions(left_foot_id) # Current left foot pos - xt_rf = robot.get_link_frame_world_positions(right_foot_id) # Current right foot pos + xt_lf = robot.get_link_world_frame_positions(left_foot_id) # Current left foot pos + xt_rf = robot.get_link_world_frame_positions(right_foot_id) # Current right foot pos # Simple balance control with IK kinematics for CoM and feet # Get Jacobians: Jcom, Jlf, and Jrf diff --git a/examples/manipulability/com_manipulability_tracking_with_balance.py b/examples/manipulability/com_manipulability_tracking_with_balance.py index c83c1a9..09752eb 100644 --- a/examples/manipulability/com_manipulability_tracking_with_balance.py +++ b/examples/manipulability/com_manipulability_tracking_with_balance.py @@ -181,15 +181,15 @@ CoMr = robot.get_center_of_mass_position() # Desired CoM print("CoMr: {}".format(CoMr)) if robot.name == 'centauro': - xref_l1f = robot.get_link_frame_world_positions(left_foot1_id) # Desired position for left foot - xref_r1f = robot.get_link_frame_world_positions(right_foot1_id) # Desired position for right foot - xref_l2f = robot.get_link_frame_world_positions(left_foot2_id) # Desired position for left foot - xref_r2f = robot.get_link_frame_world_positions(right_foot2_id) # Desired position for right foot + xref_l1f = robot.get_link_world_frame_positions(left_foot1_id) # Desired position for left foot + xref_r1f = robot.get_link_world_frame_positions(right_foot1_id) # Desired position for right foot + xref_l2f = robot.get_link_world_frame_positions(left_foot2_id) # Desired position for left foot + xref_r2f = robot.get_link_world_frame_positions(right_foot2_id) # Desired position for right foot else: - xref_lf = robot.get_link_frame_world_positions(left_foot_id) # Desired position for left foot - # Qref_lf = robot.get_link_frame_world_orientations(leftFootId) - xref_rf = robot.get_link_frame_world_positions(right_foot_id) # Desired position for right foot - # Qref_rf = robot.get_link_frame_world_orientations(rightFootId) + xref_lf = robot.get_link_world_frame_positions(left_foot_id) # Desired position for left foot + # Qref_lf = robot.get_link_world_frame_orientations(leftFootId) + xref_rf = robot.get_link_world_frame_positions(right_foot_id) # Desired position for right foot + # Qref_rf = robot.get_link_world_frame_orientations(rightFootId) # Display initial and desired manipulability ellipsoid @@ -221,14 +221,14 @@ for i in range(400): robot.draw_com_position(0.03) if robot.name == 'centauro': - xt_l1f = robot.get_link_frame_world_positions(left_foot1_id) # Current position for left foot - xt_r1f = robot.get_link_frame_world_positions(right_foot1_id) # Current position for right foot - xt_l2f = robot.get_link_frame_world_positions(left_foot2_id) # Current position for left foot - xt_r2f = robot.get_link_frame_world_positions(right_foot2_id) # Current position for right foot + xt_l1f = robot.get_link_world_frame_positions(left_foot1_id) # Current position for left foot + xt_r1f = robot.get_link_world_frame_positions(right_foot1_id) # Current position for right foot + xt_l2f = robot.get_link_world_frame_positions(left_foot2_id) # Current position for left foot + xt_r2f = robot.get_link_world_frame_positions(right_foot2_id) # Current position for right foot else: - xt_lf = robot.get_link_frame_world_positions(left_foot_id) # Current left foot pos - # Qt_lf = robot.get_link_frame_world_orientations(leftFootId) - xt_rf = robot.get_link_frame_world_positions(right_foot_id) # Current right foot pos + xt_lf = robot.get_link_world_frame_positions(left_foot_id) # Current left foot pos + # Qt_lf = robot.get_link_world_frame_orientations(leftFootId) + xt_rf = robot.get_link_world_frame_positions(right_foot_id) # Current right foot pos # Simple balance control with IK kinematics for CoM and feet # Get Jacobians: Jcom, Jlf, and Jrf diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py index 2792867..18e5a2e 100644 --- a/pyrobolearn/robots/robot.py +++ b/pyrobolearn/robots/robot.py @@ -2236,7 +2236,7 @@ class Robot(ControllableBody): Args: jacobian (np.array[D,N]): jacobian matrix - num_task_vars (float): number of task variables (usually 3 or 6) + num_task_vars (int): number of task variables (usually 3 or 6) Returns: np.array[(num_task_vars * num_task_vars + num_task_vars) / 2, N]: manipulability jacobian matrix @@ -2264,7 +2264,7 @@ class Robot(ControllableBody): # Manipulability Jacobian in matrix form (Mandel notation) # num_vars = len(num_task_vars) - Jm_red = np.zeros(((num_task_vars * num_task_vars + num_task_vars) / 2, np.sum(num_dofs))) + Jm_red = np.zeros((int((num_task_vars * num_task_vars + num_task_vars) / 2), np.sum(num_dofs))) # print("Jm_red.shape: {}".format(Jm_red.shape)) for i in range(Jm.shape[2]): @@ -2954,7 +2954,7 @@ class Robot(ControllableBody): Args: jacobian (np.array[D,N]): Jacobian matrix inertia (np.array[N,N]): inertia matrix - num_task_vars (float): number of task variables (usually 3 or 6) + num_task_vars (int): number of task variables (usually 3 or 6) Returns: np.array[(num_task_vars * num_task_vars + num_task_vars) / 2, N]: manipulability jacobian matrix @@ -2983,7 +2983,7 @@ class Robot(ControllableBody): # # Manipulability Jacobian in matrix form (Mandel notation) # num_vars = len(num_task_vars) - Jm_red = np.zeros(((num_task_vars * num_task_vars + num_task_vars) / 2, np.sum(num_dofs))) + Jm_red = np.zeros((int((num_task_vars * num_task_vars + num_task_vars) / 2), np.sum(num_dofs))) # print("Jm_red.shape: {}".format(Jm_red.shape)) for i in range(Jm.shape[2]): diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py index 2449086..a5029ec 100644 --- a/pyrobolearn/simulators/bullet.py +++ b/pyrobolearn/simulators/bullet.py @@ -987,7 +987,9 @@ class Bullet(Simulator): Returns: str: base name """ - return self.sim.getBodyInfo(body_id) + name = self.sim.getBodyInfo(body_id) + name = name if isinstance(name, str) else name.decode("utf-8") + return name def get_body_id(self, index): """ @@ -1182,7 +1184,9 @@ class Bullet(Simulator): Returns: str: base name """ - return self.sim.getBodyInfo(body_id)[0] + name = self.sim.getBodyInfo(body_id)[0] + name = name if isinstance(name, str) else name.decode("utf-8") + return name def get_center_of_mass_position(self, body_id, link_ids=None): """ @@ -1515,6 +1519,8 @@ class Bullet(Simulator): [16] int: parent link index, -1 for base """ info = list(self.sim.getJointInfo(body_id, joint_id)) + info[1] = info[1] if isinstance(info[1], str) else info[1].decode("utf-8") # bytes vs str (Py2 vs Py3) + info[12] = info[12] if isinstance(info[12], str) else info[12].decode("utf-8") info[13] = np.asarray(info[13]) info[14] = np.asarray(info[14]) info[15] = np.asarray(info[15]) @@ -1739,14 +1745,18 @@ class Bullet(Simulator): if isinstance(link_ids, int): if link_ids == -1: return self.get_base_name(body_id) - return self.sim.getJointInfo(body_id, link_ids)[12] + name = self.sim.getJointInfo(body_id, link_ids)[12] + name = name if isinstance(name, str) else name.decode("utf-8") # bytes vs str (Py2 vs Py3) + return name link_names = [] for link_id in link_ids: if link_id == -1: link_names.append(self.get_base_name(body_id)) else: - link_names.append(self.sim.getJointInfo(body_id, link_id)[12]) + name = self.sim.getJointInfo(body_id, link_id)[12] + name = name if isinstance(name, str) else name.decode("utf-8") # bytes vs str (Py2 vs Py3) + link_names.append(name) return link_names def get_link_masses(self, body_id, link_ids): @@ -1967,8 +1977,16 @@ class Bullet(Simulator): str[N]: name of each joint """ if isinstance(joint_ids, int): - return self.sim.getJointInfo(body_id, joint_ids)[1] - return [self.sim.getJointInfo(body_id, joint_id)[1] for joint_id in joint_ids] + name = self.sim.getJointInfo(body_id, joint_ids)[1] + name = name if isinstance(name, str) else name.decode("utf-8") + return name + + names = [] + for joint_id in joint_ids: + name = self.sim.getJointInfo(body_id, joint_id)[1] + name = name if isinstance(name, str) else name.decode("utf-8") + names.append(name) + return names def get_joint_type_ids(self, body_id, joint_ids): """ diff --git a/pyrobolearn/utils/manifold_utils.py b/pyrobolearn/utils/manifold_utils.py index d3b5bed..50e6209 100644 --- a/pyrobolearn/utils/manifold_utils.py +++ b/pyrobolearn/utils/manifold_utils.py @@ -31,7 +31,7 @@ def tensor_matrix_product(tensor, matrix, mode): N = len(tensor.shape) # Compute the complement of the set of modes - modec = range(0, N) + modec = list(range(0, N)) modec.remove(mode) # Permutation of the tensor