def sample_dynamics(self):
"""Sample the dynamics coefficients along the trajectory"""
self.ax_vect = zeros(self.n_steps)
self.bx_vect = zeros(self.n_steps)
self.cx_vect = zeros(self.n_steps)
self.ay_vect = zeros(self.n_steps)
self.by_vect = zeros(self.n_steps)
self.cy_vect = zeros(self.n_steps)
self.d_vect = zeros(self.n_steps)
self.e_vect = zeros(self.n_steps)
self.f_vect = zeros(self.n_steps)
for i in range(self.n_steps):
q = self.q_vect[:, i]
qd = self.qd_vect[:, i]
qdd = self.qdd_vect[:, i]
# Here we assume there is no baselink rotation
[ax, bx, cx, ay, by, cy, d, e, f] = ZMP.ComputeCoefsFractionZMP([
q[0:3], qd[0:3], qdd[0:3], q[6:len(q)], qd[6:len(q)],
qdd[6:len(q)]
], self.zmp_params)
self.ax_vect[i] = ax
self.bx_vect[i] = bx
self.cx_vect[i] = cx
self.ay_vect[i] = ay
self.by_vect[i] = by
self.cy_vect[i] = cy
self.d_vect[i] = d
self.e_vect[i] = e
self.f_vect[i] = f
def Execute(robot, traj, t_sleep, stepsize=1, drawcom=0):
global com_handle
n = robot.GetDOF()
for i in range(0, traj.n_steps, stepsize):
if traj.dim == n + 6:
robot.GetLinks()[0].SetTransform(HRP4.v2t(traj.q_vect[0:6, i]))
robot.SetDOFValues(traj.q_vect[6:traj.dim, i])
else:
robot.SetDOFValues(traj.q_vect[:, i])
if drawcom == 1:
CoM = ZMP.ComputeCOM([
robot.GetLinks()[0].GetTransform()[0:3, 3],
robot.GetDOFValues()
], {
'robot': robot,
'exclude_list': []
})
CoM_proj = zeros(3)
CoM_proj[0] = CoM[0]
CoM_proj[1] = CoM[1]
com_handle = robot.GetEnv().drawlinestrip(array([CoM, CoM_proj]),
5)
elif drawcom == 2:
q = traj.q_vect[:, i]
qd = traj.qd_vect[:, i]
qdd = traj.qdd_vect[:, i]
zmp = ZMP.ComputeZMP(
[
q[0:3], qd[0:3], qdd[0:3], q[6:len(q)], qd[6:len(q)],
qdd[6:len(q)]
], {
'robot': robot,
'exclude_list': [],
'gravity': 9.8,
'moment_coef': 1
})
zmp_proj = array([zmp[0], zmp[1], 0])
zmp_high = array([zmp[0], zmp[1], 0.5])
zmp_handle = robot.GetEnv().drawlinestrip(
array([zmp_proj, zmp_high]), 5)
time.sleep(t_sleep)
def ComputeCOM(sample_traj,params_init):
n_steps=sample_traj.n_steps
t_vect=sample_traj.t_vect
q_vect=sample_traj.q_vect
com_vect=zeros((3,n_steps))
for i in range(n_steps):
q=q_vect[:,i]
# Here we assume there is no base link rotation
com_vect[:,i]=ZMP.ComputeCOM([q[0:3],q[6:len(q)]],params_init)
return com_vect
def ComputeZMP(sample_traj,params_init):
n_steps=sample_traj.n_steps
t_vect=sample_traj.t_vect
q_vect=sample_traj.q_vect
qd_vect=sample_traj.qd_vect
qdd_vect=sample_traj.qdd_vect
zmp_vect=zeros((2,n_steps))
for i in range(n_steps):
q=q_vect[:,i]
qd=qd_vect[:,i]
qdd=qdd_vect[:,i]
# Here we assume there is no base link rotation
zmp=ZMP.ComputeZMP([q[0:3],qd[0:3],qdd[0:3],q[6:len(q)],qd[6:len(q)],qdd[6:len(q)]],params_init)
zmp_vect[:,i]=zmp
return zmp_vect
def Execute(robot,sample_traj,t_sleep,stepsize=1):
global com_handle
n=robot.GetDOF()
for i in range(0,sample_traj.n_steps,stepsize):
if sample_traj.dim==n+6:
robot.GetLinks()[0].SetTransform(HRP4.v2t(sample_traj.q_vect[0:6,i]))
robot.SetDOFValues(sample_traj.q_vect[6:sample_traj.dim,i])
else:
robot.SetDOFValues(sample_traj.q_vect[:,i])
CoM=ZMP.ComputeCOM([robot.GetLinks()[0].GetTransform()[0:3,3],robot.GetDOFValues()],{'robot':robot,'exclude_list':[]})
CoM_proj=zeros(3)
CoM_proj[0]=CoM[0]
CoM_proj[1]=CoM[1]
com_handle=robot.GetEnv().drawlinestrip(array([CoM,CoM_proj]),5)
time.sleep(t_sleep)
def sample_dynamics(self):
ax_vect = zeros(self.sample_n_steps)
bx_vect = zeros(self.sample_n_steps)
cx_vect = zeros(self.sample_n_steps)
ay_vect = zeros(self.sample_n_steps)
by_vect = zeros(self.sample_n_steps)
cy_vect = zeros(self.sample_n_steps)
d_vect = zeros(self.sample_n_steps)
e_vect = zeros(self.sample_n_steps)
f_vect = zeros(self.sample_n_steps)
self.env.GetPhysicsEngine().SetGravity([0, 0, -self.params['gravity']])
for i in range(self.sample_n_steps):
q = self.sample_q_vect[:, i]
qd = self.sample_qd_vect[:, i]
qdd = self.sample_qdd_vect[:, i]
# Here we assume there is no baselink rotation
[ax, bx, cx, ay, by, cy, d, e, f] = ZMP.ComputeCoefsFractionZMP([
q[0:3], qd[0:3], qdd[0:3], q[6:len(q)], qd[6:len(q)],
qdd[6:len(q)]
], self.params)
ax_vect[i] = ax
bx_vect[i] = bx
cx_vect[i] = cx
ay_vect[i] = ay
by_vect[i] = by
cy_vect[i] = cy
d_vect[i] = d
e_vect[i] = e
f_vect[i] = f
self.sample_ax_vect = ax_vect
self.sample_bx_vect = bx_vect
self.sample_cx_vect = cx_vect
self.sample_ay_vect = ay_vect
self.sample_by_vect = by_vect
self.sample_cy_vect = cy_vect
self.sample_d_vect = d_vect
self.sample_e_vect = e_vect
self.sample_f_vect = f_vect
base_T = traj.q_vect[0:3, i]
base_s = traj.qd_vect[0:3, i]
base_ss = traj.qdd_vect[0:3, i]
q = traj.q_vect[3:traj.n_steps, i]
qs = traj.qd_vect[3:traj.n_steps:, i]
qss = traj.qdd_vect[3:traj.n_steps:, i]
qd = sd * qs
qdd = sdd * qs + sd * sd * qss
base_vel = sd * base_s
base_acc = sdd * base_s + sd * sd * base_ss
config = [base_T, base_vel, base_acc, q, qd, qdd]
config_pure = [base_T, base_s, base_ss, q, qs, qss]
ZMP.ComputeCOM([base_T, q], params)
zmp = ZMP.ComputeZMP(config, params)
[ax, bx, cx, ay, by, cy, d, e,
f] = ZMP.ComputeCoefsFractionZMP(config_pure, params_all)
(ax * sdd + bx * sd * sd + cx) / (d * sdd + e * sd * sd + f) - zmp[0]
(ay * sdd + by * sd * sd + cy) / (d * sdd + e * sd * sd + f) - zmp[1]
deb = time.time()
for i in range(1000):
HRP4.SetConfig(robot, q_init)
robot.CalculateJacobian(0, [0, 0, 0])
print(time.time() - deb) / 1000
base_ss=array([-40,100,3])
q=traj.q_vect[:,i]
qs=traj.qd_vect[:,i]
qss=traj.qdd_vect[:,i]
qd=sd*qs
qdd=sdd*qs+sd*sd*qss
base_vel=sd*base_s
base_acc=sdd*base_s+sd*sd*base_ss
config=[q,qd,qdd,base_T,base_vel,base_acc]
config_pure=[q,qs,qss,base_T,base_s,base_ss]
ZMP.ComputeCOM(q,params_all)
ZMP.ComputeZMP(config,params_all)
[ax,bx,cx,ay,by,cy,d,e,f]=ZMP.ComputeCoefsFractionZMP(config_pure,params_all)
(ax*sdd+bx*sd*sd+cx)/(d*sdd+e*sd*sd+f)
(ay*sdd+by*sd*sd+cy)/(d*sdd+e*sd*sd+f)
deb=time.time()
for i in range(10):
print time.time()-deb
rq = Numdiff.ComputeJacobian(ZMP.com, q, 1e-7, {
'linkindex': i,
'robot': robot
})
print rq
print dot(rq, qd)
#### Test the basic ZMP computation
sd = 2
sdd = 1.5
f_ = array([0.2, -0.3, -0.4, 0.5])
fs = array([0.7, 0.4, 0.9, -1])
fss = array([0.9, -0.5, -0.4, -2])
q = f_
qd = sd * fs
qdd = sdd * fs + sd * sd * fss
ZMP.ComputeZMP(q, qd, qdd, params)
[ax, bx, cx, ay, by, cy, d, e,
f] = ZMP.ComputeCoefsFraction(f_, fs, fss, params)
(ax * sdd + bx * sd * sd + cx) / (d * sdd + e * sd * sd + f)
(ay * sdd + by * sd * sd + cy) / (d * sdd + e * sd * sd + f)
deb = time.time()
for i in range(100):
[ax, bx, cx, ay, by, cy, d, e,
f] = ZMP.ComputeCoefsFraction(f_, fs, fss, params)
print time.time() - deb
##################### Plotting ###########################
print('\n*****************************************************************\n')
print('Total execution time: ' + str(time.time() - deb) + 's')
print('\n*****************************************************************\n')
input(
'Press Enter to execute the trajectory /before/ time-reparameterization (duration='
+ str(traj.duration) + 's)')
MintimeProblemZMP.Execute(robot, traj, 0.02, drawcom=2)
input(
'Press Enter to execute the trajectory /after/ time-reparameterization (duration='
+ str(traj2.duration) + 's)')
MintimeProblemZMP.Execute(robot, traj2, 0.02, drawcom=2)
zmp = ZMP.ComputeZMPTraj(traj, params)
zmp2 = ZMP.ComputeZMPTraj(traj2, params)
com = ZMP.ComputeCOMTraj(traj, params)
figure(1)
clf()
algo.plot_profiles()
axis([0, 1.4, 0, 10])
title('Phase plane')
xlabel('$s$', fontsize=15)
ylabel('$\dot s$', fontsize=15)
#savefig('../../Reda/mintime/fig/zmp-phase.eps')
figure(2)
clf()
plot([xminf, xminf, xmaxf, xmaxf, xminf], [yminf, ymaxf, ymaxf, yminf, yminf],
def IKreach(drag, pinned_links, p_end):
global thread_params
robot = thread_params['robot']
p_step = thread_params['p_step']
Q0 = thread_params['Q0']
Q0inv = thread_params['Q0inv']
lower_lim = thread_params['lower_lim']
upper_lim = thread_params['upper_lim']
K_li = thread_params['K_li']
K_sr = thread_params['K_sr']
ndof = robot.GetDOF()
drag_link = drag[0]
drag_type = drag[1]
n_pins = len(pinned_links)
baselink = robot.GetLinks()[0]
link = robot.GetLinks()[drag_link]
p_init = link.GetGlobalCOM()
n_steps = norm(p_end - p_init) / p_step
for steps in range(int(n_steps) + 1):
p_cur = link.GetGlobalCOM()
T = baselink.GetTransform()
euler = HRP4.mat2euler(T[0:3, 0:3])
# Compute the dragged link Jacobian
if drag_type == 'translation':
J_drag_a = Jacobian(euler, drag_link, p_cur)
J_drag_b = Jacobian(euler, drag_link, p_cur + array([0, 0, 1]))
J_drag_c = Jacobian(euler, drag_link, p_cur + array([0, 1, 0]))
J_drag = concat([J_drag_a, J_drag_b, J_drag_c])
(k, nvar) = shape(J_drag_a)
else:
J_drag = Jacobian(euler, drag_link, p_cur)
(k, nvar) = shape(J_drag)
# Compute the desired_velocity
dist = norm(p_end - p_cur)
if dist < p_step:
v_drag_0 = p_end - p_cur
else:
v_drag_0 = (p_end - p_cur) / dist * p_step
if drag_type == 'translation':
v_drag = concat([v_drag_0, v_drag_0, v_drag_0])
else:
v_drag = v_drag_0
# Compute the Jacobians and the desired velocities of the pins
J_pins = None
v_pins = None
for i in range(n_pins):
pinned_i = pinned_links[i]
pinned_link = robot.GetLinks()[pinned_i]
CoMi = pinned_link.GetGlobalCOM()
J_pinned_ia = Jacobian(euler, pinned_i, CoMi)
J_pinned_ib = Jacobian(euler, pinned_i, CoMi + array([0, 0, 1]))
J_pinned_ic = Jacobian(euler, pinned_i, CoMi + array([0, 1, 0]))
J_pins = concat([J_pins, J_pinned_ia, J_pinned_ib, J_pinned_ic])
v_pins = concat([v_pins, zeros(3 * k)])
# Find the out-of-range DOFs
lower_list = []
upper_list = []
DOFvalues = robot.GetDOFValues()
for j in range(ndof):
if DOFvalues[j] < lower_lim[j]:
lower_list.append(j)
elif DOFvalues[j] > upper_lim[j]:
upper_list.append(j)
# Compute the Jacobians and the desired velocities for the out-of-range DOFs
J_lower = zeros((nvar, nvar))
v_lower = zeros(nvar)
J_upper = zeros((nvar, nvar))
v_upper = zeros(nvar)
for i in lower_list:
J_lower[6 + i, 6 + i] = 1
v_lower[6 + i] = K_li * (lower_lim[i] - DOFvalues[i])
for i in upper_list:
J_upper[6 + i, 6 + i] = 1
v_upper[6 + i] = K_li * (upper_lim[i] - DOFvalues[i])
J_limits = concat([J_lower, J_upper])
v_limits = concat([v_lower, v_upper])
# Computations
if thread_params['priority'] == 'drag':
J_main = J_drag
v_main = v_drag
J_aux = J_pins
v_aux = v_pins
else:
J_main = J_pins
v_main = v_pins
J_aux = J_drag
v_aux = v_drag
J_aux = concat([J_aux, J_limits])
v_aux = concat([v_aux, v_limits])
if J_main != None:
J_main_star = dot(J_main, Q0inv)
J_main_star_dash = linalg.pinv(J_main_star)
J_weighted_pinv = dot(Q0inv, J_main_star_dash)
thetad_0 = dot(J_weighted_pinv, v_main)
W = eye(nvar) - dot(J_weighted_pinv, J_main)
else:
thetad_0 = zeros(nvar)
W = eye(nvar)
v_aux_0 = dot(J_aux, thetad_0)
S = dot(J_aux, W)
[ms, ns] = shape(S)
delta_v_aux = v_aux - v_aux_0
Sstar = dot(transpose(S),
linalg.inv(dot(S, transpose(S)) + K_sr * eye(ms)))
y = dot(Sstar, delta_v_aux)
thetad = thetad_0 + dot(W, y)
HRP4.SetConfig(robot, HRP4.GetConfig(robot) + thetad)
#Update the positions of the spheres
for i in range(robot.GetDOF()):
if not (i in thread_params['exclude_list']):
UpdateSphere(i)
#Draw the COM
if thread_params['draw_com']:
#try:
# params['com_handle']=None
#except AttributeError:
# pass
CoM = ZMP.ComputeCOM([
robot.GetLinks()[0].GetTransform()[0:3, 3],
robot.GetDOFValues()
], {
'robot': robot,
'exclude_list': []
})
CoM_proj = zeros(3)
CoM_proj[0] = CoM[0]
CoM_proj[1] = CoM[1]
thread_params['com_handle'] = robot.GetEnv().drawlinestrip(
array([CoM, CoM_proj]), 5)
time.sleep(thread_params['timestep'])