Commit 73913659 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

memmo summer-school: ex 3 with talos

parent d990c431
......@@ -5,114 +5,125 @@ Created on Wed Apr 17 22:31:22 2019
@author: student
"""
import threading
import time
import tkinter as tk
from tkinter import HORIZONTAL, Button, Entry, Frame, Label, Scale, Tk, mainloop
import numpy as np
import pinocchio as pin
import tkinter as tk
from tkinter import Scale, Button, Frame, Entry, Label, Tk, mainloop, HORIZONTAL
import threading
import talos_conf as conf
import vizutils
from tsid_biped import TsidBiped
AXES = ['X', 'Y', 'Z']
class Scale3d:
def __init__(self, master, name, from_, to, tickinterval, length, orient, command):
self.s = 3*[None]
axes = ['X','Y','Z']
self.s = 3 * [None]
for i in range(3):
self.s[i] = Scale(master, label=name+' '+axes[i], from_=from_[i], to=to[i],
tickinterval=tickinterval[i], orient=orient[i], length=length[i], command=command)
self.s[i] = Scale(master, label='%s %s' % (name, AXES[i]), from_=from_[i], to=to[i],
tickinterval=tickinterval[i], orient=orient[i], length=length[i], command=command)
self.s[i].pack()
separator = Frame(height=2, bd=1, relief=tk.SUNKEN)
separator.pack(fill=tk.X, padx=5, pady=5)
def get(self):
return self.s[0].get(), self.s[1].get(), self.s[2].get()
class Entry3d:
def __init__(self, master, name):
self.s = 3*[None]
axes = ['X','Y','Z']
self.s = 3 * [None]
for i in range(3):
Label(master, text=name+" "+axes[i]).pack() #side=tk.TOP)
Label(master, text='%s %s' % (name, AXES[i])).pack() # side=tk.TOP)
self.s[i] = Entry(master, width=5)
self.s[i].pack() #side=tk.BOTTOM)
self.s[i].pack() # side=tk.BOTTOM)
separator = Frame(height=1, bd=1, relief=tk.SUNKEN)
separator.pack(fill=tk.X, padx=2, pady=2) #, side=tk.BOTTOM)
separator.pack(fill=tk.X, padx=2, pady=2) # , side=tk.BOTTOM)
def get(self):
try:
return [float(self.s[i].get()) for i in range(3)]
except:
print("could not convert string to float", [self.s[i].get() for i in range(3)])
return 3*[0.0]
return 3 * [0.0]
scale_com, scale_RF, scale_LF = None, None, None
button_contact_RF, button_contact_LF = None, None
push_robot_active, push_robot_com_vel, com_vel_entry = False, 3*[0.0], None
push_robot_active, push_robot_com_vel, com_vel_entry = False, 3 * [0.0], None
def update_com_ref_scale(value):
x, y, z = scale_com.get()
tsid.trajCom.setReference(com_0 + np.array([1e-2*x, 1e-2*y, 1e-2*z]).T)
tsid.trajCom.setReference(com_0 + np.array([1e-2 * x, 1e-2 * y, 1e-2 * z]).T)
def update_RF_ref_scale(value):
x, y, z = scale_RF.get()
H_rf_ref = H_rf_0.copy()
H_rf_ref.translation += + np.array([1e-2*x, 1e-2*y, 1e-2*z]).T
H_rf_ref.translation += + np.array([1e-2 * x, 1e-2 * y, 1e-2 * z]).T
tsid.trajRF.setReference(H_rf_ref)
def update_LF_ref_scale(value):
x, y, z = scale_LF.get()
H_lf_ref = H_lf_0.copy()
H_lf_ref.translation += + np.array([1e-2*x, 1e-2*y, 1e-2*z]).T
H_lf_ref.translation += + np.array([1e-2 * x, 1e-2 * y, 1e-2 * z]).T
tsid.trajLF.setReference(H_lf_ref)
def switch_contact_RF():
if(tsid.contact_RF_active):
if tsid.contact_RF_active:
tsid.remove_contact_RF()
button_contact_RF.config(text='Make contact right foot')
else:
tsid.add_contact_RF()
button_contact_RF.config(text='Break contact right foot')
def switch_contact_LF():
if(tsid.contact_LF_active):
if tsid.contact_LF_active:
tsid.remove_contact_LF()
button_contact_LF.config(text='Make contact left foot')
else:
tsid.add_contact_LF()
button_contact_LF.config(text='Break contact left foot')
def toggle_wireframe_mode():
tsid.gui.setWireFrameMode('world', 'WIREFRAME')
tsid.viz.setWireFrameMode('world', 'WIREFRAME')
def push_robot():
global push_robot_com_vel, push_robot_active
push_robot_com_vel = com_vel_entry.get()
push_robot_active = True
def create_gui():
"""thread worker function"""
global scale_com, scale_RF, scale_LF, button_contact_RF, button_contact_LF, com_vel_entry
master = Tk(className='TSID GUI')
scale_com = Scale3d(master, 'CoM', [-10,-15,-40], [10,15,40], [5,5,10], [200,250,300],
scale_com = Scale3d(master, 'CoM', [-10, -15, -40], [10, 15, 40], [5, 5, 10], [200, 250, 300],
3*[HORIZONTAL], update_com_ref_scale)
scale_RF = Scale3d(master, 'Right foot', 3*[-30], 3*[30], 3*[10], 3*[300],
scale_RF = Scale3d(master, 'Right foot', 3 * [-30], 3 * [30], 3 * [10], 3 * [300],
3*[HORIZONTAL], update_RF_ref_scale)
scale_LF = Scale3d(master, 'Left foot', 3*[-30], 3*[30], 3*[10], 3*[300],
scale_LF = Scale3d(master, 'Left foot', 3 * [-30], 3 * [30], 3 * [10], 3 * [300],
3*[HORIZONTAL], update_LF_ref_scale)
button_contact_RF = Button(master, text='Break contact right foot', command=switch_contact_RF)
button_contact_RF.pack(side=tk.LEFT)
button_contact_LF = Button(master, text='Break contact left foot', command=switch_contact_LF)
button_contact_LF.pack(side=tk.LEFT)
Button(master, text='Toggle wireframe', command=toggle_wireframe_mode).pack(side=tk.LEFT)
# Frame(height=2, bd=1, relief=tk.SUNKEN).pack(fill=tk.X, padx=5, pady=5)
# Frame(height=2, bd=1, relief=tk.SUNKEN).pack(fill=tk.X, padx=5, pady=5)
Button(master, text='Push robot CoM', command=push_robot).pack()
com_vel_entry = Entry3d(master, 'CoM vel')
mainloop()
return
import numpy as np
import time
import romeo_conf as conf
from tsid_biped import TsidBiped
def run_simu():
global push_robot_active
......@@ -121,80 +132,81 @@ def run_simu():
time_avg = 0.0
while True:
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):
if sol.status != 0:
print("QP problem could not be solved! Error code:", sol.status)
break
# tau = tsid.formulation.getActuatorForces(sol)
# 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):
i, t = i + 1, t + conf.dt
if push_robot_active:
push_robot_active = False
data = tsid.formulation.data()
if(tsid.contact_LF_active):
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_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_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.robot_display.display(q)
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.position(tsid.formulation.data(), tsid.LF)
H_rf = tsid.robot.position(tsid.formulation.data(), tsid.RF)
x_lf_ref = tsid.trajLF.getSample(t).pos()[:3]
x_rf_ref = tsid.trajRF.getSample(t).pos()[:3]
tsid.gui.applyConfiguration('world/com', x_com.tolist()+[0,0,0,1.])
tsid.gui.applyConfiguration('world/com_ref', x_com_ref.tolist()+[0,0,0,1.])
tsid.gui.applyConfiguration('world/rf', pin.SE3ToXYZQUATtuple(H_rf))
tsid.gui.applyConfiguration('world/lf', pin.SE3ToXYZQUATtuple(H_lf))
tsid.gui.applyConfiguration('world/rf_ref', x_rf_ref.tolist()+[0,0,0,1.])
tsid.gui.applyConfiguration('world/lf_ref', x_lf_ref.tolist()+[0,0,0,1.])
if i%1000==0:
print("Average loop time: %.1f (expected is %.1f)"%(1e3*time_avg, 1e3*conf.dt))
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.])
if i % 1000 == 0:
print("Average loop time: %.1f (expected is %.1f)" % (1e3 * time_avg, 1e3 * conf.dt))
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)
time_avg = (i * time_avg + time_spent) / (i + 1)
if time_avg < 0.9 * conf.dt:
time.sleep(10 * (conf.dt - time_avg))
print("".center(conf.LINE_WIDTH,'#'))
print("#" * conf.LINE_WIDTH)
print(" Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH,'#'), '\n')
print("#" * conf.LINE_WIDTH)
tsid = TsidBiped(conf)
com_0 = tsid.robot.com(tsid.formulation.data())
H_rf_0 = tsid.robot.position(tsid.formulation.data(), tsid.model.getJointId(conf.rf_frame_name))
H_lf_0 = tsid.robot.position(tsid.formulation.data(), tsid.model.getJointId(conf.lf_frame_name))
tsid.gui.addSphere('world/com', conf.SPHERE_RADIUS, conf.COM_SPHERE_COLOR)
tsid.gui.addSphere('world/com_ref', conf.REF_SPHERE_RADIUS, conf.COM_REF_SPHERE_COLOR)
tsid.gui.addSphere('world/rf', conf.SPHERE_RADIUS, conf.RF_SPHERE_COLOR)
tsid.gui.addSphere('world/rf_ref', conf.REF_SPHERE_RADIUS, conf.RF_REF_SPHERE_COLOR)
tsid.gui.addSphere('world/lf', conf.SPHERE_RADIUS, conf.LF_SPHERE_COLOR)
tsid.gui.addSphere('world/lf_ref', conf.REF_SPHERE_RADIUS, conf.LF_REF_SPHERE_COLOR)
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)
th_gui = threading.Thread(target=create_gui)
th_gui.start()
......
import os
import numpy as np
from example_robot_data.robots_loader import getModelPath
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
lxp = 0.10 # foot length in positive x direction
lxn = 0.05 # foot length in negative x direction
lyp = 0.05 # foot length in positive y direction
lyn = 0.05 # foot length in negative y direction
lz = 0.07 # foot sole height with respect to ankle joint
mu = 0.3 # friction coefficient
fMin = 5.0 # minimum normal force
fMax = 1000.0 # maximum normal force
rf_frame_name = "leg_right_sole_fix_joint" # right foot frame name
lf_frame_name = "leg_left_sole_fix_joint" # left foot frame name
contactNormal = np.array([0., 0., 1.]) # direction of the normal to the contact surface
w_com = 1.0 # weight of center of mass task
w_foot = 1e-1 # weight of the foot motion task
w_contact = -1.0 # weight of foot in contact (negative means infinite weight)
w_posture = 1e-4 # weight of joint posture task
w_forceRef = 1e-5 # weight of force regularization task
w_torque_bounds = 1.0 # weight of the torque bounds
w_joint_bounds = 0.0
tau_max_scaling = 1.45 # scaling factor of torque bounds
v_max_scaling = 0.8
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
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]
SPHERE_RADIUS = 0.03
REF_SPHERE_RADIUS = 0.03
COM_SPHERE_COLOR = (1, 0.5, 0, 0.5)
COM_REF_SPHERE_COLOR = (1, 0, 0, 0.5)
RF_SPHERE_COLOR = (0, 1, 0, 0.5)
RF_REF_SPHERE_COLOR = (0, 1, 0.5, 0.5)
LF_SPHERE_COLOR = (0, 0, 1, 0.5)
LF_REF_SPHERE_COLOR = (0.5, 0, 1, 0.5)
urdf = "/talos_data/robots/talos_reduced.urdf"
path = getModelPath(urdf)
urdf = path + urdf
srdf = path + '/talos_data/srdf/talos.srdf'
import pinocchio as pin
import tsid
import numpy as np
import os
import gepetto.corbaserver
import time
import subprocess
import time
import numpy as np
import pinocchio as pin
import tsid
class TsidBiped:
......@@ -14,7 +14,7 @@ class TsidBiped:
- 6d rigid contact constraint for both feet
- Regularization task for contact forces
'''
def __init__(self, conf, viewer=True):
self.conf = conf
vector = pin.StdVec_StdString()
......@@ -25,104 +25,104 @@ class TsidBiped:
pin.loadReferenceConfigurations(self.model, conf.srdf, False)
self.q0 = q = self.model.referenceConfigurations["half_sitting"]
v = np.zeros(robot.nv)
assert self.model.existFrame(conf.rf_frame_name)
assert self.model.existFrame(conf.lf_frame_name)
formulation = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
contact_Point = np.ones((3,4)) * conf.lz
contact_Point = np.ones((3, 4)) * conf.lz
contact_Point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp]
contact_Point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp]
contactRF =tsid.Contact6d("contact_rfoot", robot, conf.rf_frame_name, contact_Point,
conf.contactNormal, conf.mu, conf.fMin, conf.fMax)
contactRF = tsid.Contact6d("contact_rfoot", robot, conf.rf_frame_name, contact_Point,
conf.contactNormal, conf.mu, conf.fMin, conf.fMax)
contactRF.setKp(conf.kp_contact * np.ones(6))
contactRF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6))
self.RF = robot.model().getJointId(conf.rf_frame_name)
H_rf_ref = robot.position(data, self.RF)
# modify initial robot configuration so that foot is on the ground (z=0)
q[2] -= H_rf_ref.translation[2] - conf.lz
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
H_rf_ref = robot.position(data, self.RF)
contactRF.setReference(H_rf_ref)
if(conf.w_contact>=0.0):
if conf.w_contact >= 0.0:
formulation.addRigidContact(contactRF, conf.w_forceRef, conf.w_contact, 1)
else:
formulation.addRigidContact(contactRF, conf.w_forceRef)
contactLF =tsid.Contact6d("contact_lfoot", robot, conf.lf_frame_name, contact_Point,
conf.contactNormal, conf.mu, conf.fMin, conf.fMax)
contactLF = tsid.Contact6d("contact_lfoot", robot, conf.lf_frame_name, contact_Point,
conf.contactNormal, conf.mu, conf.fMin, conf.fMax)
contactLF.setKp(conf.kp_contact * np.ones(6))
contactLF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6))
self.LF = robot.model().getJointId(conf.lf_frame_name)
H_lf_ref = robot.position(data, self.LF)
contactLF.setReference(H_lf_ref)
if(conf.w_contact>=0.0):
if conf.w_contact >= 0.0:
formulation.addRigidContact(contactLF, conf.w_forceRef, conf.w_contact, 1)
else:
formulation.addRigidContact(contactLF, conf.w_forceRef)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(conf.kp_com * np.ones(3))
comTask.setKd(2.0 * np.sqrt(conf.kp_com) * np.ones(3))
formulation.addMotionTask(comTask, conf.w_com, 1, 0.0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * np.ones(robot.nv-6))
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) * np.ones(robot.nv-6))
formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot, self.conf.lf_frame_name)
self.leftFootTask.setKp(self.conf.kp_foot * np.ones(6))
self.leftFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6))
self.trajLF = tsid.TrajectorySE3Constant("traj-left-foot", H_lf_ref)
formulation.addMotionTask(self.leftFootTask, self.conf.w_foot, 1, 0.0)
self.rightFootTask = tsid.TaskSE3Equality("task-right-foot", self.robot, self.conf.rf_frame_name)
self.rightFootTask.setKp(self.conf.kp_foot * np.ones(6))
self.rightFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6))
self.trajRF = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref)
formulation.addMotionTask(self.rightFootTask, self.conf.w_foot, 1, 0.0)
self.tau_max = conf.tau_max_scaling*robot.model().effortLimit[-robot.na:]
self.tau_min = -self.tau_max
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)
actuationBoundsTask.setBounds(self.tau_min, self.tau_max)
if(conf.w_torque_bounds>0.0):
if conf.w_torque_bounds > 0.0:
formulation.addActuationTask(actuationBoundsTask, conf.w_torque_bounds, 0, 0.0)
jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, conf.dt)
self.v_max = conf.v_max_scaling * robot.model().velocityLimit[-robot.na:]
self.v_min = -self.v_max
jointBoundsTask.setVelocityBounds(self.v_min, self.v_max)
if(conf.w_joint_bounds>0.0):
if conf.w_joint_bounds > 0.0:
formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0, 0.0)
com_ref = robot.com(data)
self.trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
self.sample_com = self.trajCom.computeNext()
q_ref = q[7:]
self.trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
postureTask.setReference(self.trajPosture.computeNext())
self.sampleLF = self.trajLF.computeNext()
self.sampleLF = self.trajLF.computeNext()
self.sample_LF_pos = self.sampleLF.pos()
self.sample_LF_vel = self.sampleLF.vel()
self.sample_LF_acc = self.sampleLF.acc()
self.sampleRF = self.trajRF.computeNext()
self.sampleRF = self.trajRF.computeNext()
self.sample_RF_pos = self.sampleRF.pos()
self.sample_RF_vel = self.sampleRF.vel()
self.sample_RF_acc = self.sampleRF.acc()
self.solver = tsid.SolverHQuadProgFast("qp solver")
self.solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
self.comTask = comTask
self.postureTask = postureTask
self.contactRF = contactRF
......@@ -132,110 +132,119 @@ class TsidBiped:
self.formulation = formulation
self.q = q
self.v = v
self.contact_LF_active = True
self.contact_RF_active = True
# for gepetto viewer
if(viewer):
self.robot_display = pin.RobotWrapper.BuildFromURDF(conf.urdf, [conf.path, ], pin.JointModelFreeFlyer())
l = subprocess.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
gepetto.corbaserver.Client()
self.robot_display.initViewer(loadModel=True)
self.robot_display.displayCollisions(False)
self.robot_display.displayVisuals(True)
self.robot_display.display(q)
self.gui = self.robot_display.viewer.gui
# self.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM)
self.gui.addFloor('world/floor')
self.gui.setLightingMode('world/floor', 'OFF');
if viewer:
self.robot_display = pin.RobotWrapper.BuildFromURDF(conf.urdf, [conf.path], pin.JointModelFreeFlyer())
try:
import gepetto.corbaserver
Viewer = pin.visualize.GepettoVisualizer
launched = subprocess.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(launched[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
self.viz = Viewer(self.robot_display.model, self.robot_display.collision_model,
self.robot_display.visual_model)
self.viz.initViewer(loadModel=True)
self.viz.displayCollisions(False)
self.viz.displayVisuals(True)
self.viz.display(q)
self.gui = self.robot_display.viewer.gui
# self.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM)
self.gui.addFloor('world/floor')
self.gui.setLightingMode('world/floor', 'OFF')
except ModuleNotFoundError: # Meshcat
Viewer = pin.visualize.MeshcatVisualizer
self.viz = Viewer(self.robot_display.model, self.robot_display.collision_model,
self.robot_display.visual_model)
self.viz.initViewer(loadModel=True)
self.viz.display(q)
def integrate_dv(self, q, v, dv, dt):
v_mean = v + 0.5*dt*dv
v += dt*dv
q = pin.integrate(self.model, q, dt*v_mean)
return q,v
v_mean = v + 0.5 * dt * dv
v += dt * dv
q = pin.integrate(self.model, q, dt * v_mean)
return q, v
def get_placement_LF(self):
return self.robot.position(self.formulation.data(), self.LF)
def get_placement_RF(self):
return self.robot.position(self.formulation.data(), self.RF)