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