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)