import numpy as np
import pinocchio as pin
import matplotlib.pyplot as plt
import seaborn as sns
from example_robot_data import loadANYmal
import curves
from multicontact_api import ContactSequence
examples_dir = '/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/data/multicontact-api-master-examples/examples/quadrupeds/'
file_name = 'anymal_walk_WB.cs'
cs = ContactSequence()
cs.loadFromBinary(examples_dir + file_name)
q0 = cs.concatenateQtrajectories()(0)
contact_frames = cs.getAllEffectorsInContact()
robot = loadANYmal()
rmodel = robot.model
rdata = robot.data
contact_frame_ids = [rmodel.getFrameId(l) for l in contact_frames]
def compute_joint_jac(robot, q, cf_ids, world_frame=True):
Jlinvel = np.zeros((12, robot.nv))
for i, frame_id in enumerate(cf_ids):
if world_frame:
oTl = robot.framePlacement(q, frame_id, update_kinematics=False)
Jlinvel[3 * i:3 *
(i + 1), :] = oTl.rotation @ robot.computeFrameJacobian(
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)
dt = 1e-3 # discretization timespan
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()