v = np.zeros(12)
for j in range(3):
v[j] = M.translation[j]
v[j + 3] = M.rotation[j, 0]
v[j + 6] = M.rotation[j, 1]
v[j + 9] = M.rotation[j, 2]
return v
def MotiontoVec(M):
v = np.zeros(6)
for j in range(3):
v[j] = M.linear[j]
v[j + 3] = M.angular[j]
return v
tsid = TsidBiped(conf, conf.viewer)
i, t = 0, 0.0
q, v = tsid.q, tsid.v
cs = 0
phase_0_c = []
time_offset = 8000
sequence_change = True
swing_traj = []
while True:
if t < time_offset / 2.0 * conf.dt:
phase_0_c = get_COM_initial_traj(tsid.robot.com(tsid.formulation.data()))
sampleCom = curvesToTSID(phase_0_c,t)
tsid.comTask.setReference(sampleCom)
elif t >= time_offset / 2.0 * conf.dt and t < time_offset * conf.dt:
sampleCom = TSID.TrajectorySample(3)
import numpy as np
import numpy.matlib as matlib
from numpy import nan
from numpy.linalg import norm as norm
import matplotlib.pyplot as plt
import plot_utils as plut
import time
import romeo_conf as conf
from tsid_biped import TsidBiped
print "".center(conf.LINE_WIDTH, '#')
print " Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#')
print "".center(conf.LINE_WIDTH, '#'), '\n'
tsid = TsidBiped(conf)
N = conf.N_SIMULATION
com_pos = matlib.empty((3, N)) * nan
com_vel = matlib.empty((3, N)) * nan
com_acc = matlib.empty((3, N)) * nan
com_pos_ref = matlib.empty((3, N)) * nan
com_vel_ref = matlib.empty((3, N)) * nan
com_acc_ref = matlib.empty((3, N)) * nan
com_acc_des = matlib.empty(
(3, N)) * nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
offset = tsid.robot.com(tsid.formulation.data())
amp = np.matrix([0.0, 0.05, 0.0]).T
two_pi_f = 2 * np.pi * np.matrix([0.0, 0.5, 0.0]).T
two_pi_f_amp = np.multiply(two_pi_f, amp)
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)
print("".center(conf.LINE_WIDTH, '#'))
print(" Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH, '#'), '\n')
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)
#import ex_4_long_conf as conf
from tsid_biped import TsidBiped
print("".center(conf.LINE_WIDTH, '#'))
print(" Test Walking ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH, '#'), '\n')
PLOT_COM = 1
PLOT_COP = 1
PLOT_FOOT_TRAJ = 0
PLOT_TORQUES = 1
PLOT_JOINT_VEL = 1
data = np.load(conf.DATA_FILE_TSID)
tsid = TsidBiped(conf)
N = data['com'].shape[1]
N_pre = int(conf.T_pre / conf.dt)
N_post = int(conf.T_post / conf.dt)
com_pos = matlib.empty((3, N + N_post)) * nan
com_vel = matlib.empty((3, N + N_post)) * nan
com_acc = matlib.empty((3, N + N_post)) * nan
x_LF = matlib.empty((3, N + N_post)) * nan
dx_LF = matlib.empty((3, N + N_post)) * nan
ddx_LF = matlib.empty((3, N + N_post)) * nan
ddx_LF_des = matlib.empty((3, N + N_post)) * nan
x_RF = matlib.empty((3, N + N_post)) * nan
dx_RF = matlib.empty((3, N + N_post)) * nan
ddx_RF = matlib.empty((3, N + N_post)) * nan
import numpy as np
from numpy import nan
from numpy.linalg import norm as norm
import matplotlib.pyplot as plt
import plot_utils as plut
import time
import romeo_conf as conf
from tsid_biped import TsidBiped
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, conf.viewer)
N = conf.N_SIMULATION
com_pos = np.empty((3, N))*nan
com_vel = np.empty((3, N))*nan
com_acc = np.empty((3, N))*nan
com_pos_ref = np.empty((3, N))*nan
com_vel_ref = np.empty((3, N))*nan
com_acc_ref = np.empty((3, N))*nan
com_acc_des = np.empty((3, N))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
t = 0.0
q, v = tsid.q, tsid.v
for i in range(0, N):
time_start = time.time()
#import ex_4_long_conf as conf
from tsid_biped import TsidBiped
print("".center(conf.LINE_WIDTH, '#'))
print(" Test Walking ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH, '#'), '\n')
PLOT_COM = 0
PLOT_COP = 1
PLOT_FOOT_TRAJ = 0
PLOT_TORQUES = 0
PLOT_JOINT_VEL = 0
data = np.load(conf.DATA_FILE_TSID)
tsid = TsidBiped(conf, conf.viewer)
N = data['com'].shape[1]
N_pre = int(conf.T_pre / conf.dt)
N_post = int(conf.T_post / conf.dt)
com_pos = np.empty((3, N + N_post)) * nan
com_vel = np.empty((3, N + N_post)) * nan
com_acc = np.empty((3, N + N_post)) * nan
x_LF = np.empty((3, N + N_post)) * nan
dx_LF = np.empty((3, N + N_post)) * nan
ddx_LF = np.empty((3, N + N_post)) * nan
ddx_LF_des = np.empty((3, N + N_post)) * nan
x_RF = np.empty((3, N + N_post)) * nan
dx_RF = np.empty((3, N + N_post)) * nan
ddx_RF = np.empty((3, N + N_post)) * nan
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(10 * (conf.dt - time_avg))
print("#" * conf.LINE_WIDTH)
print(" Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#'))
print("#" * conf.LINE_WIDTH)
tsid = TsidBiped(conf, conf.viewer)
tsid.q0[2] = 1.02127
com_0 = tsid.robot.com(tsid.formulation.data())
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,
