def example_script(name_interface):
slider_box = SliderBox()
device = Solo12(name_interface,dt=0.001)
nb_motors = device.nb_motors
device.init(calibrateEncoders=True)
#CONTROL LOOP ***************************************************
while ((not device.hardware.IsTimeout()) and (clock() < 200) and not slider_box.get_e_stop()):
device.UpdateMeasurment()
device.SetDesiredJointTorque([0]*nb_motors)
device.SendCommand(WaitEndOfCycle=True)
if ((device.cpt % 100) == 0):
device.Print()
#****************************************************************
# 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
def example_script(name_interface):
# making code compatible with python 2 and 3
if hasattr(builtins, "raw_input"):
input = builtins.raw_input
else:
input = builtins.input
global init_done, calibration_done
# setting up the callback on space keystroke
keyboard.on_press_key('space', return_callback)
# Motors not being calibrated will be held at position 0
q_ref = 0
v_ref = 0
kp = 0.125 # Proportional gain
kd = 0.0025 # Derivative gain
# Change this with your robot
device = Solo12(name_interface, dt=0.001)
nb_motors = device.nb_motors
print("")
print(
"Enter a binary number that represents the motors you want to calibrate."
)
print("LSB: first motor, MSB: last motor")
print(
"Example with 8 motors: input \"10011010\" will calibrate motor 1, 3, 4, 7."
)
print("")
try:
bin_motors = int(input(), 2)
# getting binary representation of motors to be calibrated from user
print("")
except:
print("")
raise ValueError("Wrong input received, exiting.")
if bin_motors == 0:
raise ValueError("0 received, no calibration to be done, exiting.")
elif bin_motors < 0 or bin_motors >= (1 << nb_motors):
raise ValueError(
"An invalid number for your configuration has been received, no calibration to be done, exiting."
)
not_to_calibrate = []
for i in range(nb_motors):
if ((bin_motors & (1 << device.motorToUrdf[i])) >>
device.motorToUrdf[i]): # if motor needs to be calibrated
device.encoderOffsets[i] = 0
# set offset to zero
else:
not_to_calibrate.append(device.motorToUrdf[i])
device.Init(
calibrateEncoders=True) # finding encoder index happens in here
init_done = True
if not device.hardware.IsTimeout():
print("")
print("Index detected!")
print("Please move the motors to calibrate to the desired zero.")
print("Press the SPACE key when done...")
# --- Control loop ---
while ((not device.hardware.IsTimeout()) and not calibration_done):
device.UpdateMeasurment()
torques = [0] * nb_motors
for i in not_to_calibrate:
q_err = q_ref - device.q_mes[i] # Position error
v_err = v_ref - device.v_mes[i] # Velocity error
torques[i] = kp * q_err + kd * v_err
device.SetDesiredJointTorque(torques)
device.SendCommand(WaitEndOfCycle=True)
# ---
if calibration_done:
device.SetDesiredJointTorque([0] * nb_motors)
device.SendCommand(WaitEndOfCycle=True)
print("")
print("Calibration done.")
print(
"Please paste the following value into your RobotHAL file after \"self.encoderOffsets = \":"
)
print("")
print("- np.array({})".format([(device.hardware.GetMotor(i).GetPosition() if (bin_motors & (1 << device.motorToUrdf[i])) >> device.motorToUrdf[i] else - device.encoderOffsets[i]) \
for i in range(nb_motors)]))
elif device.hardware.IsTimeout():
print("")
print("Interface timed out, rerun calibration.")
print("")
device.hardware.Stop(
) # Shut down the interface between the computer and the master board
def mcapi_playback(name_interface):
"""Main function that calibrates the robot, get it into a default waiting position then launch
the main control loop once the user has pressed the Enter key
Args:
name_interface (string): name of the interface that is used to communicate with the robot
"""
#########################################
# PARAMETERS OF THE MPC-TSID CONTROLLER #
#########################################
envID = 0 # Identifier of the environment to choose in which one the simulation will happen
velID = 0 # Identifier of the reference velocity profile to choose which one will be sent to the robot
dt_tsid = 0.0020 # Time step of TSID
dt_mpc = 0.02 # Time step of the MPC
k_mpc = int(
dt_mpc /
dt_tsid) # dt is dt_tsid, defined in the TSID controller script
t = 0.0 # Time
n_periods = 1 # Number of periods in the prediction horizon
T_gait = 0.64 # Duration of one gait period
N_SIMULATION = 50000 # number of simulated TSID time steps
# Which MPC solver you want to use
# True to have PA's MPC, to False to have Thomas's MPC
type_MPC = True
# Whether PyBullet feedback is enabled or not
pyb_feedback = False
# Whether we are working with solo8 or not
on_solo8 = False
# If True the ground is flat, otherwise it has bumps
use_flat_plane = True
# If we are using a predefined reference velocity (True) or a joystick (False)
predefined_vel = False
# Enable or disable PyBullet GUI
enable_pyb_GUI = True
# Default position after calibration
q_init = np.array(
[0.0, 0.8, -1.6, 0, 0.8, -1.6, 0, -0.8, 1.6, 0, -0.8, 1.6])
# Run a scenario and retrieve data thanks to the logger
controller = Controller(q_init, envID, velID, dt_tsid, dt_mpc, k_mpc, t,
n_periods, T_gait, N_SIMULATION, type_MPC,
pyb_feedback, on_solo8, use_flat_plane,
predefined_vel, enable_pyb_GUI)
####
if SIMULATION:
device = PyBulletSimulator()
qc = None
else:
device = Solo12(name_interface, dt=DT)
qc = QualisysClient(ip="140.93.16.160", body_id=0)
if LOGGING:
logger = Logger(device, qualisys=qc, logSize=N_SIMULATION)
# Number of motors
nb_motors = device.nb_motors
q_viewer = np.array((7 + nb_motors) * [
0.,
])
# Initiate communication with the device and calibrate encoders
if SIMULATION:
device.Init(calibrateEncoders=True,
q_init=q_init,
envID=envID,
use_flat_plane=use_flat_plane,
enable_pyb_GUI=enable_pyb_GUI,
dt=dt_tsid)
else:
device.Init(calibrateEncoders=True, q_init=q_init)
# Wait for Enter input before starting the control loop
put_on_the_floor(device, q_init)
# CONTROL LOOP ***************************************************
t = 0.0
t_max = (N_SIMULATION - 2) * dt_tsid
while ((not device.hardware.IsTimeout()) and (t < t_max)):
device.UpdateMeasurment() # Retrieve data from IMU and Motion capture
# Desired torques
tau = controller.compute(device)
# print(tau[0:3].ravel())
tau = tau.ravel()
# Set desired torques for the actuators
device.SetDesiredJointTorque(tau)
# Call logger
if LOGGING:
logger.sample(device, qualisys=qc)
# Send command to the robot
device.SendCommand(WaitEndOfCycle=True)
if ((device.cpt % 1000) == 0):
device.Print()
t += DT
# ****************************************************************
# Stop MPC running in a parallel process
if controller.enable_multiprocessing:
print("Stopping parallel process")
controller.mpc_wrapper.stop_parallel_loop()
# 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
if LOGGING:
logger.saveAll()
print("Log saved")
if SIMULATION and enable_pyb_GUI:
# Disconnect the PyBullet server (also close the GUI)
device.Stop()
print("End of script")
quit()
def mcapi_playback(name_interface):
"""Main function that calibrates the robot, get it into a default waiting position then launch
the main control loop once the user has pressed the Enter key
Args:
name_interface (string): name of the interface that is used to communicate with the robot
"""
if SIMULATION:
device = PyBulletSimulator()
qc = None
else:
device = Solo12(name_interface, dt=DT)
qc = QualisysClient(ip="140.93.16.160", body_id=0)
if LOGGING:
logger = Logger(device, qualisys=qc)
# Number of motors
nb_motors = device.nb_motors
q_viewer = np.array((7 + nb_motors) * [
0.,
])
# Gepetto-gui
v = viewerClient()
v.display(q_viewer)
# PyBullet GUI
enable_pyb_GUI = True
# Maximum duration of the demonstration
t_max = 300.0
# Default position after calibration
q_init = np.array([0, 0.8, -1.6, 0, 0.8, -1.6, 0, -0.8, 1.6, 0, -0.8, 1.6])
# Create Estimator object
estimator = Estimator(DT, np.int(t_max / DT))
# Set the paths where the urdf and srdf file of the robot are registered
modelPath = "/opt/openrobots/share/example-robot-data/robots"
urdf = modelPath + "/solo_description/robots/solo12.urdf"
vector = pin.StdVec_StdString()
vector.extend(item for item in modelPath)
# Create the robot wrapper from the urdf model (which has no free flyer) and add a free flyer
robot = tsid.RobotWrapper(urdf, vector, pin.JointModelFreeFlyer(), False)
model = robot.model()
# Creation of the Invverse Dynamics HQP problem using the robot
# accelerations (base + joints) and the contact forces
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
# Compute the problem data with a solver based on EiQuadProg
invdyn.computeProblemData(0.0, np.hstack((np.zeros(7), q_init)),
np.zeros(18))
# Initiate communication with the device and calibrate encoders
if SIMULATION:
device.Init(calibrateEncoders=True,
q_init=q_init,
envID=0,
use_flat_plane=True,
enable_pyb_GUI=enable_pyb_GUI,
dt=DT)
else:
device.Init(calibrateEncoders=True, q_init=q_init)
# Wait for Enter input before starting the control loop
put_on_the_floor(device, q_init)
# CONTROL LOOP ***************************************************
t = 0.0
k = 0
while ((not device.hardware.IsTimeout()) and (t < t_max)):
device.UpdateMeasurment() # Retrieve data from IMU and Motion capture
# Run estimator with hind left leg touching the ground
estimator.run_filter(k, np.array([0, 0, 1, 0]), device, invdyn.data(),
model)
# Zero desired torques
tau = np.zeros(12)
# Set desired torques for the actuators
device.SetDesiredJointTorque(tau)
# Call logger
if LOGGING:
logger.sample(device, qualisys=qc, estimator=estimator)
# Send command to the robot
device.SendCommand(WaitEndOfCycle=True)
if ((device.cpt % 100) == 0):
device.Print()
# Gepetto GUI
if k > 0:
pos = np.array(estimator.data.oMf[26].translation).ravel()
q_viewer[0:3] = np.array([-pos[0], -pos[1],
estimator.FK_h]) # Position
q_viewer[3:7] = estimator.q_FK[3:7, 0] # Orientation
q_viewer[7:] = estimator.q_FK[7:, 0] # Encoders
v.display(q_viewer)
t += DT
k += 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."
)
# Shut down the interface between the computer and the master board
device.hardware.Stop()
# Save the logs of the Logger object
if LOGGING:
logger.saveAll()
if SIMULATION and enable_pyb_GUI:
# Disconnect the PyBullet server (also close the GUI)
device.Stop()
print("End of script")
quit()
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()
def mcapi_playback(name_interface):
"""Main function that calibrates the robot, get it into a default waiting position then launch
the main control loop once the user has pressed the Enter key
Args:
name_interface (string): name of the interface that is used to communicate with the robot
"""
device = Solo12(name_interface, dt=DT)
nb_motors = device.nb_motors
q_viewer = np.array((7 + nb_motors) * [
0.,
])
v = viewerClient()
v.display(q_viewer)
# Default position after calibration
q_init = np.zeros(12)
# Calibrate encoders
device.Init(calibrateEncoders=True, q_init=q_init)
# Wait for Enter input before starting the control loop
put_on_the_floor(device, q_init)
# CONTROL LOOP ***************************************************
t = 0.0
t_max = 300.0
while ((not device.hardware.IsTimeout()) and (t < t_max)):
device.UpdateMeasurment() # Retrieve data from IMU and Motion capture
# Zero desired torques
tau = np.zeros(12)
# Set desired torques for the actuators
device.SetDesiredJointTorque(tau)
# Send command to the robot
device.SendCommand(WaitEndOfCycle=True)
if ((device.cpt % 100) == 0):
device.Print()
# Gepetto GUI
q_viewer[3:7] = device.baseOrientation # IMU Attitude
q_viewer[7:] = device.q_mes # Encoders
q_viewer[2] = 0.5
v.display(q_viewer)
t += DT
# ****************************************************************
# 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
def mcapi_playback(name_interface):
"""Main function that calibrates the robot, get it into a default waiting position then launch
the main control loop once the user has pressed the Enter key
Args:
name_interface (string): name of the interface that is used to communicate with the robot
"""
device = Solo12(name_interface, dt=DT)
# qc = QualisysClient(ip="140.93.16.160", body_id=0) # QualisysClient
# logger = Logger(device, qualisys=qc) # Logger object
nb_motors = device.nb_motors
# Default position after calibration
q_init = np.array(
[0.0, 0.8, -1.6, 0, 0.8, -1.6, 0, -0.8, 1.6, 0, -0.8, 1.6])
# Calibrate encoders
device.Init(calibrateEncoders=True, q_init=q_init)
# Wait for Enter input before starting the control loop
put_on_the_floor(device, q_init)
# CONTROL LOOP ***************************************************
t = 0.0
t_max = t_switch[-1]
# Parameters of the PD controller
KP = 2.
KD = 0.05
tau_max = 5. * np.ones(12)
while ((not device.hardware.IsTimeout()) and (t < t_max)):
device.UpdateMeasurment() # Retrieve data from IMU and Motion capture
# Desired position and velocity for this loop and resulting torques
q_desired, v_desired = demo_solo12(t)
pos_error = q_desired.ravel() - device.q_mes.ravel()
vel_error = v_desired.ravel() - device.v_mes.ravel()
tau = KP * pos_error + KD * vel_error
tau = np.maximum(np.minimum(tau, tau_max), -tau_max)
# Set desired torques for the actuators
device.SetDesiredJointTorque(tau)
# Call logger
# logger.sample(device, qualisys=qc)
# Send command to the robot
device.SendCommand(WaitEndOfCycle=True)
if ((device.cpt % 100) == 0):
device.Print()
t += DT
# ****************************************************************
# 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
def mcapi_playback(name_interface):
"""Main function that calibrates the robot, get it into a default waiting position then launch
the main control loop once the user has pressed the Enter key
Args:
name_interface (string): name of the interface that is used to communicate with the robot
"""
name_replay = "/home/odri/git/thomasCbrs/log_eval/test_3/06_nl/"
# name_replay = "/home/odri/git/thomasCbrs/log_eval/vmax_nl/"
# replay_q = np.loadtxt(name_replay + "_q.dat", delimiter=" ")
# replay_v = np.loadtxt(name_replay + "_v.dat", delimiter=" ")
# replay_tau = np.loadtxt(name_replay + "_tau.dat", delimiter=" ")
qtsid_full = np.load(name_replay + "qtsid.npy" , allow_pickle = True)
vtsid_full = np.load(name_replay + "vtsid.npy" , allow_pickle = True)
tau_ff = np.load(name_replay + "torques_ff.npy" , allow_pickle = True)
replay_q = qtsid_full[7:,:].transpose()
replay_v = vtsid_full[6:,:].transpose()
replay_tau = tau_ff.transpose()
N_SIMULATION = replay_q.shape[0]
# Default position after calibration
# q_init = replay_q[0, 1:]
q_init = replay_q[0, :]
if SIMULATION:
device = PyBulletSimulator()
qc = None
else:
device = Solo12(name_interface, dt=DT)
qc = QualisysClient(ip="140.93.16.160", body_id=0)
if LOGGING:
logger = Logger(device, qualisys=qc, logSize=N_SIMULATION)
# Number of motors
nb_motors = device.nb_motors
# Initiate communication with the device and calibrate encoders
if SIMULATION:
device.Init(calibrateEncoders=True, q_init=q_init, envID=0,
use_flat_plane=True, enable_pyb_GUI=True, dt=DT)
else:
device.Init(calibrateEncoders=True, q_init=q_init)
# Wait for Enter input before starting the control loop
put_on_the_floor(device, q_init)
# CONTROL LOOP ***************************************************
t = 0.0
t_max = (N_SIMULATION-1) * DT
i = 0
P = 7 * np.ones(12)
D = 0.5 * np.ones(12)
q_des = np.zeros(12)
v_des = np.zeros(12)
tau_ff = np.zeros(12)
while ((not device.hardware.IsTimeout()) and (t < t_max)):
device.UpdateMeasurment() # Retrieve data from IMU and Motion capture
# Set desired quantities for the actuators
device.SetDesiredJointPDgains(P, D)
# device.SetDesiredJointPosition(replay_q[i, 1:])
# device.SetDesiredJointVelocity(replay_v[i, 1:])
# device.SetDesiredJointTorque(replay_tau[i, 1:])
device.SetDesiredJointPosition(replay_q[i, :])
device.SetDesiredJointVelocity(replay_v[i, :])
device.SetDesiredJointTorque(replay_tau[i, :])
# Call logger
if LOGGING:
logger.sample(device, qualisys=qc)
# Send command to the robot
device.SendCommand(WaitEndOfCycle=True)
if ((device.cpt % 1000) == 0):
device.Print()
t += DT
i += 1
# DAMPING TO GET ON THE GROUND PROGRESSIVELY *********************
t = 0.0
t_max = 2.5
while ((not device.hardware.IsTimeout()) and (t < t_max)):
device.UpdateMeasurment() # Retrieve data from IMU and Motion capture
# Set desired quantities for the actuators
device.SetDesiredJointPDgains(np.zeros(12), 0.1 * np.ones(12))
device.SetDesiredJointPosition(np.zeros(12))
device.SetDesiredJointVelocity(np.zeros(12))
device.SetDesiredJointTorque(np.zeros(12))
# Send command to the robot
device.SendCommand(WaitEndOfCycle=True)
if ((device.cpt % 1000) == 0):
device.Print()
t += DT
# FINAL SHUTDOWN *************************************************
# 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
if LOGGING:
logger.saveAll()
print("Log saved")
if SIMULATION:
# Disconnect the PyBullet server (also close the GUI)
device.Stop()
print("End of script")
quit()
def example_script(name_interface, legs_clib_path, shd_clib_path):
device = Solo12(name_interface, dt=0.001)
nb_motors = device.nb_motors
LOGGING = False
VIEWER = False
qc = None
if LOGGING:
# Initialize logger
qc = QualisysClient(ip="140.93.16.160", body_id=0) # ??
logger = Logger(device, qualisys=qc, logSize=50000)
#### Set ref. traj. PD parameters
ref_traj_KP = 0
ref_traj_KV = 0
active_dof = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
#### Set collision avoidance parameters
legs_threshold = 0.05
legs_kp = 20.
legs_kv = 0.0
nb_legs_pairs = 20
#### Shoulder collision parameters
shd_threshold = 0.2
shd_kp = 3.
shd_kv = 0.
#### Reference traj. parameters
q_ref_list = '###.npy'
dq_ref_list = '###.npy'
traj_KP = 1 * np.ones(12)
traj_KP[:] = 0.
traj_KV = 0 * np.ones(12)
q_init = q_ref_list[0][7:]
traj_counter = 0
### Emergency behavior switches
q_bounds = [-4, 4]
vq_max = 20.0
tau_q_max = 1.0
# Load the specified compiled C library
cCollFun = CDLL(legs_clib_path)
nnCCollFun = CDLL(shd_clib_path)
# Initialize emergency behavior trigger var.
emergencyFlag = 0
# Initialize viewer
if VIEWER:
viewer_coll = viewerClient(
nb_legs_pairs,
3,
legs_threshold,
shd_threshold,
urdf=
"/home/ada/git/tnoel/solopython/coll_avoidance_modules/urdf/solo12_simplified.urdf",
modelPath=
"/home/ada/git/tnoel/solopython/coll_avoidance_modules/urdf")
device.Init(calibrateEncoders=True, q_init=q_init)
put_on_the_floor(device, q_init)
#CONTROL LOOP ***************************************************
tau_q = np.zeros(nb_motors)
tau_PD = np.zeros(nb_motors)
while ((not device.hardware.IsTimeout()) and (clock() < 120)
and emergencyFlag == 0):
device.UpdateMeasurment()
tau_q[:] = 0.
tau_PD[:] = 0.
# Compute PD to follow reference traj.
curr_q_ref = q_ref_list[traj_counter][7:]
curr_dq_ref = dq_ref_list[traj_counter][6:]
tau_PD = compute_pd(q_desired, v_desired, KP, KD, device)
traj_counter += 1
# Compute collisions distances and jacobians from the C lib.
c_results = getLegsCollisionsResults(device.q_mes,
cCollFun,
nb_motors,
nb_legs_pairs,
witnessPoints=True)
c_dist_legs = getLegsDistances(c_results,
nb_motors,
nb_legs_pairs,
witnessPoints=True)
c_Jlegs = getLegsJacobians(c_results,
nb_motors,
nb_legs_pairs,
witnessPoints=True)
c_wPoints = getLegsWitnessPoints(c_results, nb_motors, nb_legs_pairs)
### Get results from C generated code (shoulder neural net)
#c_shd_dist, c_shd_jac = getAllShouldersCollisionsResults(device.q_mes, nnCCollFun, 2, offset=0.08) # 2D neural net
c_shd_dist, c_shd_jac = getAllShouldersCollisionsResults(
device.q_mes, nnCCollFun, 3,
offset=0.11) #offset with 3 inputs: 0.18 (small), 0.11 (large)"
# Compute collision avoidance torque
tau_legs = computeRepulsiveTorque(device.q_mes,
device.v_mes,
c_dist_legs,
c_Jlegs,
legs_threshold,
legs_kp,
legs_kv,
opposeJacIfNegDist=True)
tau_shd = computeRepulsiveTorque(device.q_mes,
device.v_mes,
c_shd_dist,
c_shd_jac,
shd_threshold,
shd_kp,
shd_kv,
opposeJacIfNegDist=False)
tau_q = 1 * tau_legs + 1 * tau_shd
# Set the computed torque as command
tau_command = tau_q + tau_PD
device.SetDesiredJointTorque(0 * tau_command)
# Check the condition for triggering emergency behavior
emergencyFlag = max(
emergencyFlag,
emergencyCondition(device.q_mes, device.v_mes, tau_command,
q_bounds, vq_max, tau_q_max))
# Call logger
if LOGGING:
logger.sample(device, qualisys=qc)
if VIEWER:
viewer_coll.display(
np.concatenate(([0, 0, 0, 0, 0, 0, 0], device.q_mes)),
c_dist_legs, c_shd_dist, c_wPoints, tau_legs, tau_shd)
device.SendCommand(WaitEndOfCycle=True)
if ((device.cpt % 100) == 0):
device.Print()
print('Avoid. torque')
print(tau_q)
print('PD torque')
print(tau_PD)
#****************************************************************
print("Emergency : {}".format(emergencyFlag))
# Whatever happened we send 0 torques to the motors.
device.SetDesiredJointTorque([0] * nb_motors)
device.SendCommand(WaitEndOfCycle=True)
# Save the logs of the Logger object
if LOGGING:
logger.saveAll()
print("Log saved")
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