nq, nv = robot.nq, robot.nv
if conf.use_viewer:
import subprocess, os
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)
robot.initViewer(loadModel=True)
robot.viewer.gui.createSceneWithFloor('world')
robot.viewer.gui.setLightingMode('world/floor', 'OFF')
# create feedback controller
if (ctrl_type == 'linear'):
refX, refU, feedBack = load_solo_ref_traj(robot, dt, motionName)
controller = LinearFeedbackController(robot, dt, refX, refU, feedBack)
q0, v0 = controller.q0, controller.v0
N = controller.refU.shape[0]
elif (ctrl_type == 'tsid-quadruped'):
controller = TsidQuadruped(conf,
dt,
robot,
com_offset,
com_freq,
com_amp,
conf.q0,
viewer=False)
q0, v0 = conf.q0, conf.v0
elif (ctrl_type == 'tsid-biped'):
controller = TsidBiped(conf, dt, conf.urdf, conf.modelPath, conf.srdf)
exp_max_mul = 100
int_max_mul = 100
robot = loadSolo(False)
nq, nv = robot.nq, robot.nv
if conf.use_viewer:
robot.initViewer(loadModel=True)
robot.viewer.gui.createSceneWithFloor('world')
robot.viewer.gui.setLightingMode('world/floor', 'OFF')
assert(np.floor(dt_ref/dt)==dt_ref/dt)
# load reference trajectories
refX, refU, feedBack = load_solo_ref_traj(robot, dt)
q0, v0 = refX[0,:nq], refX[0,nq:]
N_SIMULATION = refU.shape[0]
# TEMPORARY DEBUG CODE
N_SIMULATION = 40
#q0[2] += 1.0 # make the robot fly
def run_simulation(q0, v0, simu_params, ground_truth):
ndt = simu_params['ndt']
simu = RobotSimulator(conf, robot, pin.JointModelFreeFlyer())
for name in conf.contact_frames:
simu.add_contact(name, conf.contact_normal, conf.K, conf.B, conf.mu)
simu.init(q0, v0, p0=conf.p0)