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)))
robot.GetLinks()[0].GetTransform() robot.GetDOFValues() PinAndDrag.thread_params=params baselink=robot.GetLinks()[0] # Initial hrp.halfsit() config=HRP4.GetConfig(robot) p_init=robot.GetLinks()[18].GetGlobalCOM() center=baselink.GetTransform()[0:3,3] u=p_init-center R0=HRP4.rpy2mat(config[3:6]) #Prediction J=PinAndDrag.Jacobian(config[3:6],18,p_init) delta=zeros(56) delta[3:6]=1e-3*array([1,-1,2]) p_predicted=p_init+dot(J,delta) config2=config+delta R1=HRP4.rpy2mat(config2[3:6]) shift=dot(R1,u) p_predicted2=center+shift
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
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'])
