From 30b667aa4e000de9739f8fcbacf0cd546dae9516 Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Thu, 29 Jul 2021 18:22:51 +0200
Subject: [PATCH] Switching to odri_control_interface instead of solopython to
retrieve from and send data to the masterboard
---
include/qrw/Estimator.hpp | 4 +-
scripts/Controller.py | 37 ++++++---
scripts/Estimator.py | 12 +--
scripts/LoggerControl.py | 20 +++++
scripts/LoggerSensors.py | 24 +++---
scripts/config_solo12.yaml | 41 ++++++++++
scripts/main_solo12_control.py | 141 ++++++++++++++++++---------------
scripts/utils_mpc.py | 4 +-
src/Estimator.cpp | 4 +-
9 files changed, 188 insertions(+), 99 deletions(-)
create mode 100644 scripts/config_solo12.yaml
diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index 0a789cb3..8cb1dfd2 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -106,10 +106,10 @@ public:
///
/// \param[in] baseLinearAcceleration Linear acceleration of the IMU (gravity compensated)
/// \param[in] baseAngularVelocity Angular velocity of the IMU
- /// \param[in] baseOrientation Quaternion orientation of the IMU
+ /// \param[in] baseOrientation Euler orientation of the IMU
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity, Vector4 const& baseOrientation);
+ void get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity, Vector3 const& baseOrientation);
////////////////////////////////////////////////////////////////////////////////////////////////
///
diff --git a/scripts/Controller.py b/scripts/Controller.py
index aacc38a7..14e632fc 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -38,6 +38,23 @@ class dummyHardware:
return 0.0
+class dummyIMU:
+ """Fake IMU for initialisation purpose"""
+
+ def __init__(self):
+
+ self.linear_acceleration = np.zeros(3)
+ self.gyroscope = np.zeros(3)
+ self.attitude_euler = np.zeros(3)
+ self.attitude_quaternion = np.zeros(4)
+
+class dummyJoints:
+ """Fake joints for initialisation purpose"""
+
+ def __init__(self):
+
+ self.positions = np.zeros(12)
+ self.velocities = np.zeros(12)
class dummyDevice:
"""Fake device for initialisation purpose"""
@@ -45,6 +62,8 @@ class dummyDevice:
def __init__(self):
self.hardware = dummyHardware()
+ self.imu = dummyIMU()
+ self.joints = dummyJoints()
class Controller:
@@ -181,13 +200,7 @@ class Controller:
# Run the control loop once with a dummy device for initialization
dDevice = dummyDevice()
- dDevice.q_mes = q_init
- dDevice.v_mes = np.zeros(12)
- dDevice.baseLinearAcceleration = np.zeros(3)
- dDevice.baseAngularVelocity = np.zeros(3)
- dDevice.baseOrientation = np.array([0.0, 0.0, 0.0, 1.0])
- dDevice.dummyPos = np.array([0.0, 0.0, q_init[2]])
- dDevice.b_baseVel = np.zeros(3)
+ dDevice.joints.positions = q_init
self.compute(dDevice)
def compute(self, device):
@@ -205,11 +218,11 @@ class Controller:
# Process state estimator
self.estimator.run_filter(self.gait.getCurrentGait(),
self.footTrajectoryGenerator.getFootPosition(),
- device.baseLinearAcceleration.reshape((-1, 1)),
- device.baseAngularVelocity.reshape((-1, 1)),
- device.baseOrientation.reshape((-1, 1)),
- device.q_mes.reshape((-1, 1)),
- device.v_mes.reshape((-1, 1)),
+ device.imu.linear_acceleration.reshape((-1, 1)),
+ device.imu.gyroscope.reshape((-1, 1)),
+ device.imu.attitude_euler.reshape((-1, 1)),
+ device.joints.positions.reshape((-1, 1)),
+ device.joints.velocities.reshape((-1, 1)),
np.zeros((3, 1)), # device.dummyPos.reshape((-1, 1)), #Â TODO: Case of real device
np.zeros((3, 1))) # device.b_baseVel.reshape((-1, 1)))
diff --git a/scripts/Estimator.py b/scripts/Estimator.py
index c3241c4b..060f63ce 100644
--- a/scripts/Estimator.py
+++ b/scripts/Estimator.py
@@ -352,16 +352,16 @@ class Estimator:
"""
# Linear acceleration of the trunk (base frame)
- self.IMU_lin_acc[:] = device.baseLinearAcceleration
+ self.IMU_lin_acc[:] = device.imu.linear_acceleration
# Angular velocity of the trunk (base frame)
- self.IMU_ang_vel[:] = device.baseAngularVelocity
+ self.IMU_ang_vel[:] = device.imu.gyroscope
# Angular position of the trunk (local frame)
- self.RPY = pin.rpy.matrixToRpy(pin.Quaternion(device.baseOrientation).toRotationMatrix())
+ self.RPY = device.imu.attitude_euler
if (self.k_log <= 1):
- self.offset_yaw_IMU = self.RPY[2]
+ self.offset_yaw_IMU = self.RPY[2].copy()
self.RPY[2] -= self.offset_yaw_IMU # Remove initial offset of IMU
self.IMU_ang_pos[:] = pin.Quaternion(pin.rpy.rpyToMatrix(self.RPY[0],
@@ -379,8 +379,8 @@ class Estimator:
device (object): Interface with the masterboard or the simulation
"""
- self.actuators_pos[:] = device.q_mes
- self.actuators_vel[:] = device.v_mes
+ self.actuators_pos[:] = device.joints.positions
+ self.actuators_vel[:] = device.joints.velocities
return 0
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 2ccedc2f..8ac0db67 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -543,6 +543,26 @@ class LoggerControl():
plt.legend([lgd_Y[i]], prop={'size': 8})
plt.suptitle("Evolution of the quantities of the position complementary filter")
+ # Power supply profile
+ plt.figure()
+ for i in range(3):
+ if i == 0:
+ ax0 = plt.subplot(3, 1, i+1)
+ else:
+ plt.subplot(3, 1, i+1, sharex=ax0)
+
+ if i == 0:
+ plt.plot(t_range, loggerSensors.current[:], linewidth=2)
+ plt.ylabel("Bus current [A]")
+ elif i == 1:
+ plt.plot(t_range, loggerSensors.voltage[:], linewidth=2)
+ plt.ylabel("Bus voltage [V]")
+ else:
+ plt.plot(t_range, loggerSensors.energy[:], linewidth=2)
+ plt.ylabel("Bus energy [J]")
+ plt.xlabel("Time [s]")
+
+ # Display all graphs and wait
plt.show(block=True)
from IPython import embed
diff --git a/scripts/LoggerSensors.py b/scripts/LoggerSensors.py
index ad710968..c2aaf2ba 100644
--- a/scripts/LoggerSensors.py
+++ b/scripts/LoggerSensors.py
@@ -10,20 +10,20 @@ class LoggerSensors():
logSize = np.int(logSize)
self.logSize = logSize
self.i = 0
- if device is not None:
- nb_motors = device.nb_motors
- else:
- nb_motors = 12
+ nb_motors = 12
# Allocate the data:
# IMU and actuators:
self.q_mes = np.zeros([logSize, nb_motors])
self.v_mes = np.zeros([logSize, nb_motors])
self.torquesFromCurrentMeasurment = np.zeros([logSize, nb_motors])
- self.baseOrientation = np.zeros([logSize, 4])
+ self.baseOrientation = np.zeros([logSize, 3])
self.baseAngularVelocity = np.zeros([logSize, 3])
self.baseLinearAcceleration = np.zeros([logSize, 3])
self.baseAccelerometer = np.zeros([logSize, 3])
+ self.current = np.zeros(logSize)
+ self.voltage = np.zeros(logSize)
+ self.energy = np.zeros(logSize)
# Motion capture:
self.mocapPosition = np.zeros([logSize, 3])
@@ -43,13 +43,13 @@ class LoggerSensors():
return
# Logging from the device (data coming from the robot)
- self.q_mes[self.i] = device.q_mes
- self.v_mes[self.i] = device.v_mes
- self.baseOrientation[self.i] = device.baseOrientation
- self.baseAngularVelocity[self.i] = device.baseAngularVelocity
- self.baseLinearAcceleration[self.i] = device.baseLinearAcceleration
- self.baseAccelerometer[self.i] = device.baseAccelerometer
- self.torquesFromCurrentMeasurment[self.i] = device.torquesFromCurrentMeasurment
+ self.q_mes[self.i] = device.joints.positions
+ self.v_mes[self.i] = device.joints.velocities
+ self.baseOrientation[self.i] = device.imu.attitude_euler
+ self.baseAngularVelocity[self.i] = device.imu.gyroscope
+ self.baseLinearAcceleration[self.i] = device.imu.linear_acceleration
+ self.baseAccelerometer[self.i] = device.imu.accelerometer
+ self.torquesFromCurrentMeasurment[self.i] = device.joints.measured_torques
# Logging from qualisys (motion capture)
if qualisys is not None:
diff --git a/scripts/config_solo12.yaml b/scripts/config_solo12.yaml
new file mode 100644
index 00000000..1b39a7a8
--- /dev/null
+++ b/scripts/config_solo12.yaml
@@ -0,0 +1,41 @@
+robot:
+ interface: enp2s0
+ joint_modules:
+ motor_numbers: [0, 3, 2, 1, 5, 4, 6, 9, 8, 7, 11, 10]
+ motor_constants: 0.025
+ gear_ratios: 9.
+ max_currents: 12.
+ reverse_polarities: [
+ true, true, true, false, false, false,
+ true, true, true, false, false, false
+ ]
+ lower_joint_limits: [
+ -1.2, -1.7, -3.4, -1.2, -1.7, -3.4,
+ -1.2, -1.7, -3.4, -1.2, -1.7, -3.4
+ ]
+ upper_joint_limits: [
+ 1.2, 1.7, +3.4, +1.2, +1.7, +3.4,
+ 1.2, 1.7, +3.4, +1.2, +1.7, +3.4
+ ]
+ max_joint_velocities: 80.
+ safety_damping: 0.5
+ imu:
+ rotate_vector: [1, 2, 3]
+ orientation_vector: [-4, 3, -2, 1]
+joint_calibrator:
+ # Can be either POS, NEG, ALT or AUTO
+ search_methods: [
+ AUTO, AUTO, AUTO, AUTO, AUTO, AUTO,
+ AUTO, AUTO, AUTO, AUTO, AUTO, AUTO
+ ]
+ position_offsets: [
+ -0.19501629, -0.31352305, -0.03753992, -0.20769028, 0.28213935,
+ -0.2759672 , -0.1941794 , 0.24587372, 0.21717513, 0.23665195,
+ 0.1188744 , 0.3867009
+ ]
+ Kp: 3.
+ Kd: 0.05
+ T: 1.
+ dt: 0.002
+
+
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index 7b57a7b7..2e81685c 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -12,26 +12,20 @@ import argparse
from LoggerSensors import LoggerSensors
from LoggerControl import LoggerControl
import libquadruped_reactive_walking as lqrw
+import time
params = lqrw.Params() # Object that holds all controller parameters
if params.SIMULATION:
from PyBulletSimulator import PyBulletSimulator
else:
- # from pynput import keyboard
- from solopython.solo12 import Solo12
+ import libodri_control_interface_pywrap as oci
from solopython.utils.qualisysClient import QualisysClient
-
-key_pressed = False
-
-
def get_input():
- global key_pressed
- keystrk = input('Put the robot on the floor and press Enter \n')
+ keystrk = input()
# thread doesn't continue until key is pressed
- key_pressed = True
-
+ # and so it remains alive
def put_on_the_floor(device, q_init):
"""Make the robot go to the default initial position and wait for the user
@@ -41,25 +35,26 @@ def put_on_the_floor(device, q_init):
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]
+
+ print("PUT ON THE FLOOR.")
+
+ Kp_pos = 5.
+ Kd_pos = 0.2
+
+ device.joints.set_position_gains(Kp_pos * np.ones(12))
+ device.joints.set_velocity_gains(Kd_pos * np.ones(12))
+ device.joints.set_desired_positions(q_init)
+ device.joints.set_desired_velocities(np.zeros(12))
+ device.joints.set_torques(np.zeros(12))
+
i = threading.Thread(target=get_input)
i.start()
- 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("Put the robot on the floor and press Enter")
+ while i.is_alive():
+ device.parse_sensor_data()
+ device.send_command_and_wait_end_of_cycle()
+
print("Start the motion.")
@@ -129,7 +124,8 @@ def control_loop(name_interface, name_interface_clone=None, des_vel_analysis=Non
device = PyBulletSimulator()
qc = None
else:
- device = Solo12(name_interface, dt=params.dt_wbc)
+ device = oci.robot_from_yaml_file('config_solo12.yaml')
+ joint_calibrator = oci.joint_calibrator_from_yaml_file('config_solo12.yaml', device.joints)
qc = QualisysClient(ip="140.93.16.160", body_id=0)
if name_interface_clone is not None:
@@ -156,14 +152,22 @@ def control_loop(name_interface, name_interface_clone=None, des_vel_analysis=Non
logSize=params.N_SIMULATION-3)
# Number of motors
- nb_motors = device.nb_motors
+ nb_motors = 12
# Initiate communication with the device and calibrate encoders
if params.SIMULATION:
device.Init(calibrateEncoders=True, q_init=q_init, envID=params.envID,
use_flat_plane=params.use_flat_plane, enable_pyb_GUI=params.enable_pyb_GUI, dt=params.dt_wbc)
else:
- device.Init(calibrateEncoders=True, q_init=q_init)
+ # Initialize the communication and the session.
+ device.start()
+ device.wait_until_ready()
+
+ # Calibrate the robot if needed.
+ device.run_calibration(joint_calibrator, q_init)
+ device.joints.set_zero_commands()
+
+ device.parse_sensor_data()
# Wait for Enter input before starting the control loop
put_on_the_floor(device, q_init)
@@ -172,10 +176,15 @@ def control_loop(name_interface, name_interface_clone=None, des_vel_analysis=Non
t = 0.0
t_max = (params.N_SIMULATION-2) * params.dt_wbc
- while ((not device.hardware.IsTimeout()) and (t < t_max) and (not controller.error)):
+ t_log_whole = np.zeros((params.N_SIMULATION))
+ k_log_whole = 0
+ t_start_whole = 0.0
+ while ((not device.is_timeout) and (t < t_max) and (not controller.error)):
+
+ t_start_whole = time.time()
# Update sensor data (IMU, encoders, Motion capture)
- device.UpdateMeasurment()
+ device.parse_sensor_data()
# Desired torques
controller.compute(device)
@@ -183,17 +192,18 @@ def control_loop(name_interface, name_interface_clone=None, des_vel_analysis=Non
# Check that the initial position of actuators is not too far from the
# desired position of actuators to avoid breaking the robot
if (t <= 10 * params.dt_wbc):
- if np.max(np.abs(controller.result.q_des - device.q_mes)) > 0.15:
- print("DIFFERENCE: ", controller.result.q_des - device.q_mes)
+ if np.max(np.abs(controller.result.q_des - device.joints.positions)) > 0.15:
+ print("DIFFERENCE: ", controller.result.q_des - device.joints.positions)
print("q_des: ", controller.result.q_des)
- print("q_mes: ", device.q_mes)
+ print("q_mes: ", device.joints.positions)
break
# Set desired quantities for the actuators
- device.SetDesiredJointPDgains(controller.result.P, controller.result.D)
- device.SetDesiredJointPosition(controller.result.q_des)
- device.SetDesiredJointVelocity(controller.result.v_des)
- device.SetDesiredJointTorque(controller.result.tau_ff.ravel())
+ device.joints.set_position_gains(controller.result.P)
+ device.joints.set_velocity_gains(controller.result.D)
+ device.joints.set_desired_positions(controller.result.q_des)
+ device.joints.set_desired_velocities(controller.result.v_des)
+ device.joints.set_torques(controller.result.tau_ff.ravel())
# Call logger
if params.LOGGING or params.PLOTTING:
@@ -203,11 +213,16 @@ def control_loop(name_interface, name_interface_clone=None, des_vel_analysis=Non
controller.footstepPlanner, controller.footTrajectoryGenerator,
controller.wbcWrapper)
+
+ t_end_whole = time.time()
# Send command to the robot
for i in range(1):
- device.SendCommand(WaitEndOfCycle=True)
- """if ((device.cpt % 100) == 0):
- device.Print()"""
+ device.send_command_and_wait_end_of_cycle()
+ """if (t % 1) < 5e-5:
+ print('IMU attitude:', device.imu.attitude_euler)
+ print('joint pos :', device.joints.positions)
+ print('joint vel :', device.joints.velocities)
+ device.robot_interface.PrintStats()"""
"""import os
from matplotlib import pyplot as plt
@@ -236,6 +251,9 @@ def control_loop(name_interface, name_interface_clone=None, des_vel_analysis=Non
t += params.dt_wbc # Increment loop time
+ t_log_whole[k_log_whole] = t_end_whole - t_start_whole
+ k_log_whole += 1
+
# ****************************************************************
if (t >= t_max):
@@ -256,33 +274,36 @@ def control_loop(name_interface, name_interface_clone=None, des_vel_analysis=Non
# DAMPING TO GET ON THE GROUND PROGRESSIVELY *********************
t = 0.0
t_max = 2.5
- while ((not device.hardware.IsTimeout()) and (t < t_max)):
+ while ((not device.is_timeout) and (t < t_max)):
- device.UpdateMeasurment() # Retrieve data from IMU and Motion capture
+ device.parse_sensor_data() # 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))
+ device.joints.set_position_gains(np.zeros(nb_motors))
+ device.joints.set_velocity_gains(0.1 * np.ones(nb_motors))
+ device.joints.set_desired_positions(np.zeros(nb_motors))
+ device.joints.set_desired_velocities(np.zeros(nb_motors))
+ device.joints.set_torques(np.zeros(nb_motors))
# Send command to the robot
- device.SendCommand(WaitEndOfCycle=True)
- if ((device.cpt % 1000) == 0):
- device.Print()
+ device.send_command_and_wait_end_of_cycle()
+ if (t % 1) < 5e-5:
+ print('IMU attitude:', device.imu.attitude_euler)
+ print('joint pos :', device.joints.positions)
+ print('joint vel :', device.joints.velocities)
+ device.robot_interface.PrintStats()
t += params.dt_wbc
# FINAL SHUTDOWN *************************************************
# Whatever happened we send 0 torques to the motors.
- device.SetDesiredJointTorque([0]*nb_motors)
- device.SendCommand(WaitEndOfCycle=True)
+ device.joints.set_torques(np.zeros(nb_motors))
+ device.send_command_and_wait_end_of_cycle()
- if device.hardware.IsTimeout():
+ if device.is_timeout:
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
# Plot estimated computation time for each step for the control architecture
from matplotlib import pyplot as plt
@@ -292,16 +313,10 @@ def control_loop(name_interface, name_interface_clone=None, des_vel_analysis=Non
plt.plot(controller.t_list_mpc[1:], 'b+')
plt.plot(controller.t_list_wbc[1:], '+', color="violet")
plt.plot(controller.t_list_loop[1:], 'k+')
- plt.plot(controller.t_list_InvKin[1:], 'o', color="darkgreen")
- plt.plot(controller.t_list_QPWBC[1:], 'o', color="royalblue")
- plt.legend(["Estimator", "Planner", "MPC", "WBC", "Whole loop", "InvKin", "QP WBC"])
+ plt.plot(t_log_whole, 'x')
+ plt.legend(["Estimator", "Planner", "MPC", "WBC", "Whole loop", "Whole loop + Interface"])
plt.title("Loop time [s]")
plt.show(block=True)"""
- """plt.figure()
- for i in range(3):
- plt.subplot(3, 1, i+1)
- plt.plot(controller.o_log_foot[:, i, 0])
- plt.show(block=True)"""
# Plot recorded data
if params.PLOTTING:
@@ -345,7 +360,7 @@ def main():
help='Name of the clone interface that will reproduce the movement of the first one \
(use ifconfig in a terminal), for instance "enp1s0"')
- f, v = control_loop(parser.parse_args().interface, parser.parse_args().clone, np.array([1.5, 0.0, 0.0, 0.0, 0.0, 0.0]))
+ f, v = control_loop(parser.parse_args().interface, parser.parse_args().clone) # , np.array([1.5, 0.0, 0.0, 0.0, 0.0, 0.0]))
print(f, v)
quit()
diff --git a/scripts/utils_mpc.py b/scripts/utils_mpc.py
index 563cae75..52c7a5af 100644
--- a/scripts/utils_mpc.py
+++ b/scripts/utils_mpc.py
@@ -143,9 +143,9 @@ def init_robot(q_init, params, enable_viewer):
solo.initViewer(loadModel=True)
if ('viewer' in solo.viz.__dict__):
solo.viewer.gui.addFloor('world/floor')
- solo.viewer.gui.setRefreshIsSynchronous(False)"""
+ solo.viewer.gui.setRefreshIsSynchronous(False)
if enable_viewer:
- solo.display(q)
+ solo.display(q)"""
# Initialisation of model quantities
pin.centerOfMass(solo.model, solo.data, q, np.zeros((18, 1)))
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 8ee057f8..1756e574 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -135,7 +135,7 @@ void Estimator::initialize(Params& params)
}
-void Estimator::get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity, Vector4 const& baseOrientation)
+void Estimator::get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity, Vector3 const& baseOrientation)
{
// Linear acceleration of the trunk (base frame)
IMU_lin_acc_ = baseLinearAcceleration;
@@ -144,7 +144,7 @@ void Estimator::get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 cons
IMU_ang_vel_ = baseAngularVelocity;
// Angular position of the trunk (local frame)
- IMU_RPY_ = pinocchio::rpy::matrixToRpy(pinocchio::SE3::Quaternion(baseOrientation).toRotationMatrix());
+ IMU_RPY_ = baseOrientation;
if(k_log_ <= 1) {
offset_yaw_IMU_ = IMU_RPY_(2, 0);
--
GitLab