def generate_effector_trajectories_for_sequence_load(cfg, cs, fullBody=None):
cs_ref = ContactSequence()
filename = cfg.REF_FILENAME
print("Load contact sequence with end effector trajectories from : ",
filename)
cs_ref.loadFromBinary(filename)
return cs_ref
def test_step_in_place(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "step_in_place.cs"))
self.assertEqual(cs.size(), 9)
self.assertTrue(cs.haveConsistentContacts())
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
def generate_contact_sequence_load(cfg):
fb, v = display_tools.initScene(cfg.Robot, cfg.ENV_NAME)
cs = ContactSequence(0)
print("Import contact sequence binary file : ", cfg.CS_FILENAME)
cs.loadFromBinary(cfg.CS_FILENAME)
display_tools.displayWBconfig(v, cs.contactPhases[0].q_init)
return cs, fb, v
def test_walk_20cm_quasistatic(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic.cs"))
self.assertEqual(cs.size(), 23)
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
self.assertTrue(cs.haveConsistentContacts())
def generateCentroidalTrajectory(cs,
cs_initGuess=None,
fullBody=None,
viewer=None):
cs_res = ContactSequence(0)
cs_res.loadFromBinary(cfg.COM_FILENAME)
print("Import contact sequence binary file : ", cfg.COM_FILENAME)
return cs_res
def test_com_motion_above_feet_COM(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "com_motion_above_feet_COM.cs"))
self.assertEqual(cs.size(), 1)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
checkCS(self, cs)
def generate_wholebody_load(cfg, cs, fullBody=None, viewer=None):
rp = RosPack()
urdf = rp.get_path(cfg.Robot.packageName) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
logger.info("load robot : %s", urdf)
robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False)
logger.info("robot loaded.")
cs_wb = ContactSequence()
logger.warning("Load wholebody contact sequence from file : %s", cfg.WB_FILENAME)
cs_wb.loadFromBinary(cfg.WB_FILENAME)
return cs_wb, robot
def test_walk_20cm_quasistatic_COM(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_COM.cs"))
self.assertEqual(cs.size(), 23)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
checkCS(self, cs, quasistatic=True, effector=False, wholeBody=False)
def generate_centroidal_load(cfg,
cs,
cs_initGuess=None,
fullBody=None,
viewer=None,
first_iter=True):
cs_res = ContactSequence(0)
cs_res.loadFromBinary(cfg.COM_FILENAME)
logger.warning("Import contact sequence binary file : %s",
cfg.COM_FILENAME)
return cs_res
def test_step_in_place_REF(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "step_in_place_REF.cs"))
self.assertEqual(cs.size(), 9)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories(1e-6, False))
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, root=True, effector=True, wholeBody=False)
def test_com_motion_above_feet_WB(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "com_motion_above_feet_WB.cs"))
self.assertEqual(cs.size(), 1)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveJointsTrajectories())
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
checkCS(self, cs, wholeBody=True)
def test_walk_20cm_quasistatic_WB(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_WB.cs"))
self.assertEqual(cs.size(), 23)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories(1e-1))
self.assertTrue(cs.haveJointsTrajectories())
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True)
# file_name = 'sinY_trunk.cs'
# file_name = 'sinY_notrunk.cs'
file_name = 'talos_sin_traj_R3SO3.cs'
# file_name = 'talos_contact_switch_R3SO3.cs'
# INTTYPE = 'preint'
INTTYPE = 'direct'
# INTTYPE = 'directSE3'
# INTTYPE = 'pinocchio'
PATH = 'figs_'+INTTYPE+'/'
cs = ContactSequence()
print('Loadding cs file: ', examples_dir + file_name)
cs.loadFromBinary(examples_dir + file_name)
q_traj = cs.concatenateQtrajectories()
dq_traj = cs.concatenateDQtrajectories()
ddq_traj = cs.concatenateDDQtrajectories()
c_traj = cs.concatenateCtrajectories()
dc_traj = cs.concatenateDCtrajectories()
Lc_traj = cs.concatenateLtrajectories()
contact_frames = cs.getAllEffectorsInContact()
f_traj_lst = [cs.concatenateContactForceTrajectories(l) for l in contact_frames]
print(contact_frames)
min_ts = q_traj.min()
max_ts = q_traj.max()
print('traj dur (s): ', max_ts - min_ts)
subprocess.run(["killall", "gepetto-gui"])
process_viewer = subprocess.Popen("gepetto-gui",
stdout=subprocess.PIPE,
stderr=subprocess.DEVNULL,
preexec_fn=os.setpgrp)
atexit.register(process_viewer.kill)
# Load robot model in pinocchio
rp = RosPack()
urdf = rp.get_path(robot_package_name) + '/urdf/' + urdf_name + '.urdf'
robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer())
robot.initDisplay(loadModel=True)
robot.displayCollisions(False)
robot.displayVisuals(True)
# Load environment model
cl = gepetto.corbaserver.Client()
gui = cl.gui
env_package_path = rp.get_path(env_package_name)
env_urdf_path = env_package_path + '/urdf/' + env_name + '.urdf'
gui.addUrdfObjects(scene_name + "/environments", env_urdf_path, True)
# Load the motion from the multicontact-api file
cs = ContactSequence()
cs.loadFromBinary(cs_name)
assert cs.haveJointsTrajectories(
), "The file loaded do not have joint trajectories stored."
q_t = cs.concatenateQtrajectories()
display_wb(robot, q_t)
import mlp.viewer.display_tools as display_tools
from pinocchio import SE3
from numpy import array
from mlp.utils.cs_tools import walk
from talos_rbprm.talos import Robot # change robot here
multicontact_api.switchToNumpyArray()
ENV_NAME = "multicontact/plateforme_surfaces_noscale"
fb, v = display_tools.initScene(Robot, ENV_NAME, False)
gui = v.client.gui
sceneName = v.sceneName
cs = ContactSequence(0)
cs_platforms = ContactSequence(0)
cs_platforms.loadFromBinary("talos_plateformes.cs")
p0_platform = cs_platforms.contactPhases[0]
q = [ 0.0,
0.0,
1.02127,
0.0,
0.0,
0.0,
1., # Free flyer
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
-0.001708, # Left Leg
cs_file_dir = '/home/mfourmy/Documents/Phd_LAAS/data/trajs/'
# cs_file_name = 'solo_nomove.cs'
# cs_file_name = 'solo_sin_y_notrunk.cs'
# cs_file_name = 'solo_sin_y_trunk.cs'
# cs_file_name = 'solo_sin_traj.cs'
# cs_file_name = 'solo_sin_rp+y.cs'
cs_file_name = 'solo_stamping.cs'
# cs_file_name = 'solo_stamping_nofeet.cs'
cs_file_path = cs_file_dir + cs_file_name
cs_file_pyb_name = cs_file_name.split('.')[0] + '_Pyb'
if NOFEET:
cs_file_pyb_name += '_Nofeet'
cs = ContactSequence()
cs.loadFromBinary(cs_file_path)
q_traj = cs.concatenateQtrajectories()
dq_traj = cs.concatenateDQtrajectories()
tau_traj = cs.concatenateTauTrajectories()
# ee_names = cs.getAllEffectorsInContact() # not taken into account
q_arr = []
v_arr = []
tau_arr = []
t = q_traj.min()
while t < q_traj.max():
q_arr.append(q_traj(t))
v_arr.append(dq_traj(t))
tau_arr.append(tau_traj(t))
t += DT
q_arr = np.array(q_arr)
v_arr = np.array(v_arr)
def mcapi_playback(name_interface, filename):
device = Solo12(name_interface,dt=DT)
qc = QualisysClient(ip="140.93.16.160", body_id=0)
logger = Logger(device, qualisys=qc)
nb_motors = device.nb_motors
q_viewer = np.array((7 + nb_motors) * [0.,])
# Load contactsequence from file:
cs = ContactSequence(0)
cs.loadFromBinary(filename)
# extract (q, dq, tau) trajectories:
q_t = cs.concatenateQtrajectories() # with the freeflyer configuration
dq_t = cs.concatenateDQtrajectories() # with the freeflyer configuration
ddq_t = cs.concatenateDDQtrajectories() # with the freeflyer configuration
tau_t = cs.concatenateTauTrajectories() # joints torques
# Get time interval from planning:
t_min = q_t.min()
t_max = q_t.max()
print("## Complete duration of the motion loaded: ", t_max - t_min)
# Sanity checks:
assert t_min < t_max
assert dq_t.min() == t_min
assert ddq_t.min() == t_min
assert tau_t.min() == t_min
assert dq_t.max() == t_max
assert ddq_t.max() == t_max
assert tau_t.max() == t_max
assert q_t.dim() == 19
assert dq_t.dim() == 18
assert ddq_t.dim() == 18
assert tau_t.dim() == 12
num_steps = ceil((t_max - t_min) / DT) + 1
q_mes_t = np.zeros([8, num_steps])
v_mes_t = np.zeros([8, num_steps])
q_des_t = np.zeros([8, num_steps])
v_des_t = np.zeros([8, num_steps])
tau_des_t = np.zeros([8, num_steps])
tau_send_t = np.zeros([8, num_steps])
tau_mesured_t = np.zeros([8, num_steps])
q_init = config_12_to_8(q_t(t_min)[7:])
device.Init(calibrateEncoders=True, q_init=q_init)
t = t_min
put_on_the_floor(device, q_init)
#CONTROL LOOP ***************************************************
t_id = 0
while ((not device.hardware.IsTimeout()) and (t < t_max)):
# q_desired = config_12_to_8(q_t(t)[7:]) # remove freeflyer
# dq_desired = config_12_to_8(dq_t(t)[6:]) # remove freeflyer
# tau_desired = config_12_to_8(tau_t(t))
device.UpdateMeasurment()
# tau = compute_pd(q_desired, dq_desired, tau_desired, device)
# Parameters of the PD controller
KP = 4.
KD = 0.05
KT = 1.
tau_max = 3. * np.ones(12)
# Desired position and velocity for this loop and resulting torques
q_desired, v_desired = test_solo12(t)
pos_error = q_desired.ravel() - actuators_pos[:]
vel_error = v_desired.ravel() - actuators_vel[:]
tau = KP * pos_error + KD * vel_error
tau = 0.0 * np.maximum(np.minimum(tau, tau_max), -tau_max)
# Send desired torques to the robot
device.SetDesiredJointTorque(tau)
# store desired and mesured data for plotting
q_des_t[:, t_id] = q_desired
v_des_t[:, t_id] = dq_desired
q_mes_t[:, t_id] = device.q_mes
v_mes_t[:, t_id] = device.v_mes
tau_des_t[:, t_id] = tau_desired
tau_send_t[:, t_id] = tau
tau_mesured_t[: , t_id] = device.torquesFromCurrentMeasurment
# call logger
# logger.sample(device, qualisys=qc)
device.SendCommand(WaitEndOfCycle=True)
if ((device.cpt % 100) == 0):
device.Print()
q_viewer[3:7] = device.baseOrientation # IMU Attitude
q_viewer[7:] = device.q_mes # Encoders
t += DT
t_id += 1
#****************************************************************
# Whatever happened we send 0 torques to the motors.
device.SetDesiredJointTorque([0]*nb_motors)
device.SendCommand(WaitEndOfCycle=True)
if device.hardware.IsTimeout():
print("Masterboard timeout detected.")
print("Either the masterboard has been shut down or there has been a connection issue with the cable/wifi.")
device.hardware.Stop() # Shut down the interface between the computer and the master board
# Save the logs of the Logger object
# logger.saveAll()
# Plot the results
times = np.arange(t_min, t_max + DT, DT)
plot_mes_des_curve(times, q_mes_t, q_des_t, "Joints positions", "joint position")
plot_mes_des_curve(times, v_mes_t, v_des_t, "Joints velocities", "joint velocity")
plot_mes_des_curve(times, tau_send_t, tau_des_t, "Joints torque", "Nm")
current_t = np.zeros([8, num_steps])
for motor in range(device.nb_motors):
current_t[device.motorToUrdf[motor], :] = tau_send_t[device.motorToUrdf[motor], :] / device.jointKtSigned[motor]
plot_mes_des_curve(times, current_t, title="Motor current", y_label="A")
tracking_pos_error = q_des_t - q_mes_t
plot_mes_des_curve(times, tracking_pos_error, title="Tracking error")
plot_mes_des_curve(times, tau_mesured_t, title="Torque mesured from current", y_label="nM")
current_mesured_t = np.zeros([8, num_steps])
for motor in range(device.nb_motors):
current_mesured_t[device.motorToUrdf[motor], :] = tau_mesured_t[device.motorToUrdf[motor], :] / device.jointKtSigned[motor]
plot_mes_des_curve(times, current_mesured_t, title="Measured motor current", y_label="A")
plt.show()