def DynamicShift(robot,traj,fixed_link_i):
fixed_link=robot.GetLinks()[fixed_link_i]
base_link=robot.GetLinks()[0]
traj2=copy.deepcopy(traj)
with robot:
HRP4.SetConfig(robot,traj.q_vect[:,0])
fixed_link_init=fixed_link.GetGlobalCOM()
for i in range(traj.n_steps):
with robot:
HRP4.SetConfig(robot,traj.q_vect[:,i])
fixed_link_cur=fixed_link.GetGlobalCOM()
shift=fixed_link_init-fixed_link_cur
traj2.q_vect[0:3,i]=traj2.q_vect[0:3,i]+shift
return traj2
def exec_traj(p_sphere_pos,v_sphere_pos,params):
global thread_data
global thread_lock
robot=params['robot']
#Reach the current position of p_sphere
HRP4.SetConfig(robot,thread_data['initial_q'])
config=Reach(params['linkindex'],params['linkindex2'],p_sphere_pos,params['n_steps'],params)
HRP4.SetConfig(robot,config)
#If Move_p, update the position of the v_sphere
if thread_data['state']=='Move_p':
Trans=eye(4)
Trans[0:3,3]=p_sphere_pos+thread_data['cur_vit']*params['dist']
thread_data['v_sphere'].SetTransform(Trans)
#Else, i.e. Move_v, update the current v
else:
thread_data['cur_vit']=(v_sphere_pos-p_sphere_pos)/params['dist']
#COmpute the deformed trajectory and move robot 2
traj=params['traj']
tau=params['tau']
T=params['T']
q_T=thread_data['initial_q']
q_Td=config
qd_T=thread_data['initial_qd']
J=compute_Jacobian_speed(params)
qd_Td=qd_T+dot(linalg.pinv(J),thread_data['cur_vit']-dot(J,qd_T))
if params['with_velocity']:
traj_def=Affine.deform_traj(traj,tau,T,q_Td,params['active_dofs'],qd_T,qd_Td)
else:
traj_def=Affine.deform_traj(traj,tau,T,q_Td,params['active_dofs'])
for k in range(traj_def.n_steps):
traj_def.q_vect[[0,1,2],k]+=params['offset']
robot2=params['robot2']
Trajectory.Execute(robot2,traj_def,0.001)
thread_data['traj_def']=traj_def
def start(params):
global thread_data
global thread_lock
robot=params['robot']
robot2=params['robot2']
traj=params['traj']
linkindex=params['linkindex']
linkindex2=params['linkindex2']
#Robot 2
final_config=array(traj.q_vect[:,-1])
final_config[[0,1,2]]+=params['offset']
HRP4.SetConfig(robot2,final_config)
#Find the initial speed of the sphere
T=params['T']
initial_q=traj.q_vect[:,T]
initial_qd=traj.q_vect[:,T]-traj.q_vect[:,T-1]
J_link2=compute_Jacobian_speed(params)
initial_sphere_speed=dot(J_link2,initial_qd)
#Initial positions of the spheres
p_sphere_pos=robot.GetLinks()[linkindex2].GetGlobalCOM()
v_sphere_pos=p_sphere_pos+initial_sphere_speed*params['dist']
p_sphere=CreateSphere(p_sphere_pos,[0,1,0],params)
v_sphere=CreateSphere(v_sphere_pos,[1,0,0],params)
thread_data={'continue':True}
thread_data['initial_q']=initial_q
thread_data['initial_qd']=initial_qd
thread_data['p_sphere']=p_sphere
thread_data['v_sphere']=v_sphere
thread_data['p_sphere_pos']=p_sphere_pos
thread_data['v_sphere_pos']=v_sphere_pos
thread_data['cur_vit']=initial_sphere_speed
thread_data['state']='Rest'
thread_lock=thread.allocate_lock()
thread.start_new_thread(update,(params,))
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
def Reach(markers,milestones,n_steps,params):
robot=params['robot']
Q0=params['Q0']
Q0inv=params['Q0inv']
lower_lim=params['lower_lim']
upper_lim=params['upper_lim']
K_v=params['K_v']
K_p=params['K_p']
K_li=params['K_li']
K_sr=params['K_sr']
n_dof=robot.GetDOF()
n_var=n_dof+6
baselink=robot.GetLinks()[0]
res=[]
res.append(HRP4.GetConfig(robot))
cur_config=res[0]
with robot:
for kk in range(len(milestones)):
for step in range(n_steps):
T=baselink.GetTransform()
euler=HRP4.mat2euler(T[0:3,0:3])
# Compute the Jacobians and desired velocities of the markers to follow
J_markers=None
v_desired=None
for j in range(len(markers)):
# Jacobian
linkj=robot.GetLinks()[markers[j]['link_index']]
p_cur=dot(linkj.GetTransform(),add1(markers[j]['local_pos']))[0:3]
J_markers=concat([J_markers,Jacobian(euler,markers[j]['link_index'],p_cur,params)])
# velocity
p_end=markers[j]['p_vect'][:,milestones[kk]]
v_desired=concat([v_desired,(p_end-p_cur)/(n_steps-step)])
# Find the out-of-range DOFs
lower_list=[]
upper_list=[]
DOFvalues=robot.GetDOFValues()
for j in range(n_dof):
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((n_var,n_var))
v_lower=zeros(n_var)
J_upper=zeros((n_var,n_var))
v_upper=zeros(n_var)
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])
# Inverse kinematics computations
# J_main=J_markers
# v_main=v_desired
# J_aux=J_limits
# v_aux=v_limits
# 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(n_var)-dot(J_weighted_pinv,J_main)
# 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)
J_main=concat([J_markers,J_limits])
v_main=concat([v_desired,v_limits])
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)
thetad=thetad_0
cur_config=cur_config+thetad
res.append(cur_config)
HRP4.SetConfig(robot,cur_config)
return Trajectory.SampleTrajectory(transpose(array(res)))
def Reach(linkindex,linkindex2,p_end,n_steps,params):
robot=params['robot']
Q0=params['Q0']
Q0inv=params['Q0inv']
lower_lim=params['lower_lim']
upper_lim=params['upper_lim']
K_v=params['K_v']
K_p=params['K_p']
K_li=params['K_li']
K_sr=params['K_sr']
n_dof=robot.GetDOF()
n_var=n_dof+6
baselink=robot.GetLinks()[0]
res=[]
res.append(HRP4.GetConfig(robot))
cur_config=res[0]
link2=robot.GetLinks()[linkindex2]
with robot:
for step in range(n_steps):
T=baselink.GetTransform()
euler=HRP4.mat2euler(T[0:3,0:3])
# Compute the Jacobians and desired velocities of the markers to follow
p_cur=link2.GetGlobalCOM()
J_marker=FollowTrajectory.Jacobian(euler,linkindex,p_cur,params)
v_desired=(p_end-p_cur)/(n_steps-step)
# Find the out-of-range DOFs
lower_list=[]
upper_list=[]
DOFvalues=robot.GetDOFValues()
for j in range(n_dof):
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((n_var,n_var))
v_lower=zeros(n_var)
J_upper=zeros((n_var,n_var))
v_upper=zeros(n_var)
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=FollowTrajectory.concat([J_lower,J_upper])
v_limits=FollowTrajectory.concat([v_lower,v_upper])
J_main=FollowTrajectory.concat([J_marker,J_limits])
v_main=FollowTrajectory.concat([v_desired,v_limits])
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)
thetad=thetad_0
cur_config=cur_config+thetad
res.append(cur_config)
HRP4.SetConfig(robot,cur_config)
return cur_config
-2.44140e-16, -1.38744e-16, -2.46371e-16, -2.65448e-02,
-1.11867e-16, 3.52050e-18, -1.60872e-16, -1.07424e-16,
-6.02101e-17, -1.16830e-16, -1.72097e-16, -1.01949e-16,
-4.88103e-17, -1.75168e-16, -1.45074e-17, 7.35638e-18,
6.67107e-21, -2.03958e-22, -9.36739e-22, -9.41175e-24,
2.72573e-27, -4.44707e-27, 9.01336e-27, 1.03314e-27,
6.81670e-27, -1.28606e-27, 3.77427e-01, -5.97724e-01,
1.05678e-02, -2.67625e-03, 6.18971e-01, 1.73002e-01,
-1.24488e+00, -1.41274e+00, 1.39935e-26, -5.13813e-18,
-3.40759e-18, 1.36842e-16, 0.00000e+00, -6.28591e-01,
8.13047e-01, -4.56412e-01, -1.86819e+00, 0.00000e+00,
0.00000e+00, 0.00000e+00, -1.61425e-16, 0.00000e+00])
PinAndDrag.stop()
HRP4.SetConfig(robot,config)
PinAndDrag.start(params)
PinAndDrag.thread_data['drag']=[7,'translation']
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'])
def run2():
params['with_velocity']=True
HRP4.SetConfig(robot,traj_mod.q_vect[:,params['T']])
AffineDemo.start(params)
def run():
HRP4.SetConfig(robot,traj_mod.q_vect[:,params['T']])
Trajectory.Execute(robot2,traj_mod_shift,0.001)
