From 23aa85b14d71fc0d94fcf05f546fe4aa67fb2ea3 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Tue, 6 Oct 2020 16:18:01 +0200
Subject: [PATCH] Add new control scripts that have to be called from
solopython repo
---
scripts_solopython/demo_estimator_solo12.py | 220 +++++++++++++++
scripts_solopython/main_mcapi_solo8.py | 279 ++++++++++++++++++++
scripts_solopython/main_solo12_openloop.py | 226 ++++++++++++++++
3 files changed, 725 insertions(+)
create mode 100644 scripts_solopython/demo_estimator_solo12.py
create mode 100644 scripts_solopython/main_mcapi_solo8.py
create mode 100644 scripts_solopython/main_solo12_openloop.py
diff --git a/scripts_solopython/demo_estimator_solo12.py b/scripts_solopython/demo_estimator_solo12.py
new file mode 100644
index 00000000..a7807ada
--- /dev/null
+++ b/scripts_solopython/demo_estimator_solo12.py
@@ -0,0 +1,220 @@
+# coding: utf8
+
+from utils.logger import Logger
+import tsid as tsid
+import pinocchio as pin
+import argparse
+import numpy as np
+from mpctsid.Estimator import Estimator
+from utils.viewerClient import viewerClient, NonBlockingViewerFromRobot
+import os
+import sys
+sys.path.insert(0, './mpctsid')
+
+SIMULATION = True
+LOGGING = False
+
+if SIMULATION:
+ from mpctsid.utils_mpc import PyBulletSimulator
+else:
+ from pynput import keyboard
+ from solo12 import Solo12
+ from utils.qualisysClient import QualisysClient
+
+DT = 0.002
+
+key_pressed = False
+
+def on_press(key):
+ """Wait for a specific key press on the keyboard
+
+ Args:
+ key (keyboard.Key): the key we want to wait for
+ """
+ global key_pressed
+ try:
+ if key == keyboard.Key.enter:
+ key_pressed = True
+ # Stop listener
+ return False
+ except AttributeError:
+ print('Unknown key {0} pressed'.format(key))
+
+
+def put_on_the_floor(device, q_init):
+ """Make the robot go to the default initial position and wait for the user
+ to press the Enter key to start the main control loop
+
+ Args:
+ device (robot wrapper): a wrapper to communicate with the robot
+ q_init (array): the default position of the robot
+ """
+ global key_pressed
+ key_pressed = False
+ Kp_pos = 3.
+ Kd_pos = 0.01
+ imax = 3.0
+ pos = np.zeros(device.nb_motors)
+ for motor in range(device.nb_motors):
+ pos[motor] = q_init[device.motorToUrdf[motor]] * \
+ device.gearRatioSigned[motor]
+ listener = keyboard.Listener(on_press=on_press)
+ listener.start()
+ print("Put the robot on the floor and press Enter")
+ while not key_pressed:
+ device.UpdateMeasurment()
+ for motor in range(device.nb_motors):
+ ref = Kp_pos*(pos[motor] - device.hardware.GetMotor(motor).GetPosition() -
+ Kd_pos*device.hardware.GetMotor(motor).GetVelocity())
+ ref = min(imax, max(-imax, ref))
+ device.hardware.GetMotor(motor).SetCurrentReference(ref)
+ device.SendCommand(WaitEndOfCycle=True)
+
+ print("Start the motion.")
+
+
+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 main():
+ """Main function
+ """
+
+ parser = argparse.ArgumentParser(
+ description='Playback trajectory to show the extent of solo12 workspace.')
+ parser.add_argument('-i',
+ '--interface',
+ required=True,
+ help='Name of the interface (use ifconfig in a terminal), for instance "enp1s0"')
+
+ mcapi_playback(parser.parse_args().interface)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/scripts_solopython/main_mcapi_solo8.py b/scripts_solopython/main_mcapi_solo8.py
new file mode 100644
index 00000000..a5900286
--- /dev/null
+++ b/scripts_solopython/main_mcapi_solo8.py
@@ -0,0 +1,279 @@
+# coding: utf8
+import numpy as np
+import argparse
+import math
+from time import clock, sleep
+from solo12 import Solo12
+from pynput import keyboard
+
+from utils.logger import Logger
+from utils.qualisysClient import QualisysClient
+import matplotlib.pyplot as plt
+from math import ceil
+import curves
+from multicontact_api import ContactSequence
+curves.switchToNumpyArray()
+DT = 0.001
+KP = 4.
+KD = 0.05
+KT = 1.
+tau_max = 3. * np.ones(8)
+
+key_pressed = False
+
+# Variables for motion range test of Solo12
+t_switch = np.array([0.0, 3.0, 6.0, 9.0, 12.0, 15.0, 18.0, 21.0, 24.0, 27.0, 33.0, 36.0, 39.0, 42.0])
+q1 = 0.66 * np.pi
+q_switch = np.array([[0.0, 0.66 * np.pi, 0.0, 0.66 * np.pi, 0.0, 0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi, 0.0, -0.1 * np.pi, 0.0, 0.22 * np.pi, 0.0, 0.0],
+ [0.8, 0.8, -0.8, -0.8, 0.8, 0.8, 0.0, -0.8, -0.8, -1.4, np.pi*0.5, np.pi*0.5, np.pi*0.5, 0.0],
+ [-1.6, -1.6, 1.6, 1.6, -1.6, -1.6, +1.0, -2.0, -2.0, 2.0, -np.pi, -np.pi*1.4, -np.pi, 0.0]])
+
+def handle_q_v_switch(t):
+
+ i = 1
+ while (i < t_switch.shape[0]) and (t_switch[i] <= t):
+ i += 1
+
+ if (i != t_switch.shape[0]):
+ return apply_q_v_change(t, i)
+ else:
+ N = q_switch.shape[1]
+ return np.tile(q_switch[:, (N-1):N], (4, 1)) * np.array([[1, 1, 1, -1, 1, 1, 1, -1, -1, -1, -1, -1]]).transpose(), np.zeros((12,))
+
+
+def apply_q_v_change(t, i):
+
+ # Position
+ ev = t - t_switch[i-1]
+ t1 = t_switch[i] - t_switch[i-1]
+ A3 = 2 * (q_switch[:, (i-1):i] - q_switch[:, i:(i+1)]) / t1**3
+ A2 = (-3/2) * t1 * A3
+ q = q_switch[:, (i-1):i] + A2*ev**2 + A3*ev**3
+ v = 2 * A2 * ev + 3 * A3 * ev**2
+
+ q = np.tile(q, (4, 1)) * np.array([[1, 1, 1, -1, 1, 1, 1, -1, -1, -1, -1, -1]]).transpose()
+ v = np.tile(v, (4, 1)) * np.array([[1, 1, 1, -1, 1, 1, 1, -1, -1, -1, -1, -1]]).transpose()
+
+ return q, v
+
+def test_solo12(t):
+
+ q = np.array([0, 0.8, -1.6, 0, 0.8, -1.6, 0, -0.8, 1.6, 0, -0.8, 1.6])
+ v = np.zeros((12,))
+
+ q, v = handle_q_v_switch(t)
+
+ return q, v
+
+def config_12_to_8(q12):
+ """
+ Remove the shoulder roll joint from the vector
+ """
+ assert q12.shape[0] == 12
+ return np.hstack((q12[1:3], q12[4:6], q12[7:9], q12[10:12]))
+
+
+def compute_pd(q_desired, v_desired, tau_desired, device):
+ pos_error = q_desired - device.q_mes
+ vel_error = v_desired - device.v_mes
+ tau = KP * pos_error + KD * vel_error + KT * tau_desired
+ tau = np.maximum(np.minimum(tau, tau_max), -tau_max)
+ return tau
+
+def on_press(key):
+ global key_pressed
+ try:
+ if key == keyboard.Key.enter:
+ key_pressed = True
+ # Stop listener
+ return False
+ except AttributeError:
+ print('Unknown key {0} pressed'.format(key))
+
+def put_on_the_floor(device, q_init):
+ global key_pressed
+ key_pressed = False
+ Kp_pos = 3.
+ Kd_pos = 0.01
+ imax = 3.0
+ pos = np.zeros(device.nb_motors)
+ for motor in range(device.nb_motors):
+ pos[motor] = q_init[device.motorToUrdf[motor]] * device.gearRatioSigned[motor]
+ listener = keyboard.Listener(on_press=on_press)
+ listener.start()
+ print("Put the robot on the floor and press Enter")
+ while not key_pressed:
+ device.UpdateMeasurment()
+ for motor in range(device.nb_motors):
+ ref = Kp_pos*(pos[motor] - device.hardware.GetMotor(motor).GetPosition() - Kd_pos*device.hardware.GetMotor(motor).GetVelocity())
+ ref = min(imax, max(-imax, ref))
+ device.hardware.GetMotor(motor).SetCurrentReference(ref)
+ device.SendCommand(WaitEndOfCycle=True)
+
+ print("Start the motion.")
+
+def plot_mes_des_curve(times, mes_t, des_t = None, title = None, y_label = None):
+ colors = ['r', 'b']
+ labels = ["Shoulder" , "Knee"]
+ legs_order = ["front left", "front right", "hind left", "hind right"]
+ # joint positions des/mes
+ fig, ax = plt.subplots(2, 2)
+ fig.canvas.set_window_title(title)
+ fig.suptitle(title, fontsize=20)
+ for i in range(2): # line (front/back)
+ for j in range(2): # column (left/right)
+ ax_sub = ax[i, j]
+ for k in range(2): # joint
+ ax_sub.plot(times, mes_t[i*4 + j*2 + k, :].T, color=colors[k], label=labels[k])
+ if des_t is not None:
+ ax_sub.plot(times, des_t[i*4 + j*2 + k, :].T, color=colors[k], label=labels[k] + "(ref)", linestyle="dashed")
+ ax_sub.set_xlabel('time (s)')
+ ax_sub.set_ylabel(y_label)
+ ax_sub.set_title(legs_order[i*2 + j])
+ ax_sub.legend()
+
+
+
+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 main():
+ parser = argparse.ArgumentParser(description='Playback trajectory stored in a multicontact-api file.')
+ parser.add_argument('-i',
+ '--interface',
+ required=True,
+ help='Name of the interface (use ifconfig in a terminal), for instance "enp1s0"')
+
+ parser.add_argument('-f',
+ '--filename',
+ type=str,
+ required=True,
+ help="The absolute path of a multicontact_api.ContactSequence serialized file")
+
+ mcapi_playback(parser.parse_args().interface, parser.parse_args().filename)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/scripts_solopython/main_solo12_openloop.py b/scripts_solopython/main_solo12_openloop.py
new file mode 100644
index 00000000..7ee6a4ef
--- /dev/null
+++ b/scripts_solopython/main_solo12_openloop.py
@@ -0,0 +1,226 @@
+# coding: utf8
+
+import os
+import sys
+sys.path.insert(0, './mpctsid')
+
+from utils.logger import Logger
+import tsid as tsid
+import pinocchio as pin
+import argparse
+import numpy as np
+from mpctsid.Estimator import Estimator
+from mpctsid.Controller import Controller
+from utils.viewerClient import viewerClient, NonBlockingViewerFromRobot
+
+SIMULATION = True
+LOGGING = False
+
+if SIMULATION:
+ from mpctsid.utils_mpc import PyBulletSimulator
+else:
+ from pynput import keyboard
+ from solo12 import Solo12
+ from utils.qualisysClient import QualisysClient
+
+DT = 0.002
+
+key_pressed = False
+
+
+def on_press(key):
+ """Wait for a specific key press on the keyboard
+
+ Args:
+ key (keyboard.Key): the key we want to wait for
+ """
+ global key_pressed
+ try:
+ if key == keyboard.Key.enter:
+ key_pressed = True
+ # Stop listener
+ return False
+ except AttributeError:
+ print('Unknown key {0} pressed'.format(key))
+
+
+def put_on_the_floor(device, q_init):
+ """Make the robot go to the default initial position and wait for the user
+ to press the Enter key to start the main control loop
+
+ Args:
+ device (robot wrapper): a wrapper to communicate with the robot
+ q_init (array): the default position of the robot
+ """
+ global key_pressed
+ key_pressed = False
+ Kp_pos = 3.
+ Kd_pos = 0.01
+ imax = 3.0
+ pos = np.zeros(device.nb_motors)
+ for motor in range(device.nb_motors):
+ pos[motor] = q_init[device.motorToUrdf[motor]] * device.gearRatioSigned[motor]
+ listener = keyboard.Listener(on_press=on_press)
+ listener.start()
+ print("Put the robot on the floor and press Enter")
+ while not key_pressed:
+ device.UpdateMeasurment()
+ for motor in range(device.nb_motors):
+ ref = Kp_pos*(pos[motor] - device.hardware.GetMotor(motor).GetPosition() -
+ Kd_pos*device.hardware.GetMotor(motor).GetVelocity())
+ ref = min(imax, max(-imax, ref))
+ device.hardware.GetMotor(motor).SetCurrentReference(ref)
+ device.SendCommand(WaitEndOfCycle=True)
+
+ print("Start the motion.")
+
+
+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.SetKp(controller.result.P)
+ device.SetKd(controller.result.D)
+ device.SetQdes(controller.result.q_des)
+ device.SetVdes(controller.result.v_des)
+ device.SetTauFF(controller.result.tau_ff)
+
+ # 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 main():
+ """Main function
+ """
+
+ parser = argparse.ArgumentParser(description='Playback trajectory to show the extent of solo12 workspace.')
+ parser.add_argument('-i',
+ '--interface',
+ required=True,
+ help='Name of the interface (use ifconfig in a terminal), for instance "enp1s0"')
+
+ mcapi_playback(parser.parse_args().interface)
+
+
+if __name__ == "__main__":
+ main()
--
GitLab