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)
q0, v0 = controller.q0, controller.v0
N = controller.N + int((conf.T_pre + conf.T_post) / dt)
if (LOAD_GROUND_TRUTH_FROM_FILE):
print("\nLoad ground truth from file")
data = pickle.load(open(ground_truth_file_name, "rb"))
else:
data = {}
for item in GROUND_TRUTH_SIMU_PARAMS.items():
cpts = [] # list of all contact points
for cname in conf.contact_frames:
if not robot.model.existFrame(cname):
print(("ERROR: Frame", cname, "does not exist"))
cpts += [
sim.add_contact_point(cname, robot.model.getFrameId(cname),
unilateral_contacts)
]
print(" %s Contact Points Added ".center(conf.LINE_WIDTH, '-') %
(len(cpts)))
# reset simulator
sim.reset_state(q[0], v[0], True)
# inverse dynamics controller
invdyn = TsidQuadruped(conf, robot, q0, viewer=False)
offset = invdyn.robot.com(invdyn.formulation.data())
two_pi_f_amp = two_pi_f * amp
two_pi_f_squared_amp = two_pi_f * two_pi_f_amp
sampleCom = invdyn.trajCom.computeNext()
for ci, cp in enumerate(cpts):
sim_f[0, ci, :] = np.resize(cp.f, 3)
contact_x[0, ci, :] = np.resize(cp.x, 3)
contact_v[0, ci, :] = np.resize(cp.v, 3)
# for ci, cframe in enumerate(conf.contact_frames):
# print(('initial contact position for contact '+cframe))
# print((contact_x[0,ci,:]))