Commit 6223fc96 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[python] Fix a few bugs introduced with recent changes to make ex_3 work with...

[python] Fix a few bugs introduced with recent changes to make ex_3 work with Talos and meshcat viewer.
parent c9618b99
......@@ -11,7 +11,7 @@ print("".center(conf.LINE_WIDTH,'#'))
print(" Test Task Space Inverse Dynamics - Biped ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH,'#'), '\n')
tsid = TsidBiped(conf)
tsid = TsidBiped(conf, conf.viewer)
N = conf.N_SIMULATION
com_pos = np.empty((3, N))*nan
......@@ -69,7 +69,7 @@ for i in range(0, N):
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
t += conf.dt
if i%conf.DISPLAY_N == 0: tsid.robot_display.display(q)
if i%conf.DISPLAY_N == 0: tsid.display(q)
time_spent = time.time() - time_start
if(time_spent < conf.dt): time.sleep(conf.dt-time_spent)
......
......@@ -5,13 +5,14 @@ import matplotlib.pyplot as plt
import plot_utils as plut
import time
import romeo_conf as conf
import pinocchio as pin
from tsid_biped import TsidBiped
print("".center(conf.LINE_WIDTH,'#'))
print(" TSID - Biped Sin Tracking ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH,'#'), '\n')
tsid = TsidBiped(conf)
tsid = TsidBiped(conf, conf.viewer)
N = conf.N_SIMULATION
com_pos = np.empty((3, N))*nan
......@@ -80,7 +81,7 @@ for i in range(0, N):
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
t += conf.dt
if i%conf.DISPLAY_N == 0: tsid.robot_display.display(q)
if i%conf.DISPLAY_N == 0: tsid.display(q)
time_spent = time.time() - time_start
if(time_spent < conf.dt): time.sleep(conf.dt-time_spent)
......
......@@ -14,6 +14,7 @@ import numpy as np
import pinocchio as pin
import talos_conf as conf
#import romeo_conf as conf
import vizutils
from tsid_biped import TsidBiped
......@@ -94,7 +95,7 @@ def switch_contact_LF():
def toggle_wireframe_mode():
tsid.viz.setWireFrameMode('world', 'WIREFRAME')
tsid.gui.setWireFrameMode('world', 'WIREFRAME')
def push_robot():
......@@ -168,7 +169,7 @@ def run_simu():
v = np.linalg.lstsq(A, b, rcond=-1)[0]
if i % conf.DISPLAY_N == 0:
tsid.viz.display(q)
tsid.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)
......@@ -196,7 +197,7 @@ print("#" * conf.LINE_WIDTH)
print(" Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#'))
print("#" * conf.LINE_WIDTH)
tsid = TsidBiped(conf)
tsid = TsidBiped(conf, conf.viewer)
tsid.q0[2] = 1.02127
com_0 = tsid.robot.com(tsid.formulation.data())
......
......@@ -7,6 +7,7 @@ Created on Thu Apr 18 09:47:07 2019
import numpy as np
import os
import pinocchio as pin
np.set_printoptions(precision=3, linewidth=200, suppress=True)
LINE_WIDTH = 60
......@@ -20,6 +21,7 @@ filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../models/romeo'
urdf = path + '/urdf/romeo.urdf'
srdf = path + '/srdf/romeo_collision.srdf'
nv = 37
foot_scaling = 1.
lxp = foot_scaling*0.10 # foot length in positive x direction
lxn = foot_scaling*0.05 # foot length in negative x direction
......@@ -68,9 +70,12 @@ kp_contact = 10.0 # proportional gain of contact constraint
kp_foot = 10.0 # proportional gain of contact constraint
kp_com = 10.0 # proportional gain of center of mass task
kp_posture = 1.0 # proportional gain of joint posture task
gain_vector = kp_posture*np.ones(nv-6)
masks_posture = np.ones(nv-6)
# configuration for viewer
# ----------------------------------------------
viewer = pin.visualize.GepettoVisualizer
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps
CAMERA_TRANSFORM = [3.578777551651001, 1.2937744855880737, 0.8885031342506409, 0.4116811454296112, 0.5468055009841919, 0.6109083890914917, 0.3978860676288605]
......@@ -20,7 +20,7 @@ PLOT_JOINT_VEL = 0
data = np.load(conf.DATA_FILE_TSID)
tsid = TsidBiped(conf)
tsid = TsidBiped(conf, conf.viewer)
N = data['com'].shape[1]
N_pre = int(conf.T_pre/conf.dt)
......@@ -146,7 +146,7 @@ for i in range(-N_pre, N+N_post):
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
t += conf.dt
if i%conf.DISPLAY_N == 0: tsid.robot_display.display(q)
if i%conf.DISPLAY_N == 0: tsid.display(q)
time_spent = time.time() - time_start
if(time_spent < conf.dt): time.sleep(conf.dt-time_spent)
......
......@@ -7,12 +7,14 @@ Created on Thu Apr 18 09:47:07 2019
import numpy as np
import os
import pinocchio as pin
np.set_printoptions(precision=3, linewidth=200, suppress=True)
LINE_WIDTH = 60
N_SIMULATION = 4000 # number of time steps simulated
dt = 0.002 # controller time step
nv = 37
lxp = 0.10 # foot length in positive x direction
lxn = 0.05 # foot length in negative x direction
......@@ -41,7 +43,10 @@ kp_contact = 10.0 # proportional gain of contact constraint
kp_foot = 10.0 # proportional gain of contact constraint
kp_com = 10.0 # proportional gain of center of mass task
kp_posture = 1.0 # proportional gain of joint posture task
gain_vector = kp_posture*np.ones(nv-6)
masks_posture = np.ones(nv-6)
viewer = pin.visualize.GepettoVisualizer
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps
CAMERA_TRANSFORM = [4.0, -0.2, 0.4, 0.5243823528289795, 0.518651008605957, 0.4620114266872406, 0.4925136864185333]
......
import os
import pinocchio as pin
import numpy as np
from example_robot_data.robots_loader import getModelPath
......@@ -75,6 +75,7 @@ kp_foot = 10.0 # proportional gain of contact constraint
kp_com = 10.0 # proportional gain of center of mass task
kp_posture = 1.0 # proportional gain of joint posture task
viewer = pin.visualize.GepettoVisualizer
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps
CAMERA_TRANSFORM = [4.0, -0.2, 0.4, 0.5243823528289795, 0.518651008605957, 0.4620114266872406, 0.4925136864185333]
......
......@@ -152,7 +152,7 @@ class TsidBiped:
self.viz.displayVisuals(True)
self.viz.display(q)
self.gui = self.robot_display.viewer.gui
self.gui = self.viz.viewer.gui
# self.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM)
self.gui.addFloor('world/floor')
self.gui.setLightingMode('world/floor', 'OFF')
......@@ -161,6 +161,9 @@ class TsidBiped:
self.robot_display.visual_model)
self.viz.initViewer(loadModel=True)
self.viz.display(q)
def display(self, q):
self.viz.display(q)
def integrate_dv(self, q, v, dv, dt):
v_mean = v + 0.5 * dt * dv
......
import meshcat
import numpy as np
import pinocchio as pin
......@@ -21,6 +20,7 @@ def meshcat_transform(x, y, z, q, u, a, t):
def addViewerBox(viz, name, sizex, sizey, sizez, rgba):
if isinstance(viz, pin.visualize.MeshcatVisualizer):
import meshcat
viz.viewer[name].set_object(meshcat.geometry.Box([sizex, sizey, sizez]),
meshcat_material(*rgba))
elif isinstance(viz, pin.visualize.GepettoVisualizer):
......@@ -31,6 +31,7 @@ def addViewerBox(viz, name, sizex, sizey, sizez, rgba):
def addViewerSphere(viz, name, size, rgba):
if isinstance(viz, pin.visualize.MeshcatVisualizer):
import meshcat
viz.viewer[name].set_object(meshcat.geometry.Sphere(size),
meshcat_material(*rgba))
elif isinstance(viz, pin.visualize.GepettoVisualizer):
......
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