diff --git a/Joystick.py b/Joystick.py
index c60e289140157210451205ceb0f8a4c3a8bf2e0b..ee9e888a6e65138d65ac24003dd886e7649ca7e2 100644
--- a/Joystick.py
+++ b/Joystick.py
@@ -2,6 +2,7 @@
import numpy as np
from time import clock
+import gamepadClient as gC
# import inputs
@@ -21,9 +22,11 @@ class Joystick:
self.vX = 0.
self.vY = 0.
self.vYaw = 0.
- self.VxScale = 0.1/32768
- self.VyScale = 0.1/32768
- self.vYawScale = 0.4/32768
+ self.VxScale = 0.1
+ self.VyScale = 0.1
+ self.vYawScale = 0.4
+
+ self.gp = gC.GamepadClient()
def update_v_ref(self, k_loop):
"""events = inputs.get_gamepad()
@@ -36,14 +39,18 @@ class Joystick:
self.vY = event.state * self.VyScale
if event.code == 'ABS_RX':
self.vYaw = event.state * self.vYawScale
- print(- self.vY, - self.vX, - self.vYaw)
+ print(- self.vY, - self.vX, - self.vYaw)"""
- self.v_ref = np.array([[- self.vY, - self.vX, 0.0, 0.0, 0.0, - self.vYaw]]).T"""
+ self.vX = self.gp.leftJoystickX.value * self.VxScale
+ self.vY = self.gp.leftJoystickY.value * self.VxScale
+ self.vYaw = self.gp.rightJoystickX.value * self.vYawScale
+ # print(- self.vY, - self.vX, - self.vYaw)
+ self.v_ref = np.array([[- self.vY, - self.vX, 0.0, 0.0, 0.0, - self.vYaw]]).T
# Change reference velocity during the simulation (in trunk frame)
# Moving forwards
- if k_loop == 200:
- self.v_ref = np.array([[0.1, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
+ """if k_loop == 200:
+ self.v_ref = np.array([[0.1, 0.0, 0.0, 0.0, 0.0, 0.0]]).T"""
"""
# Turning
if k_loop == 1500:
diff --git a/TSID_Debug_controller_four_legs_fb_vel.py b/TSID_Debug_controller_four_legs_fb_vel.py
index 78b4a4afcbad5a6c580a5ddee8c4eb973a0bab94..242935b993bbf45ada567fb60a7ca49c721a1b27 100644
--- a/TSID_Debug_controller_four_legs_fb_vel.py
+++ b/TSID_Debug_controller_four_legs_fb_vel.py
@@ -91,7 +91,7 @@ class controller:
self.memory_contacts = R @ self.shoulders.copy()
# Foot trajectory generator
- max_height_feet = 0.04
+ max_height_feet = 0.05
t_lock_before_touchdown = 0.05
self.ftgs = [ftg.Foot_trajectory_generator(max_height_feet, t_lock_before_touchdown) for i in range(4)]
@@ -509,11 +509,14 @@ class controller:
def update_footsteps(self, k_simu, k_loop, looping, sequencer, mpc_interface):
- self.t_remaining[0, [1, 2]] = np.max((0.0, 0.16 * (looping*0.5 - k_loop) * 0.001))
+ """# self.t_remaining[0, [1, 2]] = np.max((0.0, 0.16 * (looping*0.5 - k_loop) * 0.001))
+ self.t_remaining[0, [1, 2]] = 0.16 * (looping*0.5 - k_loop) * 0.001
+ (self.t_remaining[0, [1, 2]])[self.t_remaining[0, [1, 2]] < 0.0] = 0.0
if k_loop < int(looping*0.5):
self.t_remaining[0, [0, 3]] = 0.0
else:
- self.t_remaining[0, [0, 3]] = 0.16 * (looping - k_loop) * 0.001
+ self.t_remaining[0, [0, 3]] = 0.16 * (looping - k_loop) * 0.001"""
+ self.test_tmp1(k_loop, looping)
# Get PyBullet velocity in local frame
"""self.vu_m[0:2, 0:1] = mpc_interface.lV[0:2, 0:1]
@@ -527,8 +530,23 @@ class controller:
"""for i in range(4):
self.footsteps[:, i:(i+1)] = mpc_interface.o_shoulders[0:2, i:(i+1)] + \
(mpc_interface.oMl.rotation @ self.fstep_planner.footsteps_tsid[:, i]).T[0:2, :]"""
- self.footsteps = np.array(mpc_interface.o_shoulders + (mpc_interface.oMl.rotation @ self.fstep_planner.footsteps_tsid))[0:2, :]
+ # self.footsteps = np.array(mpc_interface.o_shoulders + (mpc_interface.oMl.rotation @ self.fstep_planner.footsteps_tsid))[0:2, :]
+ self.test_tmp2(mpc_interface)
+
+ return 0
+ def test_tmp1(self, k_loop, looping):
+ # self.t_remaining[0, [1, 2]] = np.max((0.0, 0.16 * (looping*0.5 - k_loop) * 0.001))
+ self.t_remaining[0, [1, 2]] = 0.16 * (looping*0.5 - k_loop) * 0.001
+ (self.t_remaining[0, [1, 2]])[self.t_remaining[0, [1, 2]] < 0.0] = 0.0
+ if k_loop < int(looping*0.5):
+ self.t_remaining[0, [0, 3]] = 0.0
+ else:
+ self.t_remaining[0, [0, 3]] = 0.16 * (looping - k_loop) * 0.001
+ return 0
+
+ def test_tmp2(self, mpc_interface):
+ self.footsteps = np.array(mpc_interface.o_shoulders + (mpc_interface.oMl.rotation @ self.fstep_planner.footsteps_tsid))[0:2, :]
return 0
def update_ref_forces(self, mpc_interface, mpc):
diff --git a/bauzil_stairs.stl b/bauzil_stairs.stl
new file mode 100644
index 0000000000000000000000000000000000000000..c75c11b9c75eccda0502c24f645bec841a864c69
Binary files /dev/null and b/bauzil_stairs.stl differ
diff --git a/bauzil_stairs.urdf b/bauzil_stairs.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..022d7ba92fd7e1984f9af4483233ffc4623ee963
--- /dev/null
+++ b/bauzil_stairs.urdf
@@ -0,0 +1,24 @@
+<robot name="bauzil_stairs.urdf">
+ <link concave="yes" name="baseLink">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.0"/>
+ <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 3.1415" xyz="-0.9 1.8 -0.01"/>
+ <geometry>
+ <mesh filename="bauzil_stairs.stl" scale="1 1 0.3"/>
+ </geometry>
+ <material name="red">
+ <color rgba="1 0 0 1"/>
+ </material>
+ </visual>
+ <collision concave="yes">
+ <origin rpy="0 0 3.1415" xyz="-0.9 1.8 -0.01"/>
+ <geometry>
+ <mesh filename="bauzil_stairs.stl" scale="1 1 0.3"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
\ No newline at end of file
diff --git a/gamepadClient.py b/gamepadClient.py
new file mode 100644
index 0000000000000000000000000000000000000000..c2e3f65f7f536b51c4f3f6b8033502e11c8c2e0f
--- /dev/null
+++ b/gamepadClient.py
@@ -0,0 +1,72 @@
+'''
+Simple python script to get Asyncronous gamepad inputs
+Thomas FLAYOLS - LAAS CNRS
+From https://github.com/thomasfla/solopython
+
+Use:
+To display data, run "python gamepadClient.py"
+'''
+import inputs
+import time
+from multiprocessing import Process
+from multiprocessing.sharedctypes import Value
+from ctypes import c_double, c_bool
+
+
+class GamepadClient():
+ def __init__(self):
+ self.running = Value(c_bool, lock=True)
+ self.startButton = Value(c_bool, lock=True)
+ self.leftJoystickX = Value(c_double, lock=True)
+ self.leftJoystickY = Value(c_double, lock=True)
+ self.rightJoystickX = Value(c_double, lock=True)
+ self.rightJoystickY = Value(c_double, lock=True)
+
+ self.startButton.value = False
+ self.leftJoystickX.value = 0.0
+ self.leftJoystickY.value = 0.0
+ self.rightJoystickX.value = 0.0
+ self.rightJoystickY.value = 0.0
+
+ args = (self.running, self.startButton, self.leftJoystickX,
+ self.leftJoystickY, self.rightJoystickX, self.rightJoystickY)
+ self.process = Process(target=self.run, args=args)
+ self.process.start()
+ time.sleep(0.2)
+
+ def run(self, running, startButton, leftJoystickX, leftJoystickY, rightJoystickX, rightJoystickY):
+ running.value = True
+ while(running.value):
+ events = inputs.get_gamepad()
+ for event in events:
+ #print(event.ev_type, event.code, event.state)
+ if event.ev_type == 'Absolute':
+ if event.code == 'ABS_X':
+ leftJoystickX.value = event.state / 32768.0
+ if event.code == 'ABS_Y':
+ leftJoystickY.value = event.state / 32768.0
+ if event.code == 'ABS_RX':
+ rightJoystickX.value = event.state / 32768.0
+ if event.code == 'ABS_RY':
+ rightJoystickY.value = event.state / 32768.0
+ if (event.ev_type == 'Key'):
+ if event.code == 'BTN_START':
+ startButton.value = event.state
+ print (event.state)
+ def stop(self):
+ self.running.value = False
+ self.process.terminate()
+ self.process.join()
+
+
+if __name__ == "__main__":
+ gp = GamepadClient()
+ for i in range(1000):
+ print("LX = ", gp.leftJoystickX.value, end=" ; ")
+ print("LY = ", gp.leftJoystickY.value, end=" ; ")
+ print("RX = ", gp.rightJoystickX.value, end=" ; ")
+ print("RY = ", gp.rightJoystickY.value, end=" ; ")
+ print("start = ",gp.startButton.value)
+ time.sleep(0.1)
+
+ gp.stop()
diff --git a/main.py b/main.py
index d3a7b332ff6da363908acdde80ede843946c3239..b6c72d41e2e6744552853156841259a09a01b293 100644
--- a/main.py
+++ b/main.py
@@ -28,8 +28,8 @@ N_SIMULATION = 1000 # number of time steps simulated
# Initialize the error for the simulation time
time_error = False
-t_list_mpc = []
-t_list_tsid = []
+t_list_mpc = [0] * int(N_SIMULATION)
+t_list_tsid = [0] * int(N_SIMULATION)
# Enable/Disable Gepetto viewer
enable_gepetto_viewer = False
@@ -66,7 +66,7 @@ for k in range(int(N_SIMULATION)):
time_start = time.time()
- if (k % 100) == 0:
+ if (k % 1000) == 0:
print("Iteration: ", k)
####################################################################
@@ -101,6 +101,9 @@ for k in range(int(N_SIMULATION)):
pyb.resetBaseVelocity(pyb_sim.sphereId2, linearVelocity=[-3.0, 0.0, 2.0])
flag_sphere2 = False
+ pyb.resetDebugVisualizerCamera(cameraDistance=0.75, cameraYaw=50, cameraPitch=-35,
+ cameraTargetPosition=[qmes12[0, 0],qmes12[1, 0] + 0.0, 0.0])
+
########################
# Update MPC interface #
########################
@@ -211,7 +214,7 @@ for k in range(int(N_SIMULATION)):
time_spent = time.time() - time_mpc
# Logging the time spent
- t_list_mpc.append(time_spent)
+ t_list_mpc[k] = time_spent
# Visualisation with gepetto viewer
if enable_gepetto_viewer:
@@ -315,7 +318,7 @@ for k in range(int(N_SIMULATION)):
time_spent = time.time() - time_start
# Logging the time spent
- t_list_tsid.append(time_spent)
+ t_list_tsid[k] = time_spent
############
# PYBULLET #
diff --git a/utils.py b/utils.py
index 018b86650c9467766f5d67a5dfbc3d701d152a49..cdbe39dcda8675f714ae9ef1e8769d1b6a5582f8 100644
--- a/utils.py
+++ b/utils.py
@@ -210,6 +210,9 @@ class pybullet_simulator:
# Load horizontal plane
pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
self.planeId = pyb.loadURDF("plane.urdf")
+ """self.stairsId = pyb.loadURDF("../../../../../Documents/Git-Repositories/mpc-tsid/bauzil_stairs.urdf")#,
+ #basePosition=[-1.25, 3.5, -0.1],
+ #baseOrientation=pyb.getQuaternionFromEuler([0.0, 0.0, 3.1415]))"""
mesh_scale = [1.0, 0.1, 0.02]
visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,