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'])