wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-09 11:31:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix errors in examples when using Python 3

Browse Source
This commit is contained in:
Brian Delhaisse
2019-07-01 19:08:31 +02:00
parent fbe4c442d8
commit 62bae6117f
7 changed files with 59 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/dynamics/control/attractor_point.py
+2 -2
View File
@@ -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)
examples/kinematics/forward/fk.py
+1 -1
View File
@@ -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
examples/manipulability/com_dynamic_manipulability_tracking_with_balance.py
+12 -12
View File
@@ -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
examples/manipulability/com_manipulability_tracking_with_balance.py
+15 -15
View File
@@ -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
pyrobolearn/robots/robot.py
+4 -4
View File
@@ -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]):
pyrobolearn/simulators/bullet.py
+24 -6
View File
@@ -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):
"""
pyrobolearn/utils/manifold_utils.py
+1 -1
View File
@@ -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