def compute_Jacobian_speed(params):
robot=params['robot']
linkindex=params['linkindex']
linkindex2=params['linkindex2']
euler=HRP4.mat2euler(robot.GetLinks()[0].GetTransform()[0:3,0:3])
link2=robot.GetLinks()[linkindex2]
p_link2=link2.GetGlobalCOM()
return FollowTrajectory.Jacobian(euler,linkindex,p_link2,params)
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
config=array([ 3.33897e-01, 3.60542e-01, 8.00304e-01, 8.35608e-08,
3.31824e-08, -3.13480e+00, 0.00000e+00, 0.00000e+00,
0.00000e+00, 0.00000e+00, 0.00000e+00, 0.00000e+00,
0.00000e+00, 0.00000e+00, 0.00000e+00, 0.00000e+00,
0.00000e+00, 0.00000e+00, 0.00000e+00, 0.00000e+00,
0.00000e+00, 0.00000e+00, 0.00000e+00, 0.00000e+00,
0.00000e+00, 0.00000e+00, 0.00000e+00, 0.00000e+00,
0.00000e+00, 0.00000e+00, 0.00000e+00, 0.00000e+00,
0.00000e+00, 0.00000e+00, 0.00000e+00, 0.00000e+00,
0.00000e+00, 0.00000e+00, 2.83020e-01, -1.16476e-01,
0.00000e+00, -6.88414e-01, 0.00000e+00, 0.00000e+00,
0.00000e+00, -1.48802e-09, 0.00000e+00, 2.95042e-01,
1.05355e-01, 0.00000e+00, -6.89127e-01, 0.00000e+00,
0.00000e+00, 0.00000e+00, 7.01535e-09, 0.00000e+00])
HRP4.SetConfig(robot,config)
FollowTrajectory.compute_local_positions(robot,q,100,1,FollowTrajectory.markers_list)
Q0=eye(n+6)
Q0[0,0]=3
Q0[1,11]=3
Q0[2,2]=3
Q0[3,3]=2
Q0[4,4]=2
Q0[5,5]=2
params={'x':1}
params['robot']=robot
params['exclude_list']=exclude_list
params['dt']=0.1
params['Q0']=Q0
params['Q0inv']=linalg.inv(Q0)
params['lower_lim']=lower_lim
