def basicAccelerationMove(self, pos, speed): pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio', Float64, latch=True, queue_size=10) sub = rospy.Subscriber('/robot/joint_states', JointState, self.joints_callback) command = JointCommand() command.names = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] command.position = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] command.acceleration = [0.3, 0.05, 0.05, 0.05, 0.05, 0.05, 0.1] command.mode = 1 #pub_speed_ratio.publish(0.2) rate = rospy.Rate(10) control_diff_record = 10.0 control_diff_temp = 0.0 threshold = 0.02 # terminate the control once the arm moved to the desired joint space within the threshold while control_diff_record > threshold: pub.publish(command) for x, y in zip(self.jointAngles, command.acceleration): control_diff_temp = abs(x - y) + control_diff_temp control_diff_record = control_diff_temp control_diff_temp = 0.0 rate.sleep() rospy.loginfo( ">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def move_with_impedance(self, waypoints, duration=1.5): """ Moves from curent position to final position while hitting waypoints :param waypoints: List of arrays containing waypoint joint angles :param duration: trajectory duration """ self._try_enable() jointnames = self.limb.joint_names() prev_joint = np.array([self.limb.joint_angle(j) for j in jointnames]) waypoints = np.array([prev_joint] + waypoints) spline = CSpline(waypoints, duration) start_time = rospy.get_time() # in seconds finish_time = start_time + duration # in seconds time = rospy.get_time() while time < finish_time: pos, velocity, acceleration = spline.get(time - start_time) command = JointCommand() command.mode = JointCommand.POSITION_MODE command.names = jointnames command.position = pos command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag) command.acceleration = np.clip(acceleration, -max_accel_mag, max_accel_mag) self._cmd_publisher.publish(command) self.control_rate.sleep() time = rospy.get_time() for i in xrange(10): command = JointCommand() command.mode = JointCommand.POSITION_MODE command.names = jointnames command.position = waypoints[-1] self._cmd_publisher.publish(command) self.control_rate.sleep()
def move_to_ja(self, waypoints, duration=1.5): """ :param waypoints: List of joint angle arrays. If len(waypoints) == 1: then go directly to point. Otherwise: take trajectory that ends at waypoints[-1] and passes through each intermediate waypoint :param duration: Total time trajectory will take before ending """ self._try_enable() jointnames = self._limb.joint_names() prev_joint = np.array([self._limb.joint_angle(j) for j in jointnames]) waypoints = np.array([prev_joint] + waypoints) spline = CSpline(waypoints, duration) start_time = rospy.get_time() # in seconds finish_time = start_time + duration # in seconds time = rospy.get_time() while time < finish_time: pos, velocity, acceleration = spline.get(time - start_time) command = JointCommand() command.mode = JointCommand.POSITION_MODE command.names = jointnames command.position = pos command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag) command.acceleration = np.clip(acceleration, -max_accel_mag, max_accel_mag) self._cmd_publisher.publish(command) self._control_rate.sleep() time = rospy.get_time() for i in xrange(10): command = JointCommand() command.mode = JointCommand.POSITION_MODE command.names = jointnames command.position = waypoints[-1] self._cmd_publisher.publish(command) self._control_rate.sleep()
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 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 set_move(): F = 100 #Frecuencia de envio rate = rospy.Rate(F) # hz limb = intera_interface.Limb('right') limb.move_to_neutral() ik_service_client_full([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0]) print "Posicion neutral terminada" time.sleep(1) p1 = np.array([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0]) p2 = np.array([0.662, 0.200, 0.290, 0.717, 0, 0.717, 0]) p3 = np.array([0.662, -0.150, 0.290, 0.717, 0, 0.717, 0]) p4 = np.array([0.662, -0.450, -0.040, 0.717, 0, 0.717, 0]) p5 = np.array([0.662, -0.150, 0.290, 0.717, 0, 0.717, 0]) p6 = np.array([0.662, 0.200, 0.290, 0.717, 0, 0.717, 0]) p7 = np.array([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0]) data = Data() data.writeon("IK_data.txt") # Control de posicion por cinematica inversa time.sleep(0.75) [succes, q1] = ik_service_client_full(p1) [succes, q2] = ik_service_client_full(p2) [succes, q3] = ik_service_client_full(p3) [succes, q4] = ik_service_client_full(p4) [succes, q5] = ik_service_client_full(p5) [succes, q6] = ik_service_client_full(p6) [succes, q7] = ik_service_client_full(p7) time.sleep(0.75) data.writeoff() print "Control de Posicion Cinematica Inversa terminada" names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) # Control de trayectoria cinematica inversa q = np.matrix([q1, q2, q3, q4, q5, q6, q7]) print q [t, t_rec] = min_time(q, F) k = 1.25 #Factor de Tiempo t_points = [k * x for x in t] print "t_points", t_points #print "Tiempo de recorrido",t_rec,"s" [j, ext] = generate_path_cub(q, t_points, F) [v, ext] = generate_vel(j, F) [a, ext] = generate_acel(v, F) [jk, v_jk, ext] = generate_jerk(a, F) v_t = 6 * (t_points[-1] - t_points[0]) raw_input('Iniciar?') print "Valor tiempo: ", v_t, "Valor jerk", v_jk save_matrix(j, "save_data_p.txt", F) save_matrix(v, "save_data_v.txt", F) save_matrix(a, "save_data_a.txt", F) save_matrix(jk, "save_data_y.txt", F) ik_service_client_full([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0]) time.sleep(1) my_msg = JointCommand() my_msg.mode = 4 my_msg.names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] data.writeon("S_data.txt") time.sleep(0.75) if (my_msg.mode == 4): for n in range(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() time.sleep(0.75) return True
def move_move_no(limb, group, target, speed_ratio=None, accel_ratio=None, timeout=None): trajectory_publisher = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size = 1) JointCommandMessage = JointCommand() JointCommandMessage.mode = 4 if speed_ratio is None: speed_ratio = 0.3 if girigiri_aiiiiii: speed_ratio = 0.8 if accel_ratio is None: accel_ratio = 0.1 if girigiri_aiiiiii: accel_ratio = 0.2 plan = group.plan(target) rospy.sleep(1) JointCommandMessage.names = plan.joint_trajectory.joint_names start_time = rospy.get_time() position_array = [] velocity_array = [] acceleration_array = [] time_array = [] for point in plan.joint_trajectory.points: position = point.positions velocity = point.velocities acceleration = point.accelerations position_array.append(position) velocity_array.append(velocity) acceleration_array.append(acceleration) desire_time = point.time_from_start.to_sec() time_array.append(desire_time) s_array = time_array wp_array = position_array path = ta.SplineInterpolator(s_array, wp_array) s_sampled = np.linspace(0, time_array[len(time_array) - 1], 100) q_sampled = path.eval(s_sampled) # --------------------------- plotting the interpolator -------------------------- # plt.plot(s_sampled, q_sampled) # plt.hold(True) # plt.plot(time_array, position_array, 'kd') # plt.legend(["joint_1","joint_2","joint_3","joint_4","joint_5","joint_6","joint_7"]) # plt.hold(False) # plt.show() # exit() # ---------------------------- end of plotting ---------------------------------- # Create velocity bounds, then velocity constraint object dof = 7 vlim_ = np.ones(dof) * 5 #* speed_ratio vlim = np.vstack((-vlim_, vlim_)).T # Create acceleration bounds, then acceleration constraint object alim_ = np.ones(dof) * 2 * accel_ratio alim = np.vstack((-alim_, alim_)).T pc_vel = constraint.JointVelocityConstraint(vlim) pc_acc = constraint.JointAccelerationConstraint( alim, discretization_scheme=constraint.DiscretizationType.Interpolation) # Setup a parametrization instance instance = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel') # Retime the trajectory, only this step is necessary. t0 = time.time() jnt_traj, aux_traj = instance.compute_trajectory(0, 0) print("TOPPRA Parameterization time: {:} secs".format(time.time() - t0)) ts_sample = np.linspace(0, jnt_traj.get_duration(), 800) position_output = jnt_traj.eval(ts_sample) velocity_output = jnt_traj.evald(ts_sample) acceleration_output = jnt_traj.evaldd(ts_sample) # --------------------------- plotting the algorithm output --------------------------- # plt.subplot(3,1,1) # plt.plot(ts_sample, position_output) # plt.subplot(3,1,2) # plt.plot(ts_sample, velocity_output) # plt.subplot(3,1,3) # plt.plot(ts_sample, acceleration_output) # plt.show() # --------------------------- end plot the algorithm output --------------------------- start_time = rospy.get_time() for i in range(len(ts_sample) - 1): JointCommandMessage.position = position_output[i] JointCommandMessage.velocity = velocity_output[i] JointCommandMessage.acceleration = acceleration_output[i] desire_time = ts_sample[i + 1] while (rospy.get_time() - start_time) < desire_time: trajectory_publisher.publish(JointCommandMessage) JointCommandMessage.position = position_output[-1] JointCommandMessage.velocity = velocity_output[-1] JointCommandMessage.acceleration = acceleration_output[-1] trajectory_publisher.publish(JointCommandMessage)
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 __init__(self, limb="right"): rospy.init_node("custom_impedance_controller") rospy.on_shutdown(self.clean_shutdown) self._rs = intera_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled self._rs.enable() if not self._rs.state().enabled: raise RuntimeError("Robot did not enable...") self._limb = intera_interface.Limb(limb) print("Robot enabled...") # control parameters self._rate = rospy.Rate(800) # Hz self._j_names = [n for n in self._limb.joint_names()] self._duration = 1.5 self._desired_joint_pos = NEUTRAL_JOINT_ANGLES.copy() self._fit_interp() #synchronizes commands self._global_lock = Lock() # create cuff disable publisher cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction' self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1) self._cmd_publisher = rospy.Publisher( '/robot/limb/{}/joint_command'.format(limb), JointCommand, queue_size=100) print("Running. Ctrl-c to quit") rospy.Subscriber("desired_joint_pos", JointState, self._set_des_pos) rospy.Subscriber("interp_duration", Float32, self._set_duration) rospy.Subscriber("imp_ctrl_active", Int64, self._imp_ctrl_active) self._ctrl_active = True self._node_running = True self._rate.sleep() self._start_time = None while self._node_running: if self._ctrl_active: self._pub_cuff_disable.publish() self._global_lock.acquire() if self._start_time is None: self._start_time = rospy.get_time() elif self._start_time + self._duration < rospy.get_time(): self._fit_interp() self._start_time = rospy.get_time() t = min(rospy.get_time() - self._start_time, self._duration) #T \in [0, self._duration] pos, velocity, accel = [ x.squeeze() for x in self._interp.get(t) ] command = JointCommand() command.mode = JointCommand.TRAJECTORY_MODE command.names = self._j_names command.position = pos command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag) command.acceleration = np.clip(accel, -max_accel_mag, max_accel_mag) self._cmd_publisher.publish(command) self._global_lock.release() self._rate.sleep()
def _ctl_loop(self): rate = rospy.Rate(self._ctl_freq) # # trajectory_options = TrajectoryOptions() # trajectory_options.interaction_control = False # trajectory_options.interpolation_type = TrajectoryOptions.JOINT last_vel = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) last_acc = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) rec_data = [] rec_seq = 0 def compute_vel(measured, desired): tol = 0.008 mp = measured[0, :] dp = desired[0, :] mv = measured[1, :] dv = desired[1, :] cycle = self._des_provider[ self._active_des_provider]._cycles_since_active if cycle == 1: max_steps = (3.5 * np.abs(dp - mp) * self._ctl_freq + 50).astype(np.int) max_steps[np.abs(dp - mp) < tol] = 1 self._des_provider[ self._active_des_provider]._HACK_MAX_STEPS = max_steps else: max_steps = self._des_provider[ self._active_des_provider]._HACK_MAX_STEPS # max_steps = 500 steps = np.minimum(cycle, max_steps).astype(np.float64) weights = (steps / max_steps) dgain = 0.0 pgain = 4.0 ep = (mp * (1.0 - weights) + weights * dp) - mp ep[np.abs(dp - mp) < tol] = 0.0 ev = dv - (mv * (1.0 - weights) + weights * dv) sv = pgain * ep + dgain * ev sv = np.maximum(np.minimum(sv, 4.0), -4.0) return sv while not rospy.is_shutdown(): des_joint_state, des_gripper_state = self.get_active_desired_states( state_ids=('joint', 'gripper')) self.run_cycle() timestamp = rospy.Time.now() if des_joint_state is not None: MODE = self._des_provider[self._active_des_provider]._HACK_MODE joint_msg = JointCommand() joint_msg.header.stamp = timestamp joint_msg.names = self._joint_names acc = (self._joint_state[1, :] - last_vel) / 0.01 jerk = (acc - last_acc) / 0.01 vel = compute_vel(self._joint_state, des_joint_state) if MODE == JointCommand.VELOCITY_MODE: # if np.any(np.abs(vel[-1]) > 10.0): # print("pos msr:{} des:{}\tvel msr:{} des:{}\tset: {} acc: {} jerk: {}".format(self._joint_state[0,-1],des_joint_state[0,-1],self._joint_state[1,-1],des_joint_state[1,-1],vel[-1],acc[-1], jerk[-1])) if self.HACK_do_print: # print("pos msr:{} des:{}\tvel msr:{} des:{}\tset: {} acc: {} jerk: {}".format(self._joint_state[0,-1],des_joint_state[0,-1],self._joint_state[1,-1],des_joint_state[1,-1],vel[-1],acc[-1], jerk[-1])) with np.printoptions(precision=6, suppress=True, floatmode='fixed', sign=' '): # print("pm:{},\npd:{},\nvm:{},\nvd:{},\nvs:{},".format(np.array2string(self._joint_state[0,:]),np.array2string(des_joint_state[0,:]),np.array2string(self._joint_state[1,:]),np.array2string(des_joint_state[1,:]),np.array2string(vel))) # print(np.array2string(np.array([self._joint_state[0,:], des_joint_state[0,:], self._joint_state[1,:], des_joint_state[1,:], vel]))) rec_data.append( np.array([ self._joint_state[0, :], des_joint_state[0, :], self._joint_state[1, :], des_joint_state[1, :], vel ])) joint_msg.mode = JointCommand.VELOCITY_MODE joint_msg.position = [] joint_msg.velocity = vel joint_msg.acceleration = [] joint_msg.effort = [] elif MODE == JointCommand.POSITION_MODE: print("NOOOOO:(") joint_msg.mode = JointCommand.POSITION_MODE joint_msg.position = des_joint_state[0, :] joint_msg.velocity = [] joint_msg.acceleration = [] joint_msg.effort = [] self._joint_command_pub.publish(joint_msg) if self._joint_state is not None and np.all( np.abs(self._joint_state[0, :] - des_joint_state[0, :]) < 0.008): self.set_reached() if self.HACK_do_print: self.HACK_do_print = False # np.save('data_{}_{:03d}'.format(self._name,rec_seq),np.array(rec_data)) rec_seq += 1 rec_data = [] last_vel = self._joint_state[1, :] last_acc = acc if des_gripper_state is not None: gripper_msg = IOComponentCommand() gripper_msg.time = timestamp gripper_msg.op = "set" gripper_msg.args = '{"signals": {"position_m": {"data": [' + str( des_gripper_state) + '], "format": {"type": "float"}}}}' self._gripper_command_pub.publish(gripper_msg) rate.sleep()
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.')