Commit 3dd022b9 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[python] Fix a few problems with the Python exercises due to API updates of...

[python] Fix a few problems with the Python exercises due to API updates of example-robot-data and LMPC-walking. Improve plot readibility using shorter labels.
parent f512b6ec
Pipeline #15132 passed with stage
in 16 minutes and 37 seconds
......@@ -126,10 +126,10 @@ if(PLOT_JOINT_POS):
(f, ax) = plut.create_empty_figure(int(robot.nv/2),2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, q[i,:-1], label='Joint pos '+str(i))
ax[i].plot(time, q_ref[i,:], '--', label='Joint ref pos '+str(i))
ax[i].plot(time, q[i,:-1], label='q '+str(i))
ax[i].plot(time, q_ref[i,:], '--', label='q ref '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('Joint angles [rad]')
ax[i].set_ylabel('Q [rad]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
......@@ -137,12 +137,12 @@ if(PLOT_JOINT_VEL):
(f, ax) = plut.create_empty_figure(int(robot.nv/2),2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, v[i,:-1], label='Joint vel '+str(i))
ax[i].plot(time, v_ref[i,:], '--', label='Joint ref vel '+str(i))
ax[i].plot(time, v[i,:-1], label='dq '+str(i))
ax[i].plot(time, v_ref[i,:], '--', label='dq ref '+str(i))
ax[i].plot([time[0], time[-1]], 2*[v_min[i]], ':')
ax[i].plot([time[0], time[-1]], 2*[v_max[i]], ':')
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('Joint velocity [rad/s]')
ax[i].set_ylabel('dq [rad/s]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
......@@ -150,11 +150,11 @@ if(PLOT_JOINT_ACC):
(f, ax) = plut.create_empty_figure(int(robot.nv/2),2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, dv[i,:-1], label='Joint acc '+str(i))
ax[i].plot(time, dv_ref[i,:], '--', label='Joint ref acc '+str(i))
ax[i].plot(time, dv_des[i,:], ':', label='Joint des acc '+str(i))
ax[i].plot(time, dv[i,:-1], label='ddq '+str(i))
ax[i].plot(time, dv_ref[i,:], '--', label='ddq ref '+str(i))
ax[i].plot(time, dv_des[i,:], ':', label='ddq des '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('Joint acceleration [rad/s^2]')
ax[i].set_ylabel('ddq [rad/s^2]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
......
......@@ -99,18 +99,17 @@ max_admissible_cop = cop_ref + np.tile([foot_length/2, foot_width/2], (N,1))
# time vs CoP and CoM in x: 'A.K.A run rabbit run !'
# -------------------------------------------------
plot_utils.plot_x(time, N, min_admissible_CoP,
max_admissible_cop, cop_x, com_state_x, cop_ref)
plot_utils.plot_x(True, time, N, min_admissible_CoP, max_admissible_cop,
cop_x, com_state_x, cop_ref)
# time VS CoP and CoM in y: 'A.K.A what goes up must go down'
# ----------------------------------------------------------
plot_utils.plot_y(time, N, min_admissible_CoP,
max_admissible_cop, cop_y, com_state_y, cop_ref)
plot_utils.plot_y(True, time, N, min_admissible_CoP, max_admissible_cop,
cop_y, com_state_y, cop_ref, 2*max_admissible_cop)
# plot CoP, CoM in x Vs Cop, CoM in y:
# -----------------------------------
plot_utils.plot_xy(time, N, foot_length, foot_width,
cop_ref, cop_x, cop_y, com_state_x, com_state_y)
plot_utils.plot_xy(time, N, foot_length, foot_width, cop_ref, cop_x, cop_y, com_state_x, com_state_y)
import matplotlib.pyplot as plt
plt.gca().set_xlim([cop_ref[0,0]-0.2, cop_ref[-1,0]+0.2])
plt.gca().set_ylim([cop_ref[0,1]-0.2, cop_ref[-1,1]+0.2])
......
......@@ -22,10 +22,12 @@ lf_frame_name = "leg_left_sole_fix_joint" # left foot frame name
contactNormal = np.array([0., 0., 1.]) # direction of the normal to the contact surface
w_com = 1.0 # weight of center of mass task
w_am = 1e-3 # weight of angular momentum task
w_foot = 1e-1 # weight of the foot motion task
w_contact = -1.0 # weight of foot in contact (negative means infinite weight)
w_posture = 1e-1 # weight of joint posture task
w_forceRef = 1e-5 # weight of force regularization task
w_cop = 0.0
w_torque_bounds = 1.0 # weight of the torque bounds
w_joint_bounds = 0.0
......@@ -73,6 +75,7 @@ v_max_scaling = 0.8
kp_contact = 10.0 # proportional gain of contact constraint
kp_foot = 10.0 # proportional gain of contact constraint
kp_com = 10.0 # proportional gain of center of mass task
kp_am = 10.0 # proportional gain of angular momentum task
kp_posture = 1.0 # proportional gain of joint posture task
viewer = pin.visualize.GepettoVisualizer
......
......@@ -45,6 +45,8 @@ EE_SPHERE_COLOR = (1, 0.5, 0, 0.5)
EE_REF_SPHERE_COLOR = (1, 0, 0, 0.5)
from example_robot_data.robots_loader import getModelPath
urdf = "/ur_description/urdf/ur5_robot.urdf"
from os.path import join
urdf = "ur_description/urdf/ur5_robot.urdf"
path = getModelPath(urdf)
urdf = path+urdf
urdf = join(path, urdf)
path = join(path, '../..')
......@@ -45,6 +45,8 @@ EE_SPHERE_COLOR = (1, 0.5, 0, 0.5)
EE_REF_SPHERE_COLOR = (1, 0, 0, 0.5)
from example_robot_data.robots_loader import getModelPath
urdf = "/ur_description/urdf/ur5_robot.urdf"
from os.path import join
urdf = "ur_description/urdf/ur5_robot.urdf"
path = getModelPath(urdf)
urdf = path+urdf
urdf = join(path, urdf)
path = join(path, '../..')
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment