#============================================================================== if var_pl != 0: plt.figure(1) plt.clf() plt.plot(T, x_opt) plt.step(T, vertcat(DM.nan(1), u_opt)) plt.show() #============================================================================== # Animation the joint space trajectories # Plot the trajectory using the 2DoF pendulum URDF # with the prismatic joint at 0.3m fixed position #============================================================================== if var_ani != 0: nj = 2 j_name = [] for j in range(nj): tmp = "J%02d" % (j + 1) j_name.append(tmp) q_pl = np.zeros([N + 1, 2]) q_pl[:, 0] = phi_opt + np.pi qdot_pl = np.zeros([N + 1, 2]) qdot_pl[:, 0] = omega_opt pose = fn.RobotPose(j_name, q_pl, qdot_pl, [], int(1 / h)) pose.interpolate(30) js.posePublisher(pose) print len(lbg)
# print type(len(joint_str-nj)) if M == 1: zerovec = np.zeros([N_stage[0] + 1, len(joint_str) - nj]) else: zerovec = np.zeros([N_tot + 1, len(joint_str) - nj]) q_opt = np.concatenate((q_opt, zerovec), axis=1) qdot_opt = np.concatenate((qdot_opt, zerovec), axis=1) qddot_opt = np.concatenate((qddot_opt, zerovec), axis=1) tau_opt = np.concatenate((tau_opt, zerovec), axis=1) # print np.shape(q_opt) slomo = 1 # if slomo < 1.0 than the motion is slowed down # print np.shape(q_opt) pose = fn.RobotPose(name=joint_str, q=q_opt, qdot=qdot_opt, qddot=qddot_opt, tau=tau_opt, rate=int(slomo * 1 / dt_opt[0])) # print joint_str # print pose.name Tf_opt = dt_opt * np.array(N_stage).reshape(-1, 1) if M == 1: T_opt = np.zeros([N_stage[0] + 1, 1]) T_opt[0:N_stage[0] + 1] = np.matlib.linspace(0, Tf_opt[0], N_stage[0] + 1) else: T_opt = np.zeros([N_tot + 1, 1]) T_opt[0:N_stage[0] + 1] = np.matlib.linspace(0, Tf_opt[0], N_stage[0] + 1) T_opt[N_cum[0]:N_cum[1] + 1] = np.matlib.linspace(Tf_opt[0], Tf_opt[0] + Tf_opt[1], N_stage[1] + 1)
# RETRIEVE CONFIGURATION TRAJECTORY #============================================================================== xyz = np.zeros([N_tot + 1, 3]) for j in range(N_tot + 1): xyz[j, :] = forKin(q=q_opt[j, :])['ee_pos'].full().reshape(-1, 3) #============================================================================== # CREATE SOLVERPARAM & ROBOTPOSE CLASSES, THEN SAVE TO FILE IF DESIRED #============================================================================== # CREATE A CLASS FOR THIS WHERE WE STORE Q,QDOT,QDDOT,TAU AND ALL THE OPTIMIZER STUFF solver_param = fn.SolverParam(solver.stats(), N, h_opt, T, Tf, sol['f'].full()) pose = fn.RobotPose(q=q_opt, qdot=qdot_opt, qddot=qddot_opt, tau=tau_opt, rate=int(1 / h_opt[0])) if var_save: solver_param.saveMat() pose.saveMat() #============================================================================== # PRINT STATEMENTS #============================================================================== print "The initial time step size: h_0 = %s" % h0_vec print "The initial final time: Tf_0 = %s" % Tf0_vec.flatten() print "The optimized time step size: h_opt = %s" % h_opt.flatten().tolist() print "The optimized final time: Tf_opt = %s" % Tf_opt.flatten().tolist()
#============================================================================== if var_ani != 0: nj = 2 j_name = [] for j in range(nj): tmp = "J%02d" % (j + 1) j_name.append(tmp) tau_opt = np.vstack((tau_opt, tau_opt[-1, :])) # print np.shape(q_opt) # print np.shape(qdot_opt) # print np.shape(qddot_opt) # print np.shape(tau_opt) pose = fn.RobotPose(j_name, q_opt, qdot_opt, tau_opt, int(1 / h_opt)) # pose.interpolate(rviz_rate) if var_rec != 0: # os.system("ffmpeg -f x11grab -s 1900x1080 -r 25 -i :0.0 -qscale 5 ~/screenGrab.mpeg") # subprocess.call(['ffmpeg','-f','x11grab','-s','1900x1080','r','25','i',':0.0','-qscale','5','screenGrab.mpeg']) # subprocess.call("ffmpeg -f x11grab -s 1900x1080 -r 25 -i :0.0 -qscale 5 ~/screenGrab.mpeg", shell=True) # subproc = subprocess.Popen("ffmpeg -f x11grab -s 1900x1080 -r 25 -i :0.0 -qscale 5 screenGrab.mpeg", shell=True) js.posePublisher(pose) # subproc.kill() # os.system("q") else: js.posePublisher(pose) #============================================================================== # DEBUGGING AREA #==============================================================================
# TESTED: True path = '/home/user/catkin_ws/src/tameshiwari/python/OCP_centauro_7dof_arm/results' # filename = 'RobotPose_centauro_max_momentum_7dof_v1_2019-11-29T14:08:09.mat' # OLD but probably the same filename = 'RobotPose_centauro_max_momentum_7dof_v1_2019-12-04T10:09:13.mat' # NEW but probably the same ####################################### # LOAD FILE ####################################### matfile = sio.loadmat(path+'/'+filename) matfile['name'] = [str(x).strip() for x in matfile['name']] # matfile[''] pose = fn.RobotPose( name=matfile['name'], q=matfile['q'], qdot=matfile['qdot'], # tau=matfile['tau'], rate=matfile['rate'] ) rostopic = '/xbotcore/ros_command' # rostopic = '/pose_state' pubrate = 120.0 slowrate = 1.0 poserate = pubrate*slowrate pose.interpolate(poserate) if rostopic == '/pose_state': T = 0.5 else: T = 3
# RETRIEVE CONFIGURATION TRAJECTORY #============================================================================== xyz = np.zeros([N_tot + 1, 3]) for j in range(N_tot + 1): xyz[j, :] = forKin(q=q_opt[j, :])['ee_pos'].full().reshape(-1, 3) #============================================================================== # CREATE SOLVERPARAM & ROBOTPOSE CLASSES, THEN SAVE TO FILE IF DESIRED #============================================================================== # CREATE A CLASS FOR THIS WHERE WE STORE Q,QDOT,QDDOT,TAU AND ALL THE OPTIMIZER STUFF solver_param = fn.SolverParam(solver.stats(), N, h_opt, T, Tf, sol['f'].full()) pose = fn.RobotPose(name=['j_arm2_1', 'j_arm2_4'], q=q_opt, qdot=qdot_opt, qddot=qddot_opt, tau=tau_opt, rate=int(1 / h_opt[0])) if var_save: solver_param.saveMat() pose.saveMat() #============================================================================== # PRINT STATEMENTS #============================================================================== print "The initial time step size: h_0 = %s" % h0_vec print "The initial final time: Tf_0 = %s" % Tf0_vec print "The optimized time step size: h_opt = %s" % h_opt.flatten().tolist() print "The optimized final time: Tf_opt = %s" % Tf_opt.flatten().tolist()
if np.shape(pose.qdot)[0] > 1: state_str.velocity = pose.qdot[iteration, :] if nj_extra > 0: state_str.velocity = np.concatenate( (state_str.velocity, zerovec)) if np.shape(pose.tau)[0] > 1: state_str.effort = pose.tau[iteration, :] if nj_extra > 0: state_str.effort = np.concatenate((state_str.effort, zerovec)) # rospy.loginfo(state_str) # use for debugging pub.publish(state_str) iteration += 1 rate.sleep() if __name__ == '__main__': try: path = '/home/user/catkin_ws/results/' filename = 'RobotPose_centauro_max_momentum_3dof_2019-11-15T11:27:39.mat' matfile = sio.loadmat(path + filename) matfile['name'] = [str(x) for x in matfile['name']] pose = fn.RobotPose(name=matfile['name'], q=matfile['q'], qdot=matfile['qdot'], tau=matfile['tau'], rate=matfile['rate']) posePublisher(pose) except rospy.ROSInterruptException: pass
def invKin(fk=None,frame=None,j_str=None,q_init=None,p_init=None,p_des=None,T=None,animate=True): if not fk: print "forward kinematics function not provided" else: forKin = fk frame = frame or 'arm1_8' j_str = j_str or config.JointNames('arm1').getName() j_lim = config.JointBounds(j_str) q_lb = j_lim.getLowerBound() q_ub = j_lim.getUpperBound() T = T or 1 nq = forKin.numel_in() q = SX.sym('q',nq) if q_init is not None: p_init = forKin(q=q_init)['ee_pos'].full() elif p_init is None and q_init is None: print "ERROR: no initial q or p provided" p_des = p_des or [1.0, 0.0, 1.35] if isinstance(p_des,list): p_des = np.array(p_des) elif not isinstance(p_des,np.ndarray): print "ERROR: in p_des creation" p_des_sym = forKin(q=q)['ee_pos'] p_des_num = SX(p_des) J = dot(p_des_sym-p_des_num,p_des_sym-p_des_num) nlp = dict(f=J, x=q) opts = {"print_time":False} opts["ipopt"] = {"print_level":0} solver = nlpsol('solver','ipopt',nlp,opts) sol = solver(x0=q_init, lbx=q_lb, ubx=q_ub) q_sol = sol['x'].full().transpose() q_motion = np.append(np.array(q_init),q_sol.flatten()).reshape(2,-1) q_dot = np.zeros([2,nq]) print "######## RUNNING INVERSE KINEMATICS CHECK ########\n" print "Final joint configuration: %s [rad]" %q_sol.flatten() print "The desired xyz end-effector position: %s" %p_des.tolist() p_sol = np.around(forKin(q=q_sol)['ee_pos'].full().flatten(),2) print "The achieved xyz end-effector position: %s" %p_sol.tolist() e_des = sqrt(sol['f'].full().flatten()) print "The achieved ||des-res||_2 error: %s [mm]\n" %round(e_des*1000,0) tolerance = 0.005 # 5 millimeter tolerance = 0.05 # 5 centimeter if solver.stats()['return_status'] == 'Solve_Succeeded' and e_des <= tolerance: print "######## INVERSE KINEMATICS SUCCEEDED ########\n" cont = True else: print "######## ERROR ######## \n\nDesired end point is not in task space \n\n######## ERROR ########\n" cont = False if animate: pose = fn.RobotPose(name=j_str,q=q_motion,qdot=q_dot,rate=1/float(T)) pose.interpolate(30.0) js.posePublisher(pose) return cont, q_sol.flatten()
v_Nz = v_N[2] print "The optimized final EE velocity along the z-axis: %s [m/s]" % v_Nz print "The optimized final joint velocties: %s [rad/s]" % qdot_opt[ impact_ind, :] p_N = forKin(q=q_opt[impact_ind, :])['ee_pos'] print "The optimized final EE cartesian position: %s [m]" % p_N print "This position is w.r.t. origin of the world frame" print "The optimized final momentum h_iota: %s [kg m/s]" % h_iota_lin print "The optimized final momentum ||h_iota||_2: %s [kg m/s]" % h_iota_resultant slomo = 1 # if slomo < 1.0 than the motion is slowed down print np.shape(q_opt) pose = fn.RobotPose(name=joint_str, q=q_opt, qdot=qdot_opt, qddot=qddot_opt, tau=tau_opt, rate=int(slomo * 1 / dt_opt[0])) print joint_str print pose.name Tf_opt = dt_opt * np.array(N_stage).reshape(-1, 1) T_opt = np.zeros([N_stage[0] + 1, 1]) T_opt[0:N_stage[0] + 1] = np.matlib.linspace(0, Tf_opt[0], N_stage[0] + 1) # pose.plot_qdot(lb=qdot_lb,ub=qdot_ub,limits=True,Tvec=T_opt) pose.interpolate(30) save = False if save: pose.saveMat()