Commit af2b5a25 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[python] Fix bug in ex_3 notebook (Talos balancing with GUI)

parent 57136684
%% Cell type:code id: tags:
``` python
import threading
import time
import sys
from IPython.display import display
from ipywidgets import interact
import ipywidgets as widgets
import numpy as np
import pinocchio as pin
sys.path.append('..')
import talos_conf as conf
import vizutils
from tsid_biped import TsidBiped
```
%% Cell type:code id: tags:
``` python
tsid = TsidBiped(conf)
tsid.q0[2] = 1.02127
com_0 = tsid.robot.com(tsid.formulation.data())
H_rf_0 = tsid.robot.framePosition(tsid.formulation.data(), tsid.model.getJointId(conf.rf_frame_name))
H_lf_0 = tsid.robot.framePosition(tsid.formulation.data(), tsid.model.getJointId(conf.lf_frame_name))
H_rf_0 = tsid.robot.framePosition(tsid.formulation.data(), tsid.model.getFrameId(conf.rf_frame_name))
H_lf_0 = tsid.robot.framePosition(tsid.formulation.data(), tsid.model.getFrameId(conf.lf_frame_name))
vizutils.addViewerSphere(tsid.viz, 'world/com', conf.SPHERE_RADIUS, conf.COM_SPHERE_COLOR)
vizutils.addViewerSphere(tsid.viz, 'world/com_ref', conf.REF_SPHERE_RADIUS, conf.COM_REF_SPHERE_COLOR)
vizutils.addViewerSphere(tsid.viz, 'world/rf', conf.SPHERE_RADIUS, conf.RF_SPHERE_COLOR)
vizutils.addViewerSphere(tsid.viz, 'world/rf_ref', conf.REF_SPHERE_RADIUS, conf.RF_REF_SPHERE_COLOR)
vizutils.addViewerSphere(tsid.viz, 'world/lf', conf.SPHERE_RADIUS, conf.LF_SPHERE_COLOR)
vizutils.addViewerSphere(tsid.viz, 'world/lf_ref', conf.REF_SPHERE_RADIUS, conf.LF_REF_SPHERE_COLOR)
```
%% Cell type:code id: tags:
``` python
push_robot_active, push_robot_com_vel, com_vel_entry = False, 3 * [0.0], None
run, push_robot_active = threading.Event(), threading.Event()
run.set()
```
%% Cell type:code id: tags:
``` python
def run_simu():
i, t = 0, 0.0
q, v = tsid.q, tsid.v
time_avg = 0.0
while run.is_set():
time_start = time.time()
tsid.comTask.setReference(tsid.trajCom.computeNext())
tsid.postureTask.setReference(tsid.trajPosture.computeNext())
tsid.rightFootTask.setReference(tsid.trajRF.computeNext())
tsid.leftFootTask.setReference(tsid.trajLF.computeNext())
HQPData = tsid.formulation.computeProblemData(t, q, v)
sol = tsid.solver.solve(HQPData)
if sol.status != 0:
print("QP problem could not be solved! Error code:", sol.status)
break
# tau = tsid.formulation.getActuatorForces(sol)
dv = tsid.formulation.getAccelerations(sol)
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
i, t = i + 1, t + conf.dt
if push_robot_active.is_set():
push_robot_active.clear()
data = tsid.formulation.data()
if tsid.contact_LF_active:
J_LF = tsid.contactLF.computeMotionTask(0.0, q, v, data).matrix
else:
J_LF = np.zeros((0, tsid.model.nv))
if tsid.contact_RF_active:
J_RF = tsid.contactRF.computeMotionTask(0.0, q, v, data).matrix
else:
J_RF = np.zeros((0, tsid.model.nv))
J = np.vstack((J_LF, J_RF))
J_com = tsid.comTask.compute(t, q, v, data).matrix
A = np.vstack((J_com, J))
b = np.concatenate((np.array(push_robot_com_vel), np.zeros(J.shape[0])))
v = np.linalg.lstsq(A, b, rcond=-1)[0]
if i % conf.DISPLAY_N == 0:
tsid.viz.display(q)
x_com = tsid.robot.com(tsid.formulation.data())
x_com_ref = tsid.trajCom.getSample(t).pos()
H_lf = tsid.robot.framePosition(tsid.formulation.data(), tsid.LF)
H_rf = tsid.robot.framePosition(tsid.formulation.data(), tsid.RF)
x_lf_ref = tsid.trajLF.getSample(t).pos()[:3]
x_rf_ref = tsid.trajRF.getSample(t).pos()[:3]
vizutils.applyViewerConfiguration(tsid.viz, 'world/com', x_com.tolist() + [0, 0, 0, 1.])
vizutils.applyViewerConfiguration(tsid.viz, 'world/com_ref', x_com_ref.tolist() + [0, 0, 0, 1.])
vizutils.applyViewerConfiguration(tsid.viz, 'world/rf', pin.SE3ToXYZQUATtuple(H_rf))
vizutils.applyViewerConfiguration(tsid.viz, 'world/lf', pin.SE3ToXYZQUATtuple(H_lf))
vizutils.applyViewerConfiguration(tsid.viz, 'world/rf_ref', x_rf_ref.tolist() + [0, 0, 0, 1.])
vizutils.applyViewerConfiguration(tsid.viz, 'world/lf_ref', x_lf_ref.tolist() + [0, 0, 0, 1.])
time_spent = time.time() - time_start
time_avg = (i * time_avg + time_spent) / (i + 1)
if time_avg < 0.9 * conf.dt:
time.sleep(conf.dt - time_avg)
print('end of simulation')
```
%% Cell type:code id: tags:
``` python
output = widgets.Output()
th_simu = threading.Thread(target=run_simu)
th_simu.start()
@interact(com_x=(-10., 10.), com_y=(-15., 15.), com_z=(-40., 40.))
def sliders_com(com_x, com_y, com_z):
tsid.trajCom.setReference(com_0 + np.array([1e-2 * com_x, 1e-2 * com_y, 1e-2 * com_z]).T)
@interact(rf_x=(-30., 30.), rf_y=(-30., 30.), rf_z=(-30., 30.))
def sliders_rf(rf_x, rf_y, rf_z):
H_rf_ref = H_rf_0.copy()
H_rf_ref.translation += np.array([1e-2 * rf_x, 1e-2 * rf_y, 1e-2 * rf_z]).T
tsid.trajRF.setReference(H_rf_ref)
@interact(lf_x=(-30., 30.), lf_y=(-30., 30.), lf_z=(-30., 30.))
def sliders_lf(lf_x, lf_y, lf_z):
H_lf_ref = H_lf_0.copy()
H_lf_ref.translation += + np.array([1e-2 * lf_x, 1e-2 * lf_y, 1e-2 * lf_z]).T
tsid.trajLF.setReference(H_lf_ref)
def buttons(b):
if b.description == 'Stop':
run.clear()
th_simu.join()
elif b.description == 'Switch Right Foot':
if tsid.contact_RF_active:
tsid.remove_contact_RF()
else:
tsid.add_contact_RF()
elif b.description == 'Switch Left Foot':
if tsid.contact_LF_active:
tsid.remove_contact_LF()
else:
tsid.add_contact_LF()
for action in ['Stop', 'Switch Right Foot', 'Switch Left Foot']:
button = widgets.Button(description=action)
button.on_click(buttons)
output.append_display_data(button)
```
%% Cell type:code id: tags:
``` python
hasattr(tsid.viz.viewer, 'jupyter_cell') and tsid.viz.viewer.jupyter_cell()
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment