def main(): try: rospy.init_node('avalos_limb_py') #Frecuency for Sawyer robot f = 100 rate = rospy.Rate(f) #Define topic pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) # Class to record data = Data() #Class limb to acces information sawyer limb = Limb() #Initial position limb.move_to_neutral() time.sleep(3) # Position init initial = limb.joint_angles() print "Posicion inicial terminada" #begin to record data data.writeon("cin_directa.txt") time.sleep(0.5) limb.move_to_joint_positions({ "right_j6": 0.0, "right_j5": 0.0, "right_j4": 0.0, "right_j3": 0.0, "right_j2": 0.0, "right_j1": 0.0, "right_j0": 0.0 }) limb.move_to_joint_positions({ "right_j6": initial["right_j6"], "right_j5": initial["right_j5"], "right_j4": initial["right_j4"], "right_j3": initial["right_j3"], "right_j2": initial["right_j2"], "right_j1": initial["right_j1"], "right_j0": initial["right_j0"] }) time.sleep(1) data.writeoff() print "FINISH" initial = limb.joint_angles() p0 = np.array([ initial["right_j0"], initial["right_j1"], initial["right_j2"], initial["right_j3"], initial["right_j4"], initial["right_j5"], initial["right_j6"] ]) # Posiition end p1 = [0, 0, 0, 0, 0, 0, 0] p2 = p0 # Knost vector time. We assum the process will take 10 sec k = np.array([0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1]) tiempo_estimado = 7 k_t = tiempo_estimado * k k_pt = np.array([k_t[0], k_t[5], k_t[-1]]) # Set inperpolation in linear form k_j0 = sp.interpolate.interp1d(k_pt, [p0[0], p1[0], p2[0]], kind='linear')(k_t) k_j1 = sp.interpolate.interp1d(k_pt, [p0[1], p1[1], p2[1]], kind='linear')(k_t) k_j2 = sp.interpolate.interp1d(k_pt, [p0[2], p1[2], p2[2]], kind='linear')(k_t) k_j3 = sp.interpolate.interp1d(k_pt, [p0[3], p1[3], p2[3]], kind='linear')(k_t) k_j4 = sp.interpolate.interp1d(k_pt, [p0[4], p1[4], p2[4]], kind='linear')(k_t) k_j5 = sp.interpolate.interp1d(k_pt, [p0[5], p1[5], p2[5]], kind='linear')(k_t) k_j6 = sp.interpolate.interp1d(k_pt, [p0[6], p1[6], p2[6]], kind='linear')(k_t) # Obtain all point following the interpolated points j0 = path_simple_cub_v0(k_j0, k_t, f) j1 = path_simple_cub_v0(k_j1, k_t, f) j2 = path_simple_cub_v0(k_j2, k_t, f) j3 = path_simple_cub_v0(k_j3, k_t, f) j4 = path_simple_cub_v0(k_j4, k_t, f) j5 = path_simple_cub_v0(k_j5, k_t, f) j6 = path_simple_cub_v0(k_j6, k_t, f) l = len(j0) print "l" print l # Vector for velocity v_j0 = np.zeros(l) v_j1 = np.zeros(l) v_j2 = np.zeros(l) v_j3 = np.zeros(l) v_j4 = np.zeros(l) v_j5 = np.zeros(l) v_j6 = np.zeros(l) # Vector for acceleration a_j0 = np.zeros(l) a_j1 = np.zeros(l) a_j2 = np.zeros(l) a_j3 = np.zeros(l) a_j4 = np.zeros(l) a_j5 = np.zeros(l) a_j6 = np.zeros(l) # Vector for jerk jk_j0 = np.zeros(l) jk_j1 = np.zeros(l) jk_j2 = np.zeros(l) jk_j3 = np.zeros(l) jk_j4 = np.zeros(l) jk_j5 = np.zeros(l) jk_j6 = np.zeros(l) for i in xrange(l - 1): v_j0[i] = (j0[i + 1] - j0[i]) * f v_j1[i] = (j1[i + 1] - j1[i]) * f v_j2[i] = (j2[i + 1] - j2[i]) * f v_j3[i] = (j3[i + 1] - j3[i]) * f v_j4[i] = (j4[i + 1] - j4[i]) * f v_j5[i] = (j5[i + 1] - j5[i]) * f v_j6[i] = (j6[i + 1] - j6[i]) * f v_j0[-1] = v_j0[-2] v_j1[-1] = v_j0[-2] v_j2[-1] = v_j0[-2] v_j3[-1] = v_j0[-2] v_j4[-1] = v_j0[-2] v_j5[-1] = v_j0[-2] v_j6[-1] = v_j0[-2] for i in xrange(l - 1): a_j0[i] = (v_j0[i + 1] - v_j0[i]) * f a_j1[i] = (v_j1[i + 1] - v_j1[i]) * f a_j2[i] = (v_j2[i + 1] - v_j2[i]) * f a_j3[i] = (v_j3[i + 1] - v_j3[i]) * f a_j4[i] = (v_j4[i + 1] - v_j4[i]) * f a_j5[i] = (v_j5[i + 1] - v_j5[i]) * f a_j6[i] = (v_j6[i + 1] - v_j6[i]) * f a_j0[-2] = a_j0[-3] a_j1[-2] = a_j1[-3] a_j2[-2] = a_j2[-3] a_j3[-2] = a_j3[-3] a_j4[-2] = a_j4[-3] a_j5[-2] = a_j5[-3] a_j6[-2] = a_j6[-3] a_j0[-1] = a_j0[-2] a_j1[-1] = a_j1[-2] a_j2[-1] = a_j2[-2] a_j3[-1] = a_j3[-2] a_j4[-1] = a_j4[-2] a_j5[-1] = a_j5[-2] a_j6[-1] = a_j6[-2] for i in xrange(l - 1): jk_j0[i] = (a_j0[i + 1] - a_j0[i]) * f jk_j1[i] = (a_j1[i + 1] - a_j1[i]) * f jk_j2[i] = (a_j2[i + 1] - a_j2[i]) * f jk_j3[i] = (a_j3[i + 1] - a_j3[i]) * f jk_j4[i] = (a_j4[i + 1] - a_j4[i]) * f jk_j5[i] = (a_j5[i + 1] - a_j5[i]) * f jk_j6[i] = (a_j6[i + 1] - a_j6[i]) * f jk_j0[-3] = jk_j0[-4] jk_j1[-3] = jk_j1[-4] jk_j2[-3] = jk_j2[-4] jk_j3[-3] = jk_j3[-4] jk_j4[-3] = jk_j4[-4] jk_j5[-3] = jk_j5[-4] jk_j6[-3] = jk_j6[-4] jk_j0[-2] = jk_j0[-3] jk_j1[-2] = jk_j1[-3] jk_j2[-2] = jk_j2[-3] jk_j3[-2] = jk_j3[-3] jk_j4[-2] = jk_j4[-3] jk_j5[-2] = jk_j5[-3] jk_j6[-2] = jk_j6[-3] jk_j0[-1] = jk_j0[-2] jk_j1[-1] = jk_j1[-2] jk_j2[-1] = jk_j2[-2] jk_j3[-1] = jk_j3[-2] jk_j4[-1] = jk_j4[-2] jk_j5[-1] = jk_j5[-2] jk_j6[-1] = jk_j6[-2] j = np.array([j0, j1, j2, j3, j4, j5, j6]) v = np.array([v_j0, v_j1, v_j2, v_j3, v_j4, v_j5, v_j6]) a = np.array([a_j0, a_j1, a_j2, a_j3, a_j4, a_j5, a_j6]) jk = np.array([jk_j0, jk_j1, jk_j2, jk_j3, jk_j4, jk_j5, jk_j6]) save_matrix(j, "data_p.txt", f) save_matrix(v, "data_v.txt", f) save_matrix(a, "data_a.txt", f) save_matrix(jk, "data_y.txt", f) #Build message my_msg = JointCommand() # POSITION_MODE my_msg.mode = 4 my_msg.names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] data.writeon("cin_trayectoria.txt") time.sleep(0.5) for i in xrange(l): my_msg.position = [j0[i], j1[i], j2[i], j3[i], j4[i], j5[i], j6[i]] my_msg.velocity = [ v_j0[i], v_j1[i], v_j2[i], v_j3[i], v_j4[i], v_j5[i], v_j6[i] ] my_msg.acceleration = [ a_j0[i], a_j1[i], a_j2[i], a_j3[i], a_j4[i], a_j5[i], a_j6[i] ] pub.publish(my_msg) rate.sleep() time.sleep(1) data.writeoff() print "Programa terminado " except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user.')
def main(): try: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('avalos_limb_py', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group_name = 'right_arm' group = moveit_commander.MoveGroupCommander(group_name) #frecuency for Sawyer robot f = 100 alfa = 0.9 rate = rospy.Rate(f) # add gripper gripper = intera_interface.Gripper('right_gripper') gripper.calibrate() gripper.open() moveit_robot_state = RobotState() joint_state = JointState() joint_state.header = Header() joint_state.header.stamp = rospy.Time.now() joint_state.name = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] #Define topic pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) limb = Limb() limb.move_to_neutral() limb.move_to_joint_positions({"right_j6": 2}) group.clear_pose_targets() group.set_start_state_to_current_state() # We can get the joint values from the group and adjust some of the values: pose_goal = geometry_msgs.msg.Pose() # Orientation pose_goal.orientation.x = -1 pose_goal.orientation.y = 0.0 pose_goal.orientation.z = 0.0 pose_goal.orientation.w = 0.0 q0 = np.array([]) q1 = np.array([]) q2 = np.array([]) q3 = np.array([]) q4 = np.array([]) q5 = np.array([]) q6 = np.array([]) # Cartesian position - Carga '01' pose_goal.position.x = 0.758552 pose_goal.position.y = -0.3435 pose_goal.position.z = 0.25 group.set_pose_target(pose_goal) carga1 = group.plan().joint_trajectory.points n = len(carga1) joint_state.position = [ carga1[-1].positions[0], carga1[-1].positions[1], carga1[-1].positions[2], carga1[-1].positions[3], carga1[-1].positions[4], carga1[-1].positions[5], carga1[-1].positions[6] ] moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) tmp = np.array([]) if (n > 8): tmp = np.append(tmp, 0) k = int(math.sqrt(n) + 2) r = int((n - 1) / float(k)) for i in range(k): print i tmp = np.append(tmp, int(r * (i + 1) - 1)) tmp = np.append(tmp, n - 1) else: for i in range(n): print i tmp = np.append(tmp, i) print "tmp:", tmp for i in range(len(tmp)): q0 = np.append(q0, carga1[int(tmp[i])].positions[0]) q1 = np.append(q1, carga1[int(tmp[i])].positions[1]) q2 = np.append(q2, carga1[int(tmp[i])].positions[2]) q3 = np.append(q3, carga1[int(tmp[i])].positions[3]) q4 = np.append(q4, carga1[int(tmp[i])].positions[4]) q5 = np.append(q5, carga1[int(tmp[i])].positions[5]) q6 = np.append(q6, carga1[int(tmp[i])].positions[6]) print "q000", q0 # Cartesian position - Carga '00' #pose_goal.position.x = 0.85 #pose_goal.position.y = -0.4 pose_goal.position.z = -0.01 group.set_pose_target(pose_goal) carga0 = group.plan().joint_trajectory.points q0 = np.append(q0, carga0[-1].positions[0]) q1 = np.append(q1, carga0[-1].positions[1]) q2 = np.append(q2, carga0[-1].positions[2]) q3 = np.append(q3, carga0[-1].positions[3]) q4 = np.append(q4, carga0[-1].positions[4]) q5 = np.append(q5, carga0[-1].positions[5]) q6 = np.append(q6, carga0[-1].positions[6]) #''' q = np.array([q0, q1, q2, q3, q4, q5, q6]) print "q001", q0 m_time, t_min_tiempo = min_time(q) start = time.time() opt = Opt_avalos(q, f, 0.9) v_time = opt.full_time() j_1, v_1, a_1, jk_1 = generate_path_cub(q, v_time, f) ext_1 = len(j_1[0, :]) end = time.time() print('Process Time:', end - start) v_jk = sqrt(np.mean(np.square(jk_1))) print("Opt Time:", v_time) print("Min Time:", m_time) #print('Optimizacion:',opt.result()) max_v = np.amax(np.absolute(v_1)) max_ac = np.amax(np.absolute(a_1)) max_jk = np.amax(np.absolute(jk_1)) print "Max Velo:", max_v print "Max Acel:", max_ac print "Max Jerk:", max_jk #raw_input('Iniciar_CT_execute?') #Build message my_msg = JointCommand() # POSITION_MODE my_msg.mode = 4 my_msg.names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5" ] #,"right_j6"] print("Control por trayectoria iniciado.") #time.sleep(0.25) q0 = np.array([]) q1 = np.array([]) q2 = np.array([]) q3 = np.array([]) q4 = np.array([]) q5 = np.array([]) q6 = np.array([]) q0 = np.append(q0, carga0[-1].positions[0]) q1 = np.append(q1, carga0[-1].positions[1]) q2 = np.append(q2, carga0[-1].positions[2]) q3 = np.append(q3, carga0[-1].positions[3]) q4 = np.append(q4, carga0[-1].positions[4]) q5 = np.append(q5, carga0[-1].positions[5]) q6 = np.append(q6, carga0[-1].positions[6]) joint_state.position = [ carga1[-1].positions[0], carga1[-1].positions[1], carga1[-1].positions[2], carga1[-1].positions[3], carga1[-1].positions[4], carga1[-1].positions[5], carga1[-1].positions[6] ] moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) # Cartesian position - Base '01' pose_goal.position.x = 0.80791 pose_goal.position.y = 0.4461 pose_goal.position.z = 0.2501 group.set_pose_target(pose_goal) base1 = group.plan().joint_trajectory.points n = len(base1) joint_state.position = [ base1[-1].positions[0], base1[-1].positions[1], base1[-1].positions[2], base1[-1].positions[3], base1[-1].positions[4], base1[-1].positions[5], base1[-1].positions[6] ] moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) tmp = np.array([]) if (n > 14): tmp = np.append(tmp, 0) k = int(math.sqrt(n) + 3) r = int((n - 1) / float(k)) for i in range(k): print i tmp = np.append(tmp, int(r * (i + 1) - 1)) tmp = np.append(tmp, n - 1) else: for i in range(n): print i tmp = np.append(tmp, i) print "tmp:", tmp for i in range(len(tmp)): q0 = np.append(q0, base1[int(tmp[i])].positions[0]) q1 = np.append(q1, base1[int(tmp[i])].positions[1]) q2 = np.append(q2, base1[int(tmp[i])].positions[2]) q3 = np.append(q3, base1[int(tmp[i])].positions[3]) q4 = np.append(q4, base1[int(tmp[i])].positions[4]) q5 = np.append(q5, base1[int(tmp[i])].positions[5]) q6 = np.append(q6, base1[int(tmp[i])].positions[6]) print "q000", q0 # Cartesian position - Base '00' #pose_goal.position.x = 0.90 #pose_goal.position.y = 0.38 pose_goal.position.z = 0.01 group.set_pose_target(pose_goal) base0 = group.plan().joint_trajectory.points q0 = np.append(q0, base0[-1].positions[0]) q1 = np.append(q1, base0[-1].positions[1]) q2 = np.append(q2, base0[-1].positions[2]) q3 = np.append(q3, base0[-1].positions[3]) q4 = np.append(q4, base0[-1].positions[4]) q5 = np.append(q5, base0[-1].positions[5]) q6 = np.append(q6, base0[-1].positions[6]) q = np.array([q0, q1, q2, q3, q4, q5, q6]) print "q001", q0 #print q[0] #return 0 m_time, t_min_tiempo = min_time(q) start = time.time() opt = Opt_avalos(q, f, alfa) v_time = opt.full_time() j_2, v_2, a_2, jk_2 = generate_path_cub(q, v_time, f) ext_2 = len(j_2[0, :]) end = time.time() print('Process Time:', end - start) #save_matrix(j,"data_p.txt",f) #save_matrix(v,"data_v.txt",f) #save_matrix(a,"data_a.txt",f) #save_matrix(jk,"data_y.txt",f) v_jk = sqrt(np.mean(np.square(jk_2))) print("Opt Time:", v_time) print("Min Time:", m_time) #print('Optimizacion:',opt.result()) max_v = np.amax(np.absolute(v_2)) max_ac = np.amax(np.absolute(a_2)) max_jk = np.amax(np.absolute(jk_2)) print "Max Velo:", max_v print "Max Acel:", max_ac print "Max Jerk:", max_jk q0 = np.array([]) q1 = np.array([]) q2 = np.array([]) q3 = np.array([]) q4 = np.array([]) q5 = np.array([]) q6 = np.array([]) q0 = np.append(q0, base0[-1].positions[0]) q1 = np.append(q1, base0[-1].positions[1]) q2 = np.append(q2, base0[-1].positions[2]) q3 = np.append(q3, base0[-1].positions[3]) q4 = np.append(q4, base0[-1].positions[4]) q5 = np.append(q5, base0[-1].positions[5]) q6 = np.append(q6, base0[-1].positions[6]) # Cartesian position - Carga '01' pose_goal.position.x = 0.7708552 pose_goal.position.y = -0.394135 pose_goal.position.z = 0.24 group.set_pose_target(pose_goal) carga1 = group.plan().joint_trajectory.points n = len(carga1) tmp = np.array([]) if (n > 10): tmp = np.append(tmp, 0) k = int(math.sqrt(n) + 2) r = int((n - 1) / float(k)) for i in range(k): print i tmp = np.append(tmp, int(r * (i + 1) - 1)) tmp = np.append(tmp, n - 1) else: for i in range(n): print i tmp = np.append(tmp, i) print "tmp:", tmp for i in range(len(tmp)): q0 = np.append(q0, carga1[int(tmp[i])].positions[0]) q1 = np.append(q1, carga1[int(tmp[i])].positions[1]) q2 = np.append(q2, carga1[int(tmp[i])].positions[2]) q3 = np.append(q3, carga1[int(tmp[i])].positions[3]) q4 = np.append(q4, carga1[int(tmp[i])].positions[4]) q5 = np.append(q5, carga1[int(tmp[i])].positions[5]) q6 = np.append(q6, carga1[int(tmp[i])].positions[6]) q = np.array([q0, q1, q2, q3, q4, q5, q6]) print "q001", q0 #print q[0] #return 0 m_time, t_min_tiempo = min_time(q) start = time.time() opt = Opt_avalos(q, f, 0.8) v_time = opt.full_time() j_3, v_3, a_3, jk_3 = generate_path_cub(q, v_time, f) ext_3 = len(j_3[0, :]) end = time.time() print('Process Time:', end - start) #save_matrix(j,"data_p.txt",f) #save_matrix(v,"data_v.txt",f) #save_matrix(a,"data_a.txt",f) #save_matrix(jk,"data_y.txt",f) v_jk = sqrt(np.mean(np.square(jk_3))) print("Opt Time:", v_time) print("Min Time:", m_time) #print('Optimizacion:',opt.result()) max_v = np.amax(np.absolute(v_3)) max_ac = np.amax(np.absolute(a_3)) max_jk = np.amax(np.absolute(jk_3)) print "Max Velo:", max_v print "Max Acel:", max_ac print "Max Jerk:", max_jk raw_input('Iniciar_CT_execute?') #Build message for n in xrange(ext_1): my_msg.position = [ j_1[0][n], j_1[1][n], j_1[2][n], j_1[3][n], j_1[4][n], j_1[5][n] ] #,j_1[6][n]] my_msg.velocity = [ v_1[0][n], v_1[1][n], v_1[2][n], v_1[3][n], v_1[4][n], v_1[5][n] ] #,v_1[6][n]] my_msg.acceleration = [ a_1[0][n], a_1[1][n], a_1[2][n], a_1[3][n], a_1[4][n], a_1[5][n] ] #,a_1[6][n]] pub.publish(my_msg) rate.sleep() print("Control por trayectoria terminado.") time.sleep(0.25) gripper.close() print("Control por trayectoria iniciado.") #time.sleep(0.25) for n in xrange(ext_2): my_msg.position = [ j_2[0][n], j_2[1][n], j_2[2][n], j_2[3][n], j_2[4][n], j_2[5][n] ] #,j_2[6][n]] my_msg.velocity = [ v_2[0][n], v_2[1][n], v_2[2][n], v_2[3][n], v_2[4][n], v_2[5][n] ] #,v_2[6][n]] my_msg.acceleration = [ a_2[0][n], a_2[1][n], a_2[2][n], a_2[3][n], a_2[4][n], a_2[5][n] ] #,a_2[6][n]] pub.publish(my_msg) rate.sleep() print("Control por trayectoria terminado.") gripper.open() time.sleep(0.5) #data.writeoff() print("Programa terminado.") for n in xrange(ext_3): my_msg.position = [ j_3[0][n], j_3[1][n], j_3[2][n], j_3[3][n], j_3[4][n], j_3[5][n] ] #,j_3[6][n]] my_msg.velocity = [ v_3[0][n], v_3[1][n], v_3[2][n], v_3[3][n], v_3[4][n], v_3[5][n] ] #,v_3[6][n]] my_msg.acceleration = [ a_3[0][n], a_3[1][n], a_3[2][n], a_3[3][n], a_3[4][n], a_3[5][n] ] #,a_3[6][n]] pub.publish(my_msg) rate.sleep() print("Control por trayectoria terminado.") gripper.open() #data.writeoff() print("Programa terminado.") except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user.')
def move2cartesian(position=None, orientation=None, relative_pose=None, in_tip_frame=False, joint_angles=[], tip_name='right_hand', linear_speed=0.6, linear_accel=0.6, rotational_speed=1.57, rotational_accel=1.57, timeout=None, neutral=False): """ Move the robot arm to the specified configuration. Call using: $ rosrun intera_examples go_to_cartesian_pose.py [arguments: see below] -p 0.4 -0.3 0.18 -o 0.0 1.0 0.0 0.0 -t right_hand --> Go to position: x=0.4, y=-0.3, z=0.18 meters --> with quaternion orientation (0, 1, 0, 0) and tip name right_hand --> The current position or orientation will be used if only one is provided. -q 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 --> Go to joint angles: 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 using default settings --> If a Cartesian pose is not provided, Forward kinematics will be used --> If a Cartesian pose is provided, the joint angles will be used to bias the nullspace -R 0.01 0.02 0.03 0.1 0.2 0.3 -T -> Jog arm with Relative Pose (in tip frame) -> x=0.01, y=0.02, z=0.03 meters, roll=0.1, pitch=0.2, yaw=0.3 radians -> The fixed position and orientation paramters will be ignored if provided """ try: #rospy.init_node('go_to_cartesian_pose_py') limb = Limb() traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options = traj_options, limb = limb) wpt_opts = MotionWaypointOptions(max_linear_speed=linear_speed, max_linear_accel=linear_accel, max_rotational_speed=rotational_speed, max_rotational_accel=rotational_accel, max_joint_speed_ratio=1.0) waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb) joint_names = limb.joint_names() if joint_angles and len(joint_angles) != len(joint_names): rospy.logerr('len(joint_angles) does not match len(joint_names!)') return None if neutral == True: limb.move_to_neutral() else: if (position is None and orientation is None and relative_pose is None): if joint_angles: # does Forward Kinematics waypoint.set_joint_angles(joint_angles, tip_name, joint_names) else: rospy.loginfo("No Cartesian pose or joint angles given. Using default") waypoint.set_joint_angles(joint_angles=None, active_endpoint=tip_name) else: endpoint_state = limb.tip_state(tip_name) if endpoint_state is None: rospy.logerr('Endpoint state not found with tip name %s', tip_name) return None pose = endpoint_state.pose if relative_pose is not None: if len(relative_pose) != 6: rospy.logerr('Relative pose needs to have 6 elements (x,y,z,roll,pitch,yaw)') return None # create kdl frame from relative pose rot = PyKDL.Rotation.RPY(relative_pose[3], relative_pose[4], relative_pose[5]) trans = PyKDL.Vector(relative_pose[0], relative_pose[1], relative_pose[2]) f2 = PyKDL.Frame(rot, trans) # and convert the result back to a pose message if in_tip_frame: # end effector frame pose = posemath.toMsg(posemath.fromMsg(pose) * f2) else: # base frame pose = posemath.toMsg(f2 * posemath.fromMsg(pose)) else: if position is not None and len(position) == 3: pose.position.x = position[0] pose.position.y = position[1] pose.position.z = position[2] if orientation is not None and len(orientation) == 4: pose.orientation.x = orientation[0] pose.orientation.y = orientation[1] pose.orientation.z = orientation[2] pose.orientation.w = orientation[3] poseStamped = PoseStamped() poseStamped.pose = pose if not joint_angles: # using current joint angles for nullspace bais if not provided joint_angles = limb.joint_ordered_angles() waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles) else: waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles) rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string()) traj.append_waypoint(waypoint.to_msg()) result = traj.send_trajectory(timeout=timeout) if result is None: rospy.logerr('Trajectory FAILED to send') return if result.result: rospy.loginfo('Motion controller successfully finished the trajectory!') else: rospy.logerr('Motion controller failed to complete the trajectory with error %s', result.errorId) except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.')
def main(): try: #rospy.init_node('avalos_limb_py') #moveit_commander.roscpp_initialize(sys.argv) moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial',anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group_name = 'right_arm' group = moveit_commander.MoveGroupCommander(group_name) display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) # We can get the name of the reference frame for this robot: planning_frame = group.get_planning_frame() #frecuency for Sawyer robot f=100 rate = rospy.Rate(f) # add gripper if(True): gripper = intera_interface.Gripper('right_gripper') gripper.calibrate() gripper.open() moveit_robot_state = RobotState() joint_state = JointState() joint_state.header = Header() joint_state.header.stamp = rospy.Time.now() joint_state.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6'] #Define topic pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) # Class to record #data=Data() #Class limb to acces information sawyer limb = Limb() limb.move_to_neutral() neutral=[-7.343481724930712e-07,-1.1799709303615113,2.7170121530417646e-05,2.1799982536216014,-0.00023687847544184848,0.5696772114967752,3.1411912264073045] base=[0.4045263671875, 0.3757021484375, -2.31678515625, 1.4790908203125, 2.14242578125, 2.1983291015625, 0.8213740234375] s6_down=[0.1123427734375, 0.398875, -2.4554755859375, 0.4891044921875, 2.4769873046875, 1.575130859375, 1.531140625] s6_up=[0.1613271484375, 0.3916650390625, -2.441814453125, 0.6957587890625, 2.515578125, 1.679708984375, 1.459033203125] obj_up=[-0.74389453125, 0.153580078125, -1.7190927734375, 0.7447021484375, 1.72510546875, 1.5934130859375, 0.317576171875] obj_down=[-0.7055927734375, 0.5030830078125, -1.7808125, 0.7994287109375, 2.0973154296875, 1.35018359375, 0.259451171875] centro=[0.0751162109375, 0.1868447265625, -1.93045703125, 1.425337890625, 1.7726181640625, 1.904037109375, 0.4765615234375] #points_1=path(neutral,centro,5) #points_2=path(centro,objeto,5) #points=path_full([neutral,objeto,centro,s6,centro,neutral],[5,5,5,5,5]) #points=path_full([neutral,obj_up,obj_down,obj_up,centro,s6,centro,neutral],[3,3,3,3,3,3,3]) alfa=0.5 p1=path_full([neutral,obj_up,obj_down],[2,3,2]) p2=path_full([obj_down,obj_up,centro,base,s6_up,s6_down],[3,3,3,3,3,2]) p3=path_full([s6_down,base,centro,obj_up],[3,3]) opt_1=Opt_avalos(p1,f,alfa) opt_2=Opt_avalos(p2,f,alfa) opt_3=Opt_avalos(p3,f,alfa) v_time1=opt_1.full_time() v_time2=opt_2.full_time() v_time3=opt_3.full_time() j,v,a,jk=generate_path_cub(p1,v_time1,f) ext=len(j[0,:]) #Build message my_msg=JointCommand() # POSITION_MODE my_msg.mode=4 my_msg.names=["right_j0","right_j1","right_j2","right_j3","right_j4","right_j5","right_j6"] for n in xrange(ext): my_msg.position=[j[0][n],j[1][n],j[2][n],j[3][n],j[4][n],j[5][n],j[6][n]] my_msg.velocity=[v[0][n],v[1][n],v[2][n],v[3][n],v[4][n],v[5][n],v[6][n]] my_msg.acceleration=[a[0][n],a[1][n],a[2][n],a[3][n],a[4][n],a[5][n],a[6][n]] pub.publish(my_msg) rate.sleep() gripper.close() time.sleep(1) j,v,a,jk=generate_path_cub(p2,v_time2,f) ext=len(j[0,:]) #Build message my_msg=JointCommand() # POSITION_MODE my_msg.mode=4 my_msg.names=["right_j0","right_j1","right_j2","right_j3","right_j4","right_j5","right_j6"] for n in xrange(ext): my_msg.position=[j[0][n],j[1][n],j[2][n],j[3][n],j[4][n],j[5][n],j[6][n]] my_msg.velocity=[v[0][n],v[1][n],v[2][n],v[3][n],v[4][n],v[5][n],v[6][n]] my_msg.acceleration=[a[0][n],a[1][n],a[2][n],a[3][n],a[4][n],a[5][n],a[6][n]] pub.publish(my_msg) rate.sleep() gripper.open() j,v,a,jk=generate_path_cub(p3,v_time3,f) ext=len(j[0,:]) #Build message my_msg=JointCommand() # POSITION_MODE my_msg.mode=4 my_msg.names=["right_j0","right_j1","right_j2","right_j3","right_j4","right_j5","right_j6"] for n in xrange(ext): my_msg.position=[j[0][n],j[1][n],j[2][n],j[3][n],j[4][n],j[5][n],j[6][n]] my_msg.velocity=[v[0][n],v[1][n],v[2][n],v[3][n],v[4][n],v[5][n],v[6][n]] my_msg.acceleration=[a[0][n],a[1][n],a[2][n],a[3][n],a[4][n],a[5][n],a[6][n]] pub.publish(my_msg) rate.sleep() ''' group.clear_pose_targets() group.set_start_state_to_current_state()s # We can get the joint values from the group and adjust some of the values: pose_goal = geometry_msgs.msg.Pose() # Orientation pose_goal.orientation.x = -1 pose_goal.orientation.y = 0.0 pose_goal.orientation.z = 0.0 pose_goal.orientation.w = 0.0 q0=np.array([]) q1=np.array([]) q2=np.array([]) q3=np.array([]) q4=np.array([]) q5=np.array([]) q6=np.array([]) # Cartesian position pose_goal.position.x = -0.1 pose_goal.position.y = 0.35 pose_goal.position.z = 0.05 pose_goal=Pose(Point(-0.1,0.6,0.05),Quaternion(-1,0,0,0)) group.set_pose_target(pose_goal) a=group.plan() points=a.joint_trajectory.points n=len(points) joint_state.position = [points[n-1].positions[0], points[n-1].positions[1], points[n-1].positions[2], points[n-1].positions[3],points[n-1].positions[4], points[n-1].positions[5], points[n-1].positions[6]] moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) for i in range(n): q0=np.append(q0, points[i].positions[0]) q1=np.append(q1, points[i].positions[1]) q2=np.append(q2, points[i].positions[2]) q3=np.append(q3, points[i].positions[3]) q4=np.append(q4, points[i].positions[4]) q5=np.append(q5, points[i].positions[5]) q6=np.append(q6, points[i].positions[6]) print "q000",q0 # Cartesian position pose_goal.position.x = 0.43 pose_goal.position.y = -0.4 pose_goal.position.z = 0.2 group.set_pose_target(pose_goal) a=group.plan() points=a.joint_trajectory.points n=len(points) joint_state.position = [points[n-1].positions[0], points[n-1].positions[1], points[n-1].positions[2], points[n-1].positions[3],points[n-1].positions[4], points[n-1].positions[5], points[n-1].positions[6]] moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) for i in range(n-1): # Si se repite un numero en posicion entra en un bug q0=np.append(q0, points[i+1].positions[0]) q1=np.append(q1, points[i+1].positions[1]) q2=np.append(q2, points[i+1].positions[2]) q3=np.append(q3, points[i+1].positions[3]) q4=np.append(q4, points[i+1].positions[4]) q5=np.append(q5, points[i+1].positions[5]) q6=np.append(q6, points[i+1].positions[6]) q=np.array([q0,q1,q2,q3,q4,q5,q6]) print "q001",q0 print q[0] #return 0 alfa=0.5 start = time.time() opt=Opt_avalos(q,f,alfa) v_time=opt.full_time() j,v,a,jk=generate_path_cub(q,v_time,f) ext=len(j[0,:]) end = time.time() print('Process Time:', end-start) print ext #save_matrix(j,"data_p.txt",f) #save_matrix(v,"data_v.txt",f) #save_matrix(a,"data_a.txt",f) #save_matrix(jk,"data_y.txt",f) v_jk=sqrt(np.mean(np.square(jk))) print("Opt Time:",v_time) print("Min Time:",m_time) #print('Optimizacion:',opt.result()) print('Costo Tiempo:',len(j[0])/float(100.0)) print('Costo Jerk:', v_jk) # Position init #raw_input('Iniciar_CT_initial_position?') #limb.move_to_joint_positions({"right_j6": ik1[6],"right_j5": ik1[5], "right_j4": ik1[4], "right_j3": ik1[3], "right_j2": #ik1[2],"right_j1": ik1[1],"right_j0": ik1[0]}) raw_input('Iniciar_CT_execute?') #Build message my_msg=JointCommand() # POSITION_MODE my_msg.mode=4 my_msg.names=["right_j0","right_j1","right_j2","right_j3","right_j4","right_j5","right_j6"] #data.writeon(str(alfa)+"trayectoria.txt") print("Control por trayectoria iniciado.") #time.sleep(0.25) for n in xrange(ext): my_msg.position=[j[0][n],j[1][n],j[2][n],j[3][n],j[4][n],j[5][n],j[6][n]] my_msg.velocity=[v[0][n],v[1][n],v[2][n],v[3][n],v[4][n],v[5][n],v[6][n]] my_msg.acceleration=[a[0][n],a[1][n],a[2][n],a[3][n],a[4][n],a[5][n],a[6][n]] #pub.publish(my_msg) rate.sleep() print("Control por trayectoria terminado.") time.sleep(0.25) #data.writeoff() ''' print("Programa terminado.") except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user.')
def main(): try: rospy.init_node('avalos_limb_py') #frecuency for Sawyer robot f = 100 rate = rospy.Rate(f) # add gripper gripper = intera_interface.Gripper('right_gripper') gripper.calibrate() gripper.open() #Define topic pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) # Class to record data = Data() #Class limb to acces information sawyer limb = Limb() #Initial position limb.move_to_neutral() ik_pose_1 = np.array([0.45, 0.70, -0.2, 0.70, 0.0, 0.70, 0.0]) ik1 = np.array([ 0.69514791, 0.64707899, 1.92295654, 0.01856896, -0.96413306, -0.92232169, 4.16828131 ]) ik_pose_2 = np.array([0.80, 0.30, 0.3, 0.70, 0.0, 0.70, 0.0]) ik2 = np.array([ 5.97103602e-01, -9.58042104e-02, 1.70891311e+00, -9.48490850e-01, -1.16882802e-03, 3.46304028e-01, 3.15488999e+00 ]) ik_pose_3 = np.array([0.50, 0.00, 0.65, 0.70, 0.0, 0.70, 0.0]) ik3 = np.array([ 0.63314606, -0.74365565, 1.14243681, -1.8618414, -0.84360096, 1.44537886, 3.41899404 ]) ik_pose_4 = np.array([0.80, -0.30, 0.3, 0.70, 0.0, 0.70, 0.0]) ik4 = np.array([ -0.31256034, -0.32949631, 2.12830197, -1.18751871, -0.45027567, 1.32627167, 2.95775708 ]) ik_pose_5 = np.array([0.45, -0.70, -0.20, 0.70, 0.0, 0.70, 0.0]) ik5 = np.array([ -1.55624859, 0.70439442, 2.52311113, -0.08708148, -0.94109983, 1.61554785, 2.54289819 ]) #ik_service_client_full(ik_pose_1) #ik_service_client_full(ik_pose_2) #ik_service_client_full(ik_pose_3) #ik_service_client_full(ik_pose_4) #ik_service_client_full(ik_pose_5) #initial=limb.joint_angles() # Define KNOTS. Set inperpolation in linear form limb.move_to_joint_positions({ "right_j6": ik1[6], "right_j5": ik1[5], "right_j4": ik1[4], "right_j3": ik1[3], "right_j2": ik1[2], "right_j1": ik1[1], "right_j0": ik1[0] }) q=np.array([[ik1[0],ik2[0],ik3[0],ik4[0],ik5[0]], \ [ik1[1],ik2[1],ik3[1],ik4[1],ik5[1]], \ [ik1[2],ik2[2],ik3[2],ik4[2],ik5[2]], \ [ik1[3],ik2[3],ik3[3],ik4[3],ik5[3]], \ [ik1[4],ik2[4],ik3[4],ik4[4],ik5[4]], \ [ik1[5],ik2[5],ik3[5],ik4[5],ik5[5]], \ [ik1[6],ik2[6],ik3[6],ik4[6],ik5[6]] \ ]) t_min, t_min_tiempo = min_time(q) print t_min_tiempo tasa = 1 / 0.2 knots_sec = np.round(t_min * tasa, 0) t_k2 = np.arange(knots_sec[-1]) k_j0 = sp.interpolate.interp1d( knots_sec, [ik1[0], ik2[0], ik3[0], ik4[0], ik5[0]], kind='linear')(t_k2) k_j1 = sp.interpolate.interp1d( knots_sec, [ik1[1], ik2[1], ik3[1], ik4[1], ik5[1]], kind='linear')(t_k2) k_j2 = sp.interpolate.interp1d( knots_sec, [ik1[2], ik2[2], ik3[2], ik4[2], ik5[2]], kind='linear')(t_k2) k_j3 = sp.interpolate.interp1d( knots_sec, [ik1[3], ik2[3], ik3[3], ik4[3], ik5[3]], kind='linear')(t_k2) k_j4 = sp.interpolate.interp1d( knots_sec, [ik1[4], ik2[4], ik3[4], ik4[4], ik5[4]], kind='linear')(t_k2) k_j5 = sp.interpolate.interp1d( knots_sec, [ik1[5], ik2[5], ik3[5], ik4[5], ik5[5]], kind='linear')(t_k2) k_j6 = sp.interpolate.interp1d( knots_sec, [ik1[6], ik2[6], ik3[6], ik4[6], ik5[6]], kind='linear')(t_k2) q = np.array([k_j0, k_j1, k_j2, k_j3, k_j4, k_j5, k_j6]) alfa = 0.15 start = time.time() opt = Opt_2_avalos(q, f, alfa) v_time = opt.full_time() j, v, a, jk = generate_path_cub(q, v_time, f) ext = len(j[0, :]) end = time.time() print('Process Time:', end - start) print ext save_matrix(j, "data_p.txt", f) save_matrix(v, "data_v.txt", f) save_matrix(a, "data_a.txt", f) save_matrix(jk, "data_y.txt", f) print("Vector Time", v_time) #print('Optimizacion:',opt.result()) print('Costo Tiempo:', opt.value_time()) print('Costo Jerk:', opt.value_jerk()) # Position init limb.move_to_joint_positions({ "right_j6": ik1[6], "right_j5": ik1[5], "right_j4": ik1[4], "right_j3": ik1[3], "right_j2": ik1[2], "right_j1": ik1[1], "right_j0": ik1[0] }) #raw_input('Cerrar?') #time.sleep(4) #gripper.close() ''' raw_input('Iniciar_CD?') data.writeon("directa.txt") #time.sleep(0.25) limb.move_to_joint_positions({"right_j6": ik2[6],"right_j5": ik2[5], "right_j4": ik2[4], "right_j3": ik2[3], "right_j2": ik2[2],"right_j1": ik2[1],"right_j0": ik2[0]}) #raw_input('Continuar?') limb.move_to_joint_positions({"right_j6": ik3[6],"right_j5": ik3[5], "right_j4": ik3[4], "right_j3": ik3[3], "right_j2": ik3[2],"right_j1": ik3[1],"right_j0": ik3[0]}) #raw_input('Continuar?') limb.move_to_joint_positions({"right_j6": ik4[6],"right_j5": ik4[5], "right_j4": ik4[4], "right_j3": ik4[3], "right_j2": ik4[2],"right_j1": ik4[1],"right_j0": ik4[0]}) #raw_input('Continuar?') limb.move_to_joint_positions({"right_j6": ik5[6],"right_j5": ik5[5], "right_j4": ik5[4], "right_j3": ik5[3], "right_j2": ik5[2],"right_j1": ik5[1],"right_j0": ik5[0]}) time.sleep(0.25) data.writeoff() print("Control por cinematica directa terminado.") ''' raw_input('Iniciar_CT_initial_position?') limb.move_to_joint_positions({ "right_j6": ik1[6], "right_j5": ik1[5], "right_j4": ik1[4], "right_j3": ik1[3], "right_j2": ik1[2], "right_j1": ik1[1], "right_j0": ik1[0] }) raw_input('Iniciar_CT_execute?') #Build message my_msg = JointCommand() # POSITION_MODE my_msg.mode = 4 my_msg.names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] data.writeon(str(alfa) + "trayectoria.txt") print("Control por trayectoria iniciado.") #time.sleep(0.25) for n in xrange(ext): my_msg.position = [ j[0][n], j[1][n], j[2][n], j[3][n], j[4][n], j[5][n], j[6][n] ] my_msg.velocity = [ v[0][n], v[1][n], v[2][n], v[3][n], v[4][n], v[5][n], v[6][n] ] my_msg.acceleration = [ a[0][n], a[1][n], a[2][n], a[3][n], a[4][n], a[5][n], a[6][n] ] pub.publish(my_msg) rate.sleep() print("Control por trayectoria terminado.") time.sleep(0.25) data.writeoff() print("Programa terminado.") except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user.')
def main(): try: rospy.init_node('avalos_limb_py') #Frecuency for Sawyer robot f = 100 rate = rospy.Rate(f) #Define topic pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) #Class limb to acces information sawyer limb = Limb() #Initial position limb.move_to_neutral() print "posicion inicial terminada" # Position init initial = limb.joint_angles() pi = [ initial["right_j0"], initial["right_j1"], initial["right_j2"], initial["right_j3"], initial["right_j4"], initial["right_j5"], initial["right_j6"] ] # Posiition end pe = [0, 0, 0, 0, 0, 0, 0] # Knost vector time. We assum the process will take 10 sec k_t = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10] # Set knots points for each joint k_j0 = np.linspace(pi[0], pe[0], num=11) k_j1 = np.linspace(pi[1], pe[1], num=11) k_j2 = np.linspace(pi[2], pe[2], num=11) k_j3 = np.linspace(pi[3], pe[3], num=11) k_j4 = np.linspace(pi[4], pe[4], num=11) k_j5 = np.linspace(pi[5], pe[5], num=11) k_j6 = np.linspace(pi[6], pe[6], num=11) # Length time that will depend of frecuecy l = k_t[-1] * f new_t = np.linspace(k_t[0], k_t[-1], l) # Obtain all point following the interpolated points j0 = sp.interpolate.interp1d(k_t, k_j0, kind='cubic')(new_t) j1 = sp.interpolate.interp1d(k_t, k_j1, kind='cubic')(new_t) j2 = sp.interpolate.interp1d(k_t, k_j2, kind='cubic')(new_t) j3 = sp.interpolate.interp1d(k_t, k_j3, kind='cubic')(new_t) j4 = sp.interpolate.interp1d(k_t, k_j4, kind='cubic')(new_t) j5 = sp.interpolate.interp1d(k_t, k_j5, kind='cubic')(new_t) j6 = sp.interpolate.interp1d(k_t, k_j6, kind='cubic')(new_t) # Vector for velocity v_j0 = np.zeros(l) v_j1 = np.zeros(l) v_j2 = np.zeros(l) v_j3 = np.zeros(l) v_j4 = np.zeros(l) v_j5 = np.zeros(l) v_j6 = np.zeros(l) # Vector for acceleration a_j0 = np.zeros(l) a_j1 = np.zeros(l) a_j2 = np.zeros(l) a_j3 = np.zeros(l) a_j4 = np.zeros(l) a_j5 = np.zeros(l) a_j6 = np.zeros(l) # Vector for acceleration jk_j0 = np.zeros(l) jk_j1 = np.zeros(l) jk_j2 = np.zeros(l) jk_j3 = np.zeros(l) jk_j4 = np.zeros(l) jk_j5 = np.zeros(l) jk_j6 = np.zeros(l) for i in xrange(l - 1): v_j0[i] = (j0[i + 1] - j0[i]) * f v_j1[i] = (j1[i + 1] - j1[i]) * f v_j2[i] = (j2[i + 1] - j2[i]) * f v_j3[i] = (j3[i + 1] - j3[i]) * f v_j4[i] = (j4[i + 1] - j4[i]) * f v_j5[i] = (j5[i + 1] - j5[i]) * f v_j6[i] = (j6[i + 1] - j6[i]) * f v_j0[-1] = v_j0[-2] v_j1[-1] = v_j1[-2] v_j2[-1] = v_j2[-2] v_j3[-1] = v_j3[-2] v_j4[-1] = v_j4[-2] v_j5[-1] = v_j5[-2] v_j6[-1] = v_j6[-2] for i in xrange(l - 1): a_j0[i] = (v_j0[i + 1] - v_j0[i]) * f a_j1[i] = (v_j1[i + 1] - v_j1[i]) * f a_j2[i] = (v_j2[i + 1] - v_j2[i]) * f a_j3[i] = (v_j3[i + 1] - v_j3[i]) * f a_j4[i] = (v_j4[i + 1] - v_j4[i]) * f a_j5[i] = (v_j5[i + 1] - v_j5[i]) * f a_j6[i] = (v_j6[i + 1] - v_j6[i]) * f a_j0[-1] = a_j0[-2] a_j1[-1] = a_j1[-2] a_j2[-1] = a_j2[-2] a_j3[-1] = a_j3[-2] a_j4[-1] = a_j4[-2] a_j5[-1] = a_j5[-2] a_j6[-1] = a_j6[-2] for i in xrange(l - 1): jk_j0[i] = (a_j0[i + 1] - a_j0[i]) * f jk_j1[i] = (a_j1[i + 1] - a_j1[i]) * f jk_j2[i] = (a_j2[i + 1] - a_j2[i]) * f jk_j3[i] = (a_j3[i + 1] - a_j3[i]) * f jk_j4[i] = (a_j4[i + 1] - a_j4[i]) * f jk_j5[i] = (a_j5[i + 1] - a_j5[i]) * f jk_j6[i] = (a_j6[i + 1] - a_j6[i]) * f jk_j0[-1] = jk_j0[-2] jk_j1[-1] = jk_j1[-2] jk_j2[-1] = jk_j2[-2] jk_j3[-1] = jk_j3[-2] jk_j4[-1] = jk_j4[-2] jk_j5[-1] = jk_j5[-2] jk_j6[-1] = jk_j6[-2] j = np.array([j0, j1, j2, j3, j4, j5, j6]) v = np.array([v_j0, v_j1, v_j2, v_j3, v_j4, v_j5, v_j6]) a = np.array([a_j0, a_j1, a_j2, a_j3, a_j4, a_j5, a_j6]) jk = np.array([jk_j0, jk_j1, jk_j2, jk_j3, jk_j4, jk_j5, jk_j6]) save_matrix(j, "data_p.txt", f) save_matrix(v, "data_v.txt", f) save_matrix(a, "data_a.txt", f) save_matrix(jk, "data_y.txt", f) #Build message my_msg = JointCommand() # POSITION_MODE my_msg.mode = 4 my_msg.names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] for i in xrange(l): my_msg.position = [j0[i], j1[i], j2[i], j3[i], j4[i], j5[i], j6[i]] my_msg.velocity = [ v_j0[i], v_j1[i], v_j2[i], v_j3[i], v_j4[i], v_j5[i], v_j6[i] ] my_msg.acceleration = [ a_j0[i], a_j1[i], a_j2[i], a_j3[i], a_j4[i], a_j5[i], a_j6[i] ] pub.publish(my_msg) rate.sleep() print "Move ok" except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user.')
def main(): try: #rospy.init_node('avalos_limb_py') #moveit_commander.roscpp_initialize(sys.argv) moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group_name = 'right_arm' group = moveit_commander.MoveGroupCommander(group_name) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) # We can get the name of the reference frame for this robot: planning_frame = group.get_planning_frame() #frecuency for Sawyer robot f = 100 rate = rospy.Rate(f) # add gripper if (False): gripper = intera_interface.Gripper('right_gripper') gripper.calibrate() gripper.open() moveit_robot_state = RobotState() joint_state = JointState() joint_state.header = Header() joint_state.header.stamp = rospy.Time.now() joint_state.name = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] #Define topic pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) # Class to record #data=Data() #Class limb to acces information sawyer limb = Limb() limb.move_to_neutral() group.clear_pose_targets() group.set_start_state_to_current_state() # We can get the joint values from the group and adjust some of the values: pose_goal = geometry_msgs.msg.Pose() # Orientation pose_goal.orientation.x = -1 pose_goal.orientation.y = 0.0 pose_goal.orientation.z = 0.0 pose_goal.orientation.w = 0.0 q0 = np.array([]) q1 = np.array([]) q2 = np.array([]) q3 = np.array([]) q4 = np.array([]) q5 = np.array([]) q6 = np.array([]) # Cartesian position pose_goal.position.x = -0.1 pose_goal.position.y = 0.35 pose_goal.position.z = 0.05 pose_goal = Pose(Point(-0.1, 0.6, 0.05), Quaternion(-1, 0, 0, 0)) group.set_pose_target(pose_goal) a = group.plan() points = a.joint_trajectory.points n = len(points) joint_state.position = [ points[n - 1].positions[0], points[n - 1].positions[1], points[n - 1].positions[2], points[n - 1].positions[3], points[n - 1].positions[4], points[n - 1].positions[5], points[n - 1].positions[6] ] moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) for i in range(n): q0 = np.append(q0, points[i].positions[0]) q1 = np.append(q1, points[i].positions[1]) q2 = np.append(q2, points[i].positions[2]) q3 = np.append(q3, points[i].positions[3]) q4 = np.append(q4, points[i].positions[4]) q5 = np.append(q5, points[i].positions[5]) q6 = np.append(q6, points[i].positions[6]) print "q000", q0 # Cartesian position pose_goal.position.x = 0.43 pose_goal.position.y = -0.4 pose_goal.position.z = 0.2 group.set_pose_target(pose_goal) a = group.plan() points = a.joint_trajectory.points n = len(points) joint_state.position = [ points[n - 1].positions[0], points[n - 1].positions[1], points[n - 1].positions[2], points[n - 1].positions[3], points[n - 1].positions[4], points[n - 1].positions[5], points[n - 1].positions[6] ] moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) for i in range( n - 1): # Si se repite un numero en posicion entra en un bug q0 = np.append(q0, points[i + 1].positions[0]) q1 = np.append(q1, points[i + 1].positions[1]) q2 = np.append(q2, points[i + 1].positions[2]) q3 = np.append(q3, points[i + 1].positions[3]) q4 = np.append(q4, points[i + 1].positions[4]) q5 = np.append(q5, points[i + 1].positions[5]) q6 = np.append(q6, points[i + 1].positions[6]) #''' q = np.array([q0, q1, q2, q3, q4, q5, q6]) print "q001", q0 print q[0] #return 0 alfa = 0.5 start = time.time() opt = Opt_avalos(q, f, alfa) v_time = opt.full_time() j, v, a, jk = generate_path_cub(q, v_time, f) ext = len(j[0, :]) end = time.time() print('Process Time:', end - start) print ext #save_matrix(j,"data_p.txt",f) #save_matrix(v,"data_v.txt",f) #save_matrix(a,"data_a.txt",f) #save_matrix(jk,"data_y.txt",f) v_jk = sqrt(np.mean(np.square(jk))) print("Opt Time:", v_time) print("Min Time:", m_time) #print('Optimizacion:',opt.result()) print('Costo Tiempo:', len(j[0]) / float(100.0)) print('Costo Jerk:', v_jk) # Position init #raw_input('Iniciar_CT_initial_position?') #limb.move_to_joint_positions({"right_j6": ik1[6],"right_j5": ik1[5], "right_j4": ik1[4], "right_j3": ik1[3], "right_j2": #ik1[2],"right_j1": ik1[1],"right_j0": ik1[0]}) raw_input('Iniciar_CT_execute?') #Build message my_msg = JointCommand() # POSITION_MODE my_msg.mode = 4 my_msg.names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] #data.writeon(str(alfa)+"trayectoria.txt") print("Control por trayectoria iniciado.") #time.sleep(0.25) for n in xrange(ext): my_msg.position = [ j[0][n], j[1][n], j[2][n], j[3][n], j[4][n], j[5][n], j[6][n] ] my_msg.velocity = [ v[0][n], v[1][n], v[2][n], v[3][n], v[4][n], v[5][n], v[6][n] ] my_msg.acceleration = [ a[0][n], a[1][n], a[2][n], a[3][n], a[4][n], a[5][n], a[6][n] ] pub.publish(my_msg) rate.sleep() print("Control por trayectoria terminado.") time.sleep(0.25) #data.writeoff() print("Programa terminado.") except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user.')