diff --git a/examples/manipulability/com_dynamic_manipulability_tracking_with_balance.py b/examples/manipulability/com_dynamic_manipulability_tracking_with_balance.py new file mode 100644 index 0000000..a4f029c --- /dev/null +++ b/examples/manipulability/com_dynamic_manipulability_tracking_with_balance.py @@ -0,0 +1,386 @@ +#!/usr/bin/env python +"""Center of mass manipulability tracking + +Track the dynamic manipulability ellipsoid of the center of mass of a particular robot while keeping its balance. + +See Also: + - `com_manipulability_tracking.py`: in this example, we track the velocity manipulability ellipsoid while the base + is fixed. + - `com_manipulability_tracking_with_balance.py`: in this example, we track the velocity manipulability ellipsoid + instead of the dynamic one. + +References: + [1] "Robotics: Modelling, Planning and Control" (section 3.9), Siciliano et al., 2010 + [2] "Geometry-aware Tracking of Manipulability Ellipsoids", Jaquier et al., R:SS, 2018 +""" + +import time +# from itertools import count +import numpy as np +from scipy.linalg import block_diag +import matplotlib.pyplot as plt +import argparse + +from pyrobolearn.simulators import Bullet +from pyrobolearn.worlds import BasicWorld +from pyrobolearn.robots import Nao, Centauro + + +# create parser +parser = argparse.ArgumentParser() +parser.add_argument('-r', '--robot', help='the robot to track the velocity manipulability ellipsoid', type=str, + choices=['nao', 'centauro'], default='nao') +parser.add_argument('-n', '--init_manipulability', help='which desired velocity manipulability we want to track', + type=int, default=3) +parser.add_argument('-i', '--init_config', help='the initial configuration for the robot', type=int, + choices=range(1, 6), default=1) +args = parser.parse_args() + +# program variable +robot_name = args.robot +dt = 0.01 # Sampling time +num_des_manip = args.init_manipulability # Number of desired manipulability (Useful for tests) +init_qs = args.init_config # Number of initial configuration for the robots (Useful for tests) + + +# Create simulator and world +sim = Bullet() +world = BasicWorld(sim) + +# load robot +if robot_name == 'nao': + robot = Nao(sim, fixed_base=False) +elif robot_name == 'centauro': + robot = Centauro(sim, fixed_base=False) +else: + raise NotImplementedError("The given robot has not been implemented") + +# Loop for setting stable initial conditions +# for i in range(50): +# world.step(sleep_dt=0.1) + +if robot.name == 'nao': + Km = 100 * np.eye(6) # Proportional gain for Nao for manip. tracking + + # desired dynamic Manipulability for CoM (Nao) + des_manips = np.array([[[1.539e-03, 6.653e-04, 0.833e-04], + [6.653e-04, 2.080e-03, -5.843e-05], + [0.833e-04, -5.843e-05, 8.601e-04]], + [[2.580e-03, 1.653e-03, 4.833e-04], + [1.653e-03, 1.539e-03, -5.843e-05], + [4.833e-04, -5.843e-05, 9.601e-04]], + [[1.580e-03, .653e-03, 4.833e-04], + [.653e-03, 1.539e-03, -5.843e-05], + [4.833e-04, -5.843e-05, 9.601e-04]], + [[1e-04, 0.0, 0.0], + [0.0, 5e-04, 0.0], + [0.0, 0.0, 1e-04]]]) + des_dyn_manip = des_manips[num_des_manip-1, :, :] + + # Get ids for feet (used for kinematics function) + left_foot_id = robot.get_link_ids('l_ankle') + right_foot_id = robot.get_link_ids('r_ankle') + + # Gain matrices + Kcom = np.diag((50, 40, 0)) # Proportional gain for CoM position control + Klf = np.diag((20, 20, 20)) # Proportional gain for foot position control + Krf = np.diag((20, 20, 20)) # Proportional gain for foot position control + + # Setting initial configuration of the robot + if init_qs == 1: + q0 = [1.55, 0.135, -1.05, -0.36, 1.55, -0.135, 1.05, 0.36] + q0id = ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', + 'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll'] + elif init_qs == 2: + q0 = [1.55, 0.135, -1.05, -0.36, 1.55, -0.135, 1.05, 0.36, 0.1, -0.1, 0.12, 0.12, -0.12, -0.12] + q0id = ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'RShoulderPitch', 'RShoulderRoll', + 'RElbowYaw', 'RElbowRoll', 'LHipRoll', 'RHipRoll', 'LKneePitch', 'RKneePitch', 'LAnklePitch', + 'RAnklePitch'] + elif init_qs == 3: + q0 = [2.0, 0.4, -1.3, -1.2, 2.0, -0.4, 1.3, 1.2, 0.05, -0.05, 0.2, 0.2, -0.2, -0.2] + q0id = ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'RShoulderPitch', 'RShoulderRoll', + 'RElbowYaw', 'RElbowRoll', 'LHipRoll', 'RHipRoll', 'LKneePitch', 'RKneePitch', 'LAnklePitch', + 'RAnklePitch'] + else: + q0 = 0 + q0id = 0 + +elif robot.name == 'centauro': + Km = 700 * np.eye(6) # Proportional gain for Centauro + + # desired dynamic Manipulability for CoM (Centauro) + des_manips = np.array([[[0.321, 0.025, -0.118], + [0.025, 0.176, -0.058], + [-0.118, -0.058, 0.187]], + [[0.044, 0.016, 0.005], + [0.016, 0.464, -0.013], + [0.005, -0.013, 0.049]], + [[0.827, 0.096, 0.311], + [0.096, 0.515, -1.770], + [0.311, -1.770, 8.547]]]) + des_dyn_manip = des_manips[num_des_manip-1, :, :] + print("desVelMan: {}".format(des_dyn_manip)) + + # Get ids for "feet" (used for kinematics function) + left_foot1_id = robot.get_link_ids('wheel_1') + right_foot1_id = robot.get_link_ids('wheel_2') + left_foot2_id = robot.get_link_ids('wheel_3') + right_foot2_id = robot.get_link_ids('wheel_4') + + # Gain matrices + Kcom = np.diag((250., 250., 0.)) # Proportional gain for CoM position control + Kl1f = np.diag((180., 180., 180.)) # Proportional gain for foot position control + Kr1f = np.diag((180., 180., 180.)) # Proportional gain for foot position control + Kl2f = np.diag((180., 180., 180.)) # Proportional gain for foot position control + Kr2f = np.diag((180., 180., 180.)) # Proportional gain for foot position control + + # Setting initial configuration of the robot + if init_qs == 1: + q0 = [-.7, -.65, .61, -.5, .71, .64, -.68, .7] + q0id = ['j_arm1_1', 'j_arm1_2', 'j_arm1_3', 'j_arm1_4', 'j_arm2_1', 'j_arm2_2', 'j_arm2_3', 'j_arm2_4'] + elif init_qs == 2: + q0 = [-.7, -.65, .61, -.5, .71, .64, -.68, .7, -0.42, -0.96, -0.59, + 0.42, 0.96, 0.59, 0.42, 0.96, 0.59, -0.42, -0.96, -0.59] + q0id = ['j_arm1_1', 'j_arm1_2', 'j_arm1_3', 'j_arm1_4', 'j_arm2_1', 'j_arm2_2', 'j_arm2_3', 'j_arm2_4', + 'hip_pitch_1', 'knee_pitch_1', 'ankle_pitch_1', 'hip_pitch_2', 'knee_pitch_2', 'ankle_pitch_2', + 'hip_pitch_3', 'knee_pitch_3', 'ankle_pitch_3', 'hip_pitch_4', 'knee_pitch_4', 'ankle_pitch_4'] + elif init_qs == 3: + q0 = [-.3, -1.3, .61, -.5, .3, 1.3, -.68, .7, -0.42, -0.96, -0.59, + 0.42, 0.96, 0.59, 0.42, 0.96, 0.59, -0.42, -0.96, -0.59] + q0id = ['j_arm1_1', 'j_arm1_2', 'j_arm1_3', 'j_arm1_4', 'j_arm2_1', 'j_arm2_2', 'j_arm2_3', 'j_arm2_4', + 'hip_pitch_1', 'knee_pitch_1', 'ankle_pitch_1', 'hip_pitch_2', 'knee_pitch_2', 'ankle_pitch_2', + 'hip_pitch_3', 'knee_pitch_3', 'ankle_pitch_3', 'hip_pitch_4', 'knee_pitch_4', 'ankle_pitch_4'] + elif init_qs == 4: + q0 = [0.0, -1.3, 1.4, -1.2, 0.0, 1.3, -1.4, 1.2, -0.42, -0.96, -0.59, + 0.42, 0.96, 0.59, 0.42, 0.96, 0.59, -0.42, -0.96, -0.59] + q0id = ['j_arm1_1', 'j_arm1_2', 'j_arm1_3', 'j_arm1_4', 'j_arm2_1', 'j_arm2_2', 'j_arm2_3', 'j_arm2_4', + 'hip_pitch_1', 'knee_pitch_1', 'ankle_pitch_1', 'hip_pitch_2', 'knee_pitch_2', 'ankle_pitch_2', + 'hip_pitch_3', 'knee_pitch_3', 'ankle_pitch_3', 'hip_pitch_4', 'knee_pitch_4', 'ankle_pitch_4'] + else: + q0 = 0 + q0id = 0 + +else: + left_foot_id, right_foot_id = 0, 0 + +if not (isinstance(q0, int) and q0 == 0 and isinstance(q0id, int) and q0id == 0): + # Loop need to set the robot initial posture + for n in range(100): + robot.set_joint_positions(np.asarray(q0), robot.get_joint_ids(np.asarray(q0id))) + world.step(sleep_dt=0.2) + +# Augmented gain matrix for balancing controller +if robot.name == 'centauro': + Kbal = block_diag(Kl1f, Kr1f, Kl2f, Kr2f, Kcom) +else: + Kbal = block_diag(Klf, Krf, Kcom) +print("Kbal: {}".format(Kbal)) + + +# Initial conditions +time.sleep(1.0) + +num_dofs = robot.num_dofs - 6 +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 +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 + + +# Display initial and desired manipulability ellipsoid +q0 = robot.get_joint_positions() +print("q0: {}".format(q0)) +Jcom0 = robot.get_center_of_mass_jacobian(q0) +H0 = robot.get_mass_matrix(q0) +if not robot.has_fixed_base(): + dynMan = robot.compute_dynamic_manipulability_ellipsoid(Jcom0[:, 6:], H0[6:, 6:]) +else: + dynMan = robot.compute_dynamic_manipulability_ellipsoid(Jcom0, H0) +print("Mv0: {}".format(dynMan[0:3, 0:3])) + +# Logging variables +# Format: [q minEigvalue(Jbal) minEigvalue(Jman) balanceError CurrentManip(1x9) SPDdistance] +num_samples = 400 +log_array = np.zeros((num_samples, num_dofs + 2 + Kbal.shape[0] + dynMan[0:3, 0:3].size + 1)) + + +# Run simulator +# for i in count(): +for i in range(num_samples): + # Update current robot state + qt = robot.get_joint_positions() + CoMt = robot.get_center_of_mass_position() # Current CoM + 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 + 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 + + # Simple balance control with IK kinematics for CoM and feet + # Get Jacobians: Jcom, Jlf, and Jrf + Jcom = robot.get_center_of_mass_jacobian(qt) + + if robot.name == 'centauro': + Jl1f = robot.get_jacobian(left_foot1_id, qt) + Jr1f = robot.get_jacobian(right_foot1_id, qt) + Jl2f = robot.get_jacobian(left_foot2_id, qt) + Jr2f = robot.get_jacobian(right_foot2_id, qt) + else: + Jlf = robot.get_jacobian(left_foot_id, qt) + Jrf = robot.get_jacobian(right_foot_id, qt) + + # Compose Jacobian and nullspace for balancing task + if robot.name == 'centauro': + Jbal = np.vstack((Jl1f[0:3, ], Jr1f[0:3, ], Jl2f[0:3, ], Jr2f[0:3, ], Jcom[0:3, ])) + else: + Jbal = np.vstack((Jlf[0:3, ], Jrf[0:3, ], Jcom[0:3, ])) + + Ubal, Sbal, VhBal = np.linalg.svd(Jbal) + if np.min(Sbal) < 4.5E-2: + pJbal = robot.get_damped_least_squares_inverse(Jbal, 4.5E-2) + else: + pJbal = robot.get_damped_least_squares_inverse(Jbal, 1E-8) + Nbal = np.eye(Jbal.shape[1]) - np.dot(pJbal, Jbal) + + # Compute balancing task errors + dx_com = CoMr - CoMt # CoM error + + if robot.name == 'centauro': + dx_l1f = xref_l1f - xt_l1f # Left foot position error + dx_r1f = xref_r1f - xt_r1f # Right foot position error + dx_l2f = xref_l2f - xt_l2f # Left foot position error + dx_r2f = xref_r2f - xt_r2f # Right foot position error + dx_bal = np.vstack((dx_l1f.reshape(3, 1), dx_r1f.reshape(3, 1), + dx_l2f.reshape(3, 1), dx_r2f.reshape(3, 1), dx_com.reshape(3, 1))) # Augmented error vector + else: + dx_lf = xref_lf - xt_lf # Left foot position error + dx_rf = xref_rf - xt_rf # Right foot position error + dx_bal = np.vstack((dx_lf.reshape(3, 1), dx_rf.reshape(3, 1), dx_com.reshape(3, 1))) # Augmented error vector + + # Proportional controller for position + dxref_bal = np.dot(Kbal, dx_bal) + + # Compute desired joint velocities for balancing + dq_bal = np.dot(pJbal, dxref_bal) + dq_bal = dq_bal.reshape((Jbal.shape[1],)) + + # Tracking of CoM dynamic manipulability in nullspace + Ht = robot.get_mass_matrix(qt) + if not robot.has_fixed_base(): + dynMan = robot.compute_dynamic_manipulability_ellipsoid(Jcom[:, 6:], Ht[6:, 6:]) + else: + dynMan = robot.compute_dynamic_manipulability_ellipsoid(Jcom, Ht) + + # Obtaining joint velocity command + if not robot.has_fixed_base(): + dq_man, minSman, SPDdist = robot.calculate_inverse_differential_kinematics_dynamic_manipulability(Jcom[:, 6:], + Ht[6:, 6:], + des_dyn_manip, + Km) + else: + dq_man, minSman, SPDdist = robot.calculate_inverse_differential_kinematics_dynamic_manipulability(Jcom, Ht, + des_dyn_manip, + Km) + + # Logging + # Format: [q minEigvalue(Jbal) minEigvalue(Jman) balanceError CurrentManip(1x9) SPDdistance] + log_array[i, ] = np.hstack((qt.reshape(1, num_dofs), np.min(Sbal).reshape(1, 1), + minSman.reshape(1, 1), dx_bal.T, dynMan[0:3, 0:3].reshape(1, dynMan[0:3, 0:3].size), + SPDdist.reshape(1, 1))) + + # Set joint position + if not robot.has_fixed_base(): + dq_man = np.concatenate((np.zeros((6,)), dq_man)) + dq = dq_bal + np.dot(Nbal, dq_man) + dq = dq[6:, ] + else: + dq = dq_bal + np.dot(Nbal, dq_man) + + q = qt + (dq * dt) + robot.set_joint_positions(q) + + world.step(sleep_dt=dt) + + +# Saving log data +# np.savetxt(robot.name + 'log_Man' + str(num_des_manip) + 'Pos' + str(init_qs) + '.csv', log_array, delimiter=',') + +# Plotting logged data +fig1 = plt.figure(1, figsize=(14, 10)) +# wspace: width reserved for blank space between subplots, hspace: height reserved for white space between subplots +fig1.subplots_adjust(left=0.09, bottom=0.05, right=0.99, wspace=0.2) +plt.suptitle('Robot joints') +plt.rcParams.update({'font.size': 8}) +for i in range(num_dofs): + if robot.name == 'nao': + plt.subplot(6, 7, i+1) # NAO + elif robot.name == 'centauro': + plt.subplot(7, 7, i + 1) # Centauro + + plt.ylabel(robot.get_joint_names(robot.get_joint_ids(i))) + plt.plot(log_array[:, i]) + plt.ylim((-1.5, 1.5)) +# fig1.savefig(robot.name + '_joints_Man' + str(num_des_manip) + 'Pos' + str(init_qs) + '.png', +# bbox_inches='tight', dpi=200) + +fig2 = plt.figure(2, figsize=(14, 10)) +fig2.subplots_adjust(left=0.09, bottom=0.05, right=0.99, wspace=0.2) +plt.rcParams.update({'font.size': 12}) +plt.suptitle('MinEigenvalues, balance and manipulatility errors.') +plt.rcParams.update({'font.size': 8}) +if robot.name == 'nao': + plt.subplot(4, 3, 1) # NAO + plt.ylim((0, 0.05)) # NAO +elif robot.name == 'centauro': + plt.subplot(6, 3, 1) # Centauro + plt.ylim((0, 0.02)) # Centauro + +plt.ylabel('minEigvalue(Jbal)') +plt.plot(log_array[:, num_dofs]) + +if robot.name == 'nao': + plt.subplot(4, 3, 2) # NAO + plt.ylim((0., 0.0001)) # NAO +elif robot.name == 'centauro': + plt.subplot(6, 3, 2) # Centauro + plt.ylim((0., 0.001)) # Centauro + +plt.ylabel('minEigvalue(Jman)') +plt.plot(log_array[:, num_dofs+1]) + +if robot.name == 'nao': + plt.subplot(4, 3, 3) # NAO + plt.ylim((0., 4.0)) # NAO +elif robot.name == 'centauro': + plt.subplot(6, 3, 3) # Centauro + plt.ylim((0., 5.0)) # Centauro + +plt.ylabel('SPDdist') +plt.plot(log_array[:, -1]) + +for i in range(dx_bal.shape[0]): + if robot.name == 'nao': + plt.subplot(4, 3, i+4) # NAO + elif robot.name == 'centauro': + plt.subplot(6, 3, i + 4) # Centauro + + plt.ylabel('dx_bal'+str(i+1)) + plt.plot(log_array[:, num_dofs + 2 + i]) + plt.ylim((-.05, .05)) +# fig2.savefig(robot.name + '_eigValsAndErrors_Man' + str(num_des_manip) + 'Pos' + str(init_qs) + '.png', +# bbox_inches='tight') + +# plt.tight_layout() +plt.show() diff --git a/examples/manipulability/com_manipulability_tracking.py b/examples/manipulability/com_manipulability_tracking.py index dc4ec17..05c3fc5 100644 --- a/examples/manipulability/com_manipulability_tracking.py +++ b/examples/manipulability/com_manipulability_tracking.py @@ -5,7 +5,10 @@ Track the velocity manipulability ellipsoid of the center of mass of a particula base is fixed. See Also: - - `file.py`: short description + - `com_manipulability_tracking_with_balance.py`: in this example, we track the velocity manipulability ellipsoid + while keeping the robot balanced. + - `com_dynamic_manipulability_tracking_with_balance.py`: in this example, the dynamic manipulability ellipsoid is + tracked instead of the velocity one. References: [1] "Robotics: Modelling, Planning and Control" (section 3.9), Siciliano et al., 2010 @@ -14,16 +17,24 @@ References: from itertools import count import numpy as np +import argparse from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Centauro, Cogimon, Nao, KukaIIWA -# select robot to use -robot_name = 'nao' # 'kuka_iiwa', 'cogimon', 'centauro' +# create parser to select the robot to use +parser = argparse.ArgumentParser() +parser.add_argument('-r', '--robot', help='the robot to track the velocity manipulability ellipsoid', type=str, + choices=['nao', 'kuka_iiwa', 'cogimon', 'centauro'], default='nao') +args = parser.parse_args() + +# get the robot to use for the example +robot_name = args.robot dt = 0.01 + # Create simulator and world sim = Bullet() world = BasicWorld(sim) @@ -118,8 +129,9 @@ for i in count(): # Plot current manipulability ellipsoid if i % 10 == 0: - robot.update_manipulability_ellipsoid(link_id=-1, ellipsoid_id=ellipsoid_id, - ellipsoid=10 * velocity_manip[:3, :3], color=(0.75, 0.1, 0.1, 0.6)) + ellipsoid_id = robot.update_manipulability_ellipsoid(link_id=-1, ellipsoid_id=ellipsoid_id, + ellipsoid=10 * velocity_manip[:3, :3], + color=(0.75, 0.1, 0.1, 0.6)) # Obtaining joint velocity command dq = robot.calculate_inverse_differential_kinematics_velocity_manipulability(Jcom, desired_velocity_manip, Km)[0] diff --git a/examples/manipulability/com_manipulability_tracking_with_balance.py b/examples/manipulability/com_manipulability_tracking_with_balance.py new file mode 100644 index 0000000..c83c1a9 --- /dev/null +++ b/examples/manipulability/com_manipulability_tracking_with_balance.py @@ -0,0 +1,392 @@ +#!/usr/bin/env python +"""Center of mass manipulability tracking + +Track the velocity manipulability ellipsoid of the center of mass of a particular robot while keeping its balance. + +See Also: + - `com_manipulability_tracking.py`: in this example, the base is fixed. + - `com_dynamic_manipulability_tracking_with_balance.py`: in this example, the dynamic manipulability ellipsoid is + tracked instead of the velocity one. + +References: + [1] "Robotics: Modelling, Planning and Control" (section 3.9), Siciliano et al., 2010 + [2] "Geometry-aware Tracking of Manipulability Ellipsoids", Jaquier et al., R:SS, 2018 +""" + +import time +# from itertools import count +import numpy as np +from scipy.linalg import block_diag +import matplotlib.pyplot as plt +import argparse + +from pyrobolearn.simulators import Bullet +from pyrobolearn.worlds import BasicWorld +from pyrobolearn.robots import Nao, Centauro + + +# create parser +parser = argparse.ArgumentParser() +parser.add_argument('-r', '--robot', help='the robot to track the velocity manipulability ellipsoid', type=str, + choices=['nao', 'centauro'], default='nao') +parser.add_argument('-n', '--init_manipulability', help='which desired velocity manipulability we want to track', + type=int, default=4) +parser.add_argument('-i', '--init_config', help='the initial configuration for the robot', type=int, + choices=range(1, 5), default=1) +args = parser.parse_args() + +# program variable +robot_name = args.robot +dt = 0.01 # Sampling time +num_des_manip = args.init_manipulability # Number of desired manipulability (Useful for tests) +init_qs = args.init_config # Number of initial configuration for the robots (Useful for tests) + + +# Create simulator and world +sim = Bullet() +world = BasicWorld(sim) + +# load robot +if robot_name == 'nao': + robot = Nao(sim, fixed_base=False) +elif robot_name == 'centauro': + robot = Centauro(sim, fixed_base=False) +else: + raise NotImplementedError("The given robot has not been implemented") + +# Loop for setting stable initial conditions +for i in range(50): + world.step(sleep_dt=0.1) + +# Define velocity manipulability for CoM, proportional gain, and initial configurations +if robot.name == 'nao': + Km = 100 * np.eye(6) # Proportional gain for Nao for manip. tracking + + # desired Velocity Manipulability for CoM (Nao) + des_manips = np.array([[[1.539e-03, 6.653e-04, 0.833e-04], + [6.653e-04, 2.080e-03, -5.843e-05], + [0.833e-04, -5.843e-05, 8.601e-04]], + [[2.580e-03, 1.653e-03, 4.833e-04], + [1.653e-03, 1.539e-03, -5.843e-05], + [4.833e-04, -5.843e-05, 9.601e-04]], + [[1.580e-03, .653e-03, 4.833e-04], + [.653e-03, 1.539e-03, -5.843e-05], + [4.833e-04, -5.843e-05, 9.601e-04]], + [[1e-04, 0.0, 0.0], + [0.0, 5e-04, 0.0], + [0.0, 0.0, 1e-04]]]) + des_vel_manip = des_manips[num_des_manip - 1, :, :] + + # Get ids for feet (used for kinematics function) + left_foot_id = robot.get_link_ids('l_ankle') + right_foot_id = robot.get_link_ids('r_ankle') + + # Gain matrices + Kcom = np.diag((50, 40, 0)) # Proportional gain for CoM position control + Klf = np.diag((20, 20, 20)) # Proportional gain for foot position control + Krf = np.diag((20, 20, 20)) # Proportional gain for foot position control + + # Setting initial configuration of the robot + if init_qs == 1: + q0 = [1.55, 0.135, -1.05, -0.36, 1.55, -0.135, 1.05, 0.36] + q0id = ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', + 'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll'] + elif init_qs == 2: + q0 = [1.55, 0.135, -1.05, -0.36, 1.55, -0.135, 1.05, 0.36, 0.1, -0.1, 0.12, 0.12, -0.12, -0.12] + q0id = ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'RShoulderPitch', 'RShoulderRoll', + 'RElbowYaw', 'RElbowRoll', 'LHipRoll', 'RHipRoll', 'LKneePitch', 'RKneePitch', 'LAnklePitch', + 'RAnklePitch'] + elif init_qs == 3: + q0 = [2.0, 0.4, -1.3, -1.2, 2.0, -0.4, 1.3, 1.2, 0.05, -0.05, 0.2, 0.2, -0.2, -0.2] + q0id = ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'RShoulderPitch', 'RShoulderRoll', + 'RElbowYaw', 'RElbowRoll', 'LHipRoll', 'RHipRoll', 'LKneePitch', 'RKneePitch', 'LAnklePitch', + 'RAnklePitch'] + else: + q0 = 0 + q0id = 0 + +elif robot.name == 'centauro': + Km = 50 * np.eye(6) # Proportional gain for Centauro + # desired Velocity Manipulability for CoM (Centauro) + des_manips = np.array([[[0.0207, 0.008, 0.0], + [0.008, 0.01, -0.005], + [0.0, -0.005, 0.006]], + [[5.173e-03, -2.733e-03, 1.920e-03], + [-2.733e-03, 2.038e-02, 7.185e-04], + [1.920e-03, 7.185e-04, 2.107e-03]], + [[2.2e-02, 0.0, -0.01], + [0.0, 2.1e-02, 0.0], + [-0.01, 0.0, 5.e-03]], + [[5e-04, 0.0, 0.0], + [0.0, .1, 0.0], + [0.0, 0.0, 5e-04]]]) + des_vel_manip = des_manips[num_des_manip - 1, :, :] + print("desired velocity manipulability: {}".format(des_vel_manip)) + + # Get ids for "feet" (used for kinematics function) + left_foot1_id = robot.get_link_ids('wheel_1') + right_foot1_id = robot.get_link_ids('wheel_2') + left_foot2_id = robot.get_link_ids('wheel_3') + right_foot2_id = robot.get_link_ids('wheel_4') + + # Gain matrices + Kcom = np.diag((250.0, 250.0, 0.0)) # Proportional gain for CoM position control + Kl1f = np.diag((180, 180, 180)) # Proportional gain for foot position control + Kr1f = np.diag((180, 180, 180)) # Proportional gain for foot position control + Kl2f = np.diag((180, 180, 180)) # Proportional gain for foot position control + Kr2f = np.diag((180, 180, 180)) # Proportional gain for foot position control + + # Setting initial configuration of the robot + if init_qs == 1: + q0 = [-.7, -.65, .61, -.5, .71, .64, -.68, .7] + q0id = ['j_arm1_1', 'j_arm1_2', 'j_arm1_3', 'j_arm1_4', 'j_arm2_1', 'j_arm2_2', 'j_arm2_3', 'j_arm2_4'] + elif init_qs == 2: + q0 = [-.7, -.65, .61, -.5, .71, .64, -.68, .7, -0.42, -0.96, -0.59, + 0.42, 0.96, 0.59, 0.42, 0.96, 0.59, -0.42, -0.96, -0.59] + q0id = ['j_arm1_1', 'j_arm1_2', 'j_arm1_3', 'j_arm1_4', 'j_arm2_1', 'j_arm2_2', 'j_arm2_3', 'j_arm2_4', + 'hip_pitch_1', 'knee_pitch_1', 'ankle_pitch_1', 'hip_pitch_2', 'knee_pitch_2', 'ankle_pitch_2', + 'hip_pitch_3', 'knee_pitch_3', 'ankle_pitch_3', 'hip_pitch_4', 'knee_pitch_4', 'ankle_pitch_4'] + elif init_qs == 3: + q0 = [-.3, -1.3, .61, -.5, .3, 1.3, -.68, .7, -0.42, -0.96, -0.59, + 0.42, 0.96, 0.59, 0.42, 0.96, 0.59, -0.42, -0.96, -0.59] + q0id = ['j_arm1_1', 'j_arm1_2', 'j_arm1_3', 'j_arm1_4', 'j_arm2_1', 'j_arm2_2', 'j_arm2_3', 'j_arm2_4', + 'hip_pitch_1', 'knee_pitch_1', 'ankle_pitch_1', 'hip_pitch_2', 'knee_pitch_2', 'ankle_pitch_2', + 'hip_pitch_3', 'knee_pitch_3', 'ankle_pitch_3', 'hip_pitch_4', 'knee_pitch_4', 'ankle_pitch_4'] + else: + q0 = 0 + q0id = 0 + +else: + left_foot_id, right_foot_id = 0, 0 + +# Loop need to set the robot initial posture +if not (isinstance(q0, int) and q0 == 0 and isinstance(q0id, int) and q0id == 0): + for n in range(15): + robot.set_joint_positions(np.asarray(q0), robot.get_joint_ids(np.asarray(q0id))) + world.step() + +# Augmented gain matrix for balancing controller +if robot.name == 'centauro': + Kbal = block_diag(Kl1f, Kr1f, Kl2f, Kr2f, Kcom) +else: + Kbal = block_diag(Klf, Krf, Kcom) +print("Kbal: {}".format(Kbal)) + + +# Initial conditions +time.sleep(2.0) + +num_dofs = robot.num_dofs - 6 +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 +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) + + +# Display initial and desired manipulability ellipsoid +q0 = robot.get_joint_positions() +print("q0: {}".format(q0)) +Jcom0 = robot.get_center_of_mass_jacobian(q0) +if not robot.has_fixed_base(): + vel_manip = robot.compute_velocity_manipulability_ellipsoid(Jcom0[:, 6:]) +else: + vel_manip = robot.compute_velocity_manipulability_ellipsoid(Jcom0) +print("Mv0: {}".format(vel_manip[0:3, 0:3])) + +base_pos = robot.get_base_position() +robot.draw_velocity_manipulability_ellipsoid(link_id=-1, JJT=10 * des_vel_manip, color=(0.1, 0.75, 0.1, 0.6)) +ellipsoid_id = robot.draw_velocity_manipulability_ellipsoid(link_id=-1, JJT=10 * vel_manip[0:3, 0:3], + color=(0.75, 0.1, 0.1, 0.6)) + +# Logging variables +# Format: [q minEigvalue(Jbal) minEigvalue(Jman) balanceError CurrentManip(1x9) SPDdistance] +log_array = np.zeros((400, num_dofs + 2 + Kbal.shape[0] + vel_manip[0:3, 0:3].size + 1)) + + +# Run simulator +# for i in count(): +for i in range(400): + # Update current robot state + qt = robot.get_joint_positions() + CoMt = robot.get_center_of_mass_position() # Current CoM + 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 + 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 + + # Simple balance control with IK kinematics for CoM and feet + # Get Jacobians: Jcom, Jlf, and Jrf + Jcom = robot.get_center_of_mass_jacobian(qt) + + if robot.name == 'centauro': + Jl1f = robot.get_jacobian(left_foot1_id, qt) + Jr1f = robot.get_jacobian(right_foot1_id, qt) + Jl2f = robot.get_jacobian(left_foot2_id, qt) + Jr2f = robot.get_jacobian(right_foot2_id, qt) + else: + Jlf = robot.get_jacobian(left_foot_id, qt) + Jrf = robot.get_jacobian(right_foot_id, qt) + + # Compose Jacobian and nullspace for balancing task + if robot.name == 'centauro': + Jbal = np.vstack((Jl1f[0:3, ], Jr1f[0:3, ], Jl2f[0:3, ], Jr2f[0:3, ], Jcom[0:3, ])) + else: + Jbal = np.vstack((Jlf[0:3, ], Jrf[0:3, ], Jcom[0:3, ])) + + Ubal, Sbal, VhBal = np.linalg.svd(Jbal) + if np.min(Sbal) < 4.5E-2: + pJbal = robot.get_damped_least_squares_inverse(Jbal, 4.5E-2) + else: + pJbal = robot.get_damped_least_squares_inverse(Jbal, 1E-8) + Nbal = np.eye(Jbal.shape[1]) - np.dot(pJbal, Jbal) + + # Compute balancing task errors + dx_com = CoMr - CoMt # CoM error + + if robot.name == 'centauro': + dx_l1f = xref_l1f - xt_l1f # Left foot position error + dx_r1f = xref_r1f - xt_r1f # Right foot position error + dx_l2f = xref_l2f - xt_l2f # Left foot position error + dx_r2f = xref_r2f - xt_r2f # Right foot position error + dx_bal = np.vstack((dx_l1f.reshape(3, 1), dx_r1f.reshape(3, 1), + dx_l2f.reshape(3, 1), dx_r2f.reshape(3, 1), dx_com.reshape(3, 1))) # Augmented error vector + else: + dx_lf = xref_lf - xt_lf # Left foot position error + dx_rf = xref_rf - xt_rf # Right foot position error + dx_bal = np.vstack((dx_lf.reshape(3, 1), dx_rf.reshape(3, 1), dx_com.reshape(3, 1))) # Augmented error vector + # dx_bal = np.vstack((np.zeros((6, 1)), dx_com.reshape(3, 1))) # Augmented error vector + + # Proportional controller for position + dxref_bal = np.dot(Kbal, dx_bal) + + # Compute desired joint velocities for balancing + dq_bal = np.dot(pJbal, dxref_bal) + dq_bal = dq_bal.reshape((Jbal.shape[1],)) + + # Tracking of CoM velocity manipulability in nullspace + if not robot.has_fixed_base(): + vel_manip = robot.compute_velocity_manipulability_ellipsoid(Jcom[:, 6:]) + else: + vel_manip = robot.compute_velocity_manipulability_ellipsoid(Jcom) + + # Plot current manipulability ellipsoid + if i % 40 == 0: + ellipsoid_id = robot.update_manipulability_ellipsoid(link_id=-1, ellipsoid_id=ellipsoid_id, + ellipsoid=10 * vel_manip[:3, :3], + color=(0.75, 0.1, 0.1, 0.6)) + + # Obtaining joint velocity command + if not robot.has_fixed_base(): + dq_man, minSman, SPDdist = robot.calculate_inverse_differential_kinematics_velocity_manipulability(Jcom[:, 6:], + des_vel_manip, + Km) + # dq_man = np.vstack((np.zeros((6, 1)), dq_man.reshape(num_dofs, 1))) + else: + dq_man, minSman, SPDdist = robot.calculate_inverse_differential_kinematics_velocity_manipulability(Jcom, des_vel_manip, Km) + + # Logging + # Format: [q minEigvalue(Jbal) minEigvalue(Jman) balanceError CurrentManip(1x9) SPDdistance] + log_array[i,] = np.hstack((qt.reshape(1, num_dofs), np.min(Sbal).reshape(1, 1), + minSman.reshape(1, 1), dx_bal.T, vel_manip[0:3, 0:3].reshape(1, vel_manip[0:3, 0:3].size), + SPDdist.reshape(1, 1))) + + # Set joint position + if not robot.has_fixed_base(): + dq_man = np.concatenate((np.zeros((6,)), dq_man)) + dq = dq_bal + np.dot(Nbal, dq_man) + dq = dq[6:, ] + else: + dq = dq_bal + np.dot(Nbal, dq_man) + + q = qt + (dq * dt) + robot.set_joint_positions(q) + + world.step(sleep_dt=dt) + + +# Saving log data +# np.savetxt(robot.name + 'log_Man' + str(num_des_manip) + 'Pos' + str(init_qs) + '.csv', log_array, delimiter=',') + +# Plotting logged data +fig1 = plt.figure(1, figsize=(14, 10)) +# wspace: width reserved for blank space between subplots, hspace: height reserved for white space between subplots +fig1.subplots_adjust(left=0.09, bottom=0.05, right=0.99, wspace=0.2) +plt.suptitle('Robot joints') +plt.rcParams.update({'font.size': 8}) +for i in range(num_dofs): + if robot.name == 'nao': + plt.subplot(6, 7, i+1) # NAO + elif robot.name == 'centauro': + plt.subplot(7, 7, i + 1) # Centauro + + plt.ylabel(robot.get_joint_names(robot.get_joint_ids(i))) + plt.plot(log_array[:, i]) + plt.ylim((-1.5, 1.5)) +# fig1.savefig(robot.name + '_joints_Man' + str(num_des_manip) + 'Pos' + str(init_qs) + '.png', +# bbox_inches='tight', dpi=200) + +fig2 = plt.figure(2, figsize=(14, 10)) +fig2.subplots_adjust(left=0.09, bottom=0.05, right=0.99, wspace=0.2) +plt.rcParams.update({'font.size': 12}) +plt.suptitle('MinEigenvalues, balance and manipulatility errors.') +plt.rcParams.update({'font.size': 8}) +if robot.name == 'nao': + plt.subplot(4, 3, 1) # NAO + plt.ylim((0, 0.05)) # NAO +elif robot.name == 'centauro': + plt.subplot(6, 3, 1) # Centauro + plt.ylim((0, 0.02)) # Centauro + +plt.ylabel('minEigvalue(Jbal)') +plt.plot(log_array[:, num_dofs]) + +if robot.name == 'nao': + plt.subplot(4, 3, 2) # NAO + plt.ylim((0., 0.0001)) # NAO +elif robot.name == 'centauro': + plt.subplot(6, 3, 2) # Centauro + plt.ylim((0., 0.001)) # Centauro + +plt.ylabel('minEigvalue(Jman)') +plt.plot(log_array[:, num_dofs + 1]) + +if robot.name == 'nao': + plt.subplot(4, 3, 3) # NAO + plt.ylim((0., 4.0)) # NAO +elif robot.name == 'centauro': + plt.subplot(6, 3, 3) # Centauro + plt.ylim((0., 5.0)) # Centauro + +plt.ylabel('SPDdist') +plt.plot(log_array[:, -1]) + +for i in range(dx_bal.shape[0]): + if robot.name == 'nao': + plt.subplot(4, 3, i+4) # NAO + elif robot.name == 'centauro': + plt.subplot(6, 3, i + 4) # Centauro + + plt.ylabel('dx_bal'+str(i+1)) + plt.plot(log_array[:, num_dofs + 2 + i]) + plt.ylim((-.05, .05)) +# fig2.savefig(robot.name + '_eigValsAndErrors_Man' + str(num_des_manip) + 'Pos' + str(init_qs) + '.png', +# bbox_inches='tight') + +# plt.tight_layout() +plt.show()