diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index dabcea55a699bf4c00fdd305f278d401ec54e172..0d615f9846875708065b19a2aeaa3cc1c9c611ea 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -178,9 +178,6 @@ class Controller:
self.k = 0
- self.qmes12 = np.zeros((19, 1))
- self.vmes12 = np.zeros((18, 1))
-
self.q_display = np.zeros((19, 1))
self.v_ref = np.zeros((18, 1))
self.a_ref = np.zeros((18, 1))
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index a106d75d9669a89385ad68b477717676499f6068..b8e7b7a656e1880457a8243862d7c4fba7253ec4 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -53,11 +53,9 @@ def put_on_the_floor(device, q_init):
device.send_command_and_wait_end_of_cycle(params.dt_wbc)
# Slow increase till 1/4th of mass is supported by each foot
- duration_increase = 2.0 # in seconds
+ duration_increase = 2.0
steps = int(duration_increase / params.dt_wbc)
- tau_ff = np.array(
- [0.0, 0.022, 0.5, 0.0, 0.022, 0.5, 0.0, -0.022, -0.5, 0.0, -0.022, -0.5]
- )
+ tau_ff = np.array([0.0, 0.022, 0.5] * 2 + [0.0, -0.022, -0.5] * 2)
# tau_ff = np.array([0.0, 0.022, 0.5, 0.0, 0.022, 0.5, 0.0, 0.025, 0.575, 0.0, 0.025, 0.575])
for i in range(steps):
@@ -95,7 +93,7 @@ def damp_control(device, nb_motors):
t = 0.0
t_max = 2.5
while (not device.is_timeout) and (t < t_max):
- device.parse_sensor_data() # Retrieve data from IMU and Motion capture
+ device.parse_sensor_data()
# Set desired quantities for the actuators
device.joints.set_position_gains(np.zeros(nb_motors))
@@ -162,16 +160,15 @@ def control_loop(des_vel_analysis=None):
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,
+ q_init,
+ params.envID,
+ params.use_flat_plane,
+ params.enable_pyb_GUI,
+ params.dt_wbc,
)
# import ForceMonitor
# myForceMonitor = ForceMonitor.ForceMonitor(device.pyb_sim.robotId, device.pyb_sim.planeId)
- else: # Initialize the communication and the session.
+ else:
device.initialize(q_init[:])
device.joints.set_zero_commands()
device.parse_sensor_data()
@@ -286,7 +283,7 @@ def control_loop(des_vel_analysis=None):
if __name__ == "__main__":
- # os.nice(-20) # Â Set the process to highest priority (from -20 highest to +20 lowest)
+ # os.nice(-20)
f, v = control_loop() # , np.array([1.5, 0.0, 0.0, 0.0, 0.0, 0.0]))
print(f, v)
quit()
diff --git a/python/quadruped_reactive_walking/main_solo12_replay.py b/python/quadruped_reactive_walking/main_solo12_replay.py
index e3d75349a5c8f36aa787de3890f85565bd90ad9b..ddce92468f1dc990977e94fbb5d856d160621121 100644
--- a/python/quadruped_reactive_walking/main_solo12_replay.py
+++ b/python/quadruped_reactive_walking/main_solo12_replay.py
@@ -130,24 +130,20 @@ def control_loop(des_vel_analysis=None):
device, qualisys=qc, logSize=params.N_SIMULATION - 3
)
- # 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,
+ q_init,
+ params.envID,
+ params.use_flat_plane,
+ params.enable_pyb_GUI,
+ params.dt_wbc,
)
else:
- # Initialize the communication and the session.
device.initialize(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)
# CONTROL LOOP ***************************************************
@@ -223,9 +219,7 @@ def control_loop(des_vel_analysis=None):
if __name__ == "__main__":
- os.nice(
- -20
- ) # Â Set the process to highest priority (from -20 highest to +20 lowest)
+ os.nice(-20)
f, v = control_loop() # , np.array([1.5, 0.0, 0.0, 0.0, 0.0, 0.0]))
print("End of script")
print(f, v)
diff --git a/python/quadruped_reactive_walking/tools/PyBulletSimulator.py b/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
index a63cf14505b95ecd6f9c08d6299b4fd3596e7ea9..465ec1d953f361b8cf8772c94e04bc5c953bf631 100644
--- a/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
+++ b/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
@@ -32,8 +32,6 @@ class pybullet_simulator:
else:
pyb.connect(pyb.DIRECT)
- # p.GUI for graphical version
- # p.DIRECT for non-graphical version
# Load horizontal plane
pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
@@ -71,7 +69,6 @@ class pybullet_simulator:
height_prev = 0.0
for j in range(int(numHeightfieldColumns / 2)):
for i in range(int(numHeightfieldRows / 2)):
- # uniform distribution
height = random.uniform(0, heightPerturbationRange)
# height = 0.25*np.sin(2*np.pi*(i-128)/46) # sinus pattern
heightfieldData[2 * i + 2 * j * numHeightfieldRows] = (
@@ -267,25 +264,15 @@ class pybullet_simulator:
useMaximalCoordinates=True,
)
- # Create a red line for debug purpose
+ # Create a red and blue lines for debug purpose
self.lineId_red = []
-
- # Create a blue line for debug purpose
self.lineId_blue = []
- """
- cubeStartPos = [0.0, 0.45, 0.0]
- cubeStartOrientation = pyb.getQuaternionFromEuler([0, 0, 0])
- self.cubeId = pyb.loadURDF("cube_small.urdf",
- cubeStartPos, cubeStartOrientation)
- pyb.changeDynamics(self.cubeId, -1, mass=0.5)
- print("Mass of cube:", pyb.getDynamicsInfo(self.cubeId, -1)[0])"""
-
- # Set the gravity
+
pyb.setGravity(0, 0, -9.81)
# Load Quadruped robot
robotStartPos = [0, 0, 0.0]
- robotStartOrientation = pyb.getQuaternionFromEuler([0.0, 0.0, 0.0]) # -np.pi/2
+ robotStartOrientation = pyb.getQuaternionFromEuler([0.0, 0.0, 0.0])
pyb.setAdditionalSearchPath(
EXAMPLE_ROBOT_DATA_MODEL_DIR + "/solo_description/robots"
)
@@ -305,7 +292,7 @@ class pybullet_simulator:
self.q_init = np.array([q_init]).transpose()
pyb.resetJointStatesMultiDof(
self.robotId, self.revoluteJointIndices, self.q_init
- ) # q0[7:])
+ )
# Enable torque control for revolute joints
jointTorques = [0.0 for m in self.revoluteJointIndices]
@@ -367,26 +354,25 @@ class pybullet_simulator:
cameraTargetPosition=[0.0, 0.0, robotStartPos[2] - 0.2],
)
- def check_pyb_env(self, k, envID, qmes12):
+ def check_pyb_env(self, k, envID, q):
"""
Check the state of the robot to trigger events and update camera
Args:
k (int): Number of inv dynamics iterations since the start of the simulation
envID (int): Identifier of the current environment to be able to handle different scenarios
- qmes12 (19x1 array): the position/orientation of the trunk and angular position of actuators
+ q (19x1 array): the position/orientation of the trunk and angular position of actuators
"""
-
# If spheres are loaded
if envID == 1:
# Check if the robot is in front of the first sphere to trigger it
- if self.flag_sphere1 and (qmes12[1, 0] >= 0.9):
+ if self.flag_sphere1 and (q[1, 0] >= 0.9):
pyb.resetBaseVelocity(self.sphereId1, linearVelocity=[2.5, 0.0, 2.0])
self.flag_sphere1 = False
# Check if the robot is in front of the second sphere to trigger it
- if self.flag_sphere2 and (qmes12[1, 0] >= 1.1):
+ if self.flag_sphere2 and (q[1, 0] >= 1.1):
pyb.resetBaseVelocity(self.sphereId2, linearVelocity=[-2.5, 0.0, 2.0])
self.flag_sphere2 = False
@@ -425,7 +411,7 @@ class pybullet_simulator:
)
# Get the orientation of the robot to change the orientation of the camera with the rotation of the robot
- oMb_tmp = pin.SE3(pin.Quaternion(qmes12[3:7]), np.array([0.0, 0.0, 0.0]))
+ oMb_tmp = pin.SE3(pin.Quaternion(q[3:7]), np.array([0.0, 0.0, 0.0]))
RPY = pin.rpy.matrixToRpy(oMb_tmp.rotation)
# Update the PyBullet camera on the robot position to do as if it was attached to the robot
@@ -433,38 +419,16 @@ class pybullet_simulator:
cameraDistance=0.6,
cameraYaw=(0.0 * RPY[2] * (180 / 3.1415) + 45),
cameraPitch=-39.9,
- cameraTargetPosition=[qmes12[0, 0], qmes12[1, 0] + 0.0, 0.0],
+ cameraTargetPosition=[q[0, 0], q[1, 0] + 0.0, 0.0],
)
def retrieve_pyb_data(self):
"""
Retrieve the position and orientation of the base in world frame as well as its linear and angular velocities
"""
-
- # Retrieve data from the simulation
- self.jointStates = pyb.getJointStates(
- self.robotId, self.revoluteJointIndices
- ) # State of all joints
- self.baseState = pyb.getBasePositionAndOrientation(
- self.robotId
- ) # Position and orientation of the trunk
- self.baseVel = pyb.getBaseVelocity(self.robotId) # Velocity of the trunk
-
- # Joints configuration and velocity vector for free-flyer + 12 actuators
- self.qmes12 = np.vstack(
- (
- np.array([self.baseState[0]]).T,
- np.array([self.baseState[1]]).T,
- np.array([[state[0] for state in self.jointStates]]).T,
- )
- )
- self.vmes12 = np.vstack(
- (
- np.array([self.baseVel[0]]).T,
- np.array([self.baseVel[1]]).T,
- np.array([[state[1] for state in self.jointStates]]).T,
- )
- )
+ self.jointStates = pyb.getJointStates(self.robotId, self.revoluteJointIndices)
+ self.baseState = pyb.getBasePositionAndOrientation(self.robotId)
+ self.baseVel = pyb.getBaseVelocity(self.robotId)
def apply_external_force(self, k, start, duration, F, loc):
"""
@@ -503,7 +467,6 @@ class pybullet_simulator:
Args:
qtarget (12x1 array): the target position for the 12 actuators
"""
-
qmes = np.zeros((12, 1))
vmes = np.zeros((12, 1))
@@ -519,12 +482,7 @@ class pybullet_simulator:
# PD settings
P = 1.0 * 3.0
- D = (
- 0.05
- * np.array(
- [[1.0, 0.3, 0.3, 1.0, 0.3, 0.3, 1.0, 0.3, 0.3, 1.0, 0.3, 0.3]]
- ).transpose()
- )
+ D = 0.05 * np.array([[1.0, 0.3, 0.3] * 4]).transpose()
while True or np.max(np.abs(qtarget - qmes)) > 0.1:
@@ -557,10 +515,7 @@ class pybullet_simulator:
forces=jointTorques,
)
- # Compute one step of simulation
pyb.stepSimulation()
-
- # Increment loop counter
cpt += 1
while (time.time() - time_loop) < dt_traj:
@@ -713,24 +668,23 @@ class PyBulletSimulator:
self.q_des = np.zeros(12)
self.v_des = np.zeros(12)
self.tau_ff = np.zeros(12)
+ self.g = np.array([[0.0], [0.0], [-9.81]])
- def Init(self, calibrateEncoders, q_init, envID, use_flat_plane, enable_pyb_GUI, dt):
+ def Init(self, q, envID, use_flat_plane, enable_pyb_GUI, dt):
"""
Initialize the PyBullet simultor with a given environment and a given state of the robot
Args:
calibrateEncoders (bool): dummy variable, not used for simulation but used for real robot
- q_init (12 x 0 array): initial angular positions of the joints of the robot
+ q (12 x 0 array): initial angular positions of the joints of the robot
envID (int): which environment should be loaded in the simulation
use_flat_plane (bool): to use either a flat ground or a rough ground
enable_pyb_GUI (bool): to display PyBullet GUI or not
dt (float): time step of the simulation
"""
- self.pyb_sim = pybullet_simulator(
- q_init, envID, use_flat_plane, enable_pyb_GUI, dt
- )
- self.q_init = q_init
- self.joints.positions[:] = q_init
+ self.pyb_sim = pybullet_simulator(q, envID, use_flat_plane, enable_pyb_GUI, dt)
+ self.q_init = q
+ self.joints.positions[:] = q
self.dt = dt
self.time_loop = time.time()
@@ -802,9 +756,7 @@ class PyBulletSimulator:
self.prev_o_imuVel[:, 0:1] = self.o_imuVel
self.imu.accelerometer[:] = (
self.imu.linear_acceleration
- + (
- self.oMb.rotation.transpose() @ np.array([[0.0], [0.0], [-9.81]])
- ).ravel()
+ + (self.oMb.rotation.transpose() @ self.g).ravel()
)
def send_command_and_wait_end_of_cycle(self, WaitEndOfCycle=True):
@@ -814,13 +766,10 @@ class PyBulletSimulator:
Args:
WaitEndOfCycle (bool): wait to have simulation time = real time
"""
-
# Position and velocity of actuators
- jointStates = pyb.getJointStates(
- self.pyb_sim.robotId, self.revoluteJointIndices
- ) # State of all joints
- self.joints.positions[:] = np.array([state[0] for state in jointStates])
- self.joints.velocities[:] = np.array([state[1] for state in jointStates])
+ joints = pyb.getJointStates(self.pyb_sim.robotId, self.revoluteJointIndices)
+ self.joints.positions[:] = np.array([state[0] for state in joints])
+ self.joints.velocities[:] = np.array([state[1] for state in joints])
# Compute PD torques
tau_pd = self.P * (self.q_des - self.joints.positions) + self.D * (