def main(): rospy.init_node(node_name, anonymous=node_anonymous) p1 = make_pose(-0.1, -0.8, 1.4, 1, 1, 0, 0, frame_id=frame_id) p2 = make_pose(-0.5, -0.8, 1.4, 1, 1, 0, 0, frame_id=frame_id) speeds = [0.1, 0.2, 0.5, 1, 1.2, 1.5] robot = ClopemaRobotCommander('arms') robot.overwrite_time_parameterization = False state = RobotState.from_robot_state_msg(robot.get_current_state()) state.update_from_pose(p2, ik_link_id=ik_link) robot.set_joint_value_target(state.joint_state) traj = robot.plan() robot.execute(traj) for speed, pose in zip(speeds, itertools.cycle([p1, p2])): rospy.sleep(0.5) robot.set_start_state_to_current_state() state = RobotState.from_robot_state_msg(robot.get_current_state()) state.update_from_pose(pose, ik_link_id=ik_link) robot.set_joint_value_target(state.joint_state) traj = robot.plan() time_before = traj.joint_trajectory.points[-1].time_from_start.to_sec() traj = modify_speed(traj, speed) time_after = traj.joint_trajectory.points[-1].time_from_start.to_sec() traj_len = len(traj.joint_trajectory.points) print "Speed:", speed, "time before:", time_before, "after:", time_after, "length:", traj_len robot.execute(traj)
def save_cmd(filename): robot = ClopemaRobotCommander("arms") rs = robot.get_current_state() js = rs.joint_state with open(filename, "w") as f: f.write(str(js))
def main(): opt = docopt(__doc__) rospy.init_node(node_name) p1 = make_pose(-0.3,-0.8,1.4,0,1,0,0,frame_id=frame_id) p2 = make_pose(0.3,-0.8,1.4,0,1,0,0,frame_id=frame_id) poses = [] poses.append(make_pose(-0.4,-0.8,1.4,0,1,0,0)) poses.append(make_pose(-0.4,-0.9,1.4,0,1,0,0)) poses.append(make_pose(-0.4,-0.9,1.3,0,1,0,0)) poses.append(make_pose(-0.3,-0.8,1.4,0,1,0,0)) poses.append(make_pose(-0.3,-0.8,1.4,0.1,0.9,0,0)) poses.append(make_pose(-0.3,-0.8,1.4,0.29,0.95,-0.072,-0.022)) poses.append(make_pose(-0.3,-0.8,1.2,0.29,0.95,-0.072,-0.022)) poses.append(make_pose(-0.2,-0.6,1.2,0.29,0.95,-0.072,-0.022)) poses.append(make_pose(-0.3,-0.8,1.4,0,1,0,0)) poses.append(make_pose(-0.3,-0.8,1.4,0,0.98,0,0.2)) poses.append(make_pose(-0.3,-0.8,1.4,0,0.99,0,-0.1)) poses.append(make_pose(-0.3,-0.8,1.4,0,1,0,0)) # Go into start position robot = ClopemaRobotCommander('arms') state = RobotState.from_robot_state_msg(robot.get_current_state()) state.update_from_pose(p1, ik_link_1) state.update_from_pose(p2, ik_link_2) robot.set_joint_value_target(state.joint_state) traj = robot.plan() robot.execute(traj) state = RobotState.from_robot_state_msg(robot.get_current_state()) full_poses = parallel_move(state, poses) res = services.compute_cartesian_path_dual(state, (ik_link_1, ik_link_2), full_poses, jump_threshold=1.5) if res.error_code.val == res.error_code.SUCCESS and res.fraction == 1.0: traj = res.solution robot.execute(traj) else: rospy.logerr("Unable to plan Cartesian path, error_code: %d, fraction: %f", res.error_code.val, res.fraction)
def exec_cmd(filename): with open(filename, "r") as f: data = yaml.load_all(f) s = next(data) js = JointState() genpy.message.fill_message_args(js, s) robot = ClopemaRobotCommander("arms") rs = robot.get_current_state() if has_arms(rs, js): robot = ClopemaRobotCommander("arms") robot.set_joint_value_target(js) traj = robot.plan() if args['--speed'] is not None: print "Speed ", "%+3.2f" % float(args['--speed']) else: print "Speed ", "%+3.2f" % robot.get_robot_speed() if len(traj.joint_trajectory.points) <= 0: print "No valid trajectory found please check the robot output." else: r = ask("Execute trajectory? ", {'Y': 'Yes', 'n': 'No'}) if r == 'y': if args['--speed'] is not None: robot.set_robot_speed(float(args['--speed'])) robot.execute(traj) if has_ext(rs, js): robot = ClopemaRobotCommander("ext") robot.set_joint_value_target(js) traj = robot.plan() if args['--speed'] is not None: print "Speed ", "%+3.2f" % float(args['--speed']) else: print "Speed ", "%+3.2f" % robot.get_robot_speed() if len(traj.joint_trajectory.points) <= 0: print "No valid trajectory found please check the robot output." else: r = ask("Execute trajectory? ", {'Y': 'Yes', 'n': 'No'}) if r == 'y': if args['--speed'] is not None: robot.set_robot_speed(float(args['--speed'])) robot.execute(traj) if args['--sof']: robot.set_servo_power_off(True)
class TestAsyncExecute(unittest.TestCase): def setUp(self): self.commander = ClopemaRobotCommander("arms") self.commander.set_start_state_to_current_state() self.commander.set_named_target("home_arms") self.commander.go(wait=True) self.commander.set_joint_value_target({'r1_joint_t':1.0}) self.traj = self.commander.plan() def test_async(self): """Test whether the async execute returns """ self.commander.async_execute(self.traj) state1 = self.commander.get_current_state() rospy.sleep(0.1) state2 = self.commander.get_current_state() a = state1.joint_state.position[state1.joint_state.name.index('r1_joint_t')] b = state2.joint_state.position[state2.joint_state.name.index('r1_joint_t')] self.assertNotEqual(a,b) def test_stop(self): """ Test whether is possible to stop the execution of a trajectory in the middle of execution. """ self.commander.async_execute(self.traj) self.commander.stop() rospy.sleep(0.1) state1 = self.commander.get_current_state() rospy.sleep(0.1) state2 = self.commander.get_current_state() a = state1.joint_state.position[state1.joint_state.name.index('r1_joint_t')] b = state2.joint_state.position[state2.joint_state.name.index('r1_joint_t')] self.assertEqual(a,b) self.assertNotEqual(a, 1.0) self.assertNotEqual(a, 0.0)
def main(): rospy.init_node("test_planning_and_execution", anonymous=True) robot = ClopemaRobotCommander("arms") robot.set_start_state_to_current_state() # Get initial robot state state = RobotState.from_robot_state_msg(robot.get_current_state()) # Set position of both arms state.update_from_pose(pose_from_xyzrpy(0.3, -1.0, 1.5, np.pi / 2, 0, np.pi / 2), ik_link_id="r2_ee") # Set as target robot.set_joint_value_target(state.joint_state) # Plan trajectory traj = robot.plan() # Execute trajectory robot.execute(traj)
def main(): rospy.init_node(node_name, anonymous=node_anonymous) poses = [] poses.append(make_pose(-0.3, -0.8, 1.4, 1, 1, 0, 0, frame_id=frame_id)) poses.append(make_pose(-0.3, -0.8, 1.4, 0, 1, 0, 0, frame_id=frame_id)) poses.append(make_pose(-0.3, -0.8, 1.4, 1, 0, 0, 0, frame_id=frame_id)) poses.append(make_pose(-0.3, -0.8, 1.4, -1, 0, 0, 0, frame_id=frame_id)) poses.append(make_pose(-0.3, -0.8, 1.4, 1, 0, 1, 0, frame_id=frame_id)) poses.append(make_pose(-0.3, -0.8, 1.4, 1, 1, 1, 1, frame_id=frame_id)) poses.append(make_pose(-0.3, -0.8, 1.4, 1, 0, 0, 1, frame_id=frame_id)) poses.append(make_pose(-0.3, -0.8, 1.4, 1, 1, 0, 0, frame_id=frame_id)) robot = ClopemaRobotCommander('arms') for pose in poses: state = RobotState.from_robot_state_msg(robot.get_current_state()) state.update_from_pose(pose, ik_link_id=ik_link) robot.set_joint_value_target(state.joint_state) traj = robot.plan() robot.execute(traj)
def main(): # Process command line arguments argv = rospy.myargv(argv=sys.argv) opt = docopt(__doc__, argv = argv[1:]) opt_debug = opt['--debug'] opt_xtion = int(opt['--xtion']) opt_target_frame = opt['--frame'] opt_output = opt['<output_file>'] if opt_xtion == 1: opt_other_s_link = "r2_joint_s" opt_other_s_val = pi / 2 opt_ee_link = "xtion1_link_ee" opt_group = "r1_xtion" elif opt_xtion == 2: opt_other_s_link = "r1_joint_s" opt_other_s_val = -pi / 2 opt_ee_link = "xtion1_link_ee" opt_group = "r2_xtion" else: rospy.logerr("Xtion number is not valid: %d", opt_xtion) return if opt_debug: print opt # Initialise node rospy.init_node(NODE_NAME, anonymous=NODE_ANONYMOUS) # Initialise tf2 buffer tfb = get_tf2_buffer() # Initialise robot commander robot = ClopemaRobotCommander(opt_group) # Get start position, all zeros, but exte rotated towards calibration table, # and S of the other arm rotated -90 degrees. current_state = robot.get_current_state() start_state = deepcopy(current_state) start_state.joint_state.position = [0] * len(start_state.joint_state.name) start_state.joint_state.position[start_state.joint_state.name.index(opt_other_s_link)] = opt_other_s_val start_state.joint_state.position[start_state.joint_state.name.index("ext_axis")] = base_rotation("base_link", opt_target_frame, tfb) if opt_debug: print start_state # Generate poses header = Header() header.frame_id = opt_target_frame pose_list = [] for x,y,z in xyzrange(L1_X_RANGE, L1_Y_RANGE, L1_Z_RANGE, L1_X_STEP, L1_Y_STEP, L1_Z_STEP): p = make_pose(x, y, z, 0, 1, 0, 0) pose_list.append(PoseStamped(header, p)) pose_list.append(PoseStamped(header, orient_pose(p))) for x,y,z in xyzrange(L2_X_RANGE, L2_Y_RANGE, L2_Z_RANGE, L2_X_STEP, L2_Y_STEP, L2_Z_STEP): p = make_pose(x, y, z, 0, 1, 0, 0) pose_list.append(PoseStamped(header, p)) pose_list.append(PoseStamped(header, orient_pose(p))) # TODO: Sort poses # Plan throught poses trajs = plan_through_poses(robot, start_state, pose_list, opt_ee_link) # Print info print "# generated poses: %d" % len(pose_list) print "# trajectories: %d" % len(trajs) # Wite trajectories write_msgs(trajs, opt_output)
def compute_polish_move(hold_point, start_point, length=0.1, frame_id="base_link"): # Compute the third point p0 = point_to_np(hold_point) p1 = point_to_np(start_point) v = p1 - p0 v = v / np.linalg.norm(v) p2 = p1 + length * v rospy.loginfo("p0=%s, p1=%s, p2=%s", str(p0), str(p1), str(p2)) # Get the vector angle a = np.arctan2(v[1], v[0]) rospy.loginfo("Angle: [%f, %f]", a - angle_limit, a + angle_limit) # Generate poses poses0 = generate_poses(p0, 0, 2 * np.pi, no_candidates) poses1 = generate_poses(p1, a - angle_limit, a + angle_limit, no_candidates) poses2 = generate_poses(p2, a - angle_limit, a + angle_limit, no_candidates) candidates = [(p[0], p[1][0], p[1][1]) for p in itertools.product(poses0, zip(poses1, poses2))] arms = ClopemaRobotCommander("arms") state = RobotState.from_robot_state_msg(arms.get_current_state()) trajs = [] for p0, p1, p2 in candidates: trajs = [] state0 = deepcopy(state) # Point for the first arm r1_p03 = deepcopy(p0) r1_p03.position.z = r1_p03.position.z + table_ofset r1_p12 = deepcopy(p0) # Points for the second arm r2_p0 = deepcopy(p1) r2_p0.position.z = r2_p0.position.z + table_ofset r2_p1 = deepcopy(p1) r2_p2 = deepcopy(p2) r2_p3 = deepcopy(p2) r2_p3.position.z = r2_p3.position.z + table_ofset try: state0.update_from_pose(pose_stamped(r1_p03, frame_id=frame_id), ik_link_id=r1_ik_link) except Exception as e: rospy.logwarn("Arm: %s, pose: %s: %s", str(r1_ik_link), show_pose(r1_p03), str(e)) continue try: state0.update_from_pose(pose_stamped(r2_p0, frame_id=frame_id), ik_link_id=r2_ik_link) except Exception as e: rospy.logwarn("Arm: %s, pose: %s: %s", str(r2_ik_link), show_pose(r2_p0), str(e)) continue arms.set_start_state_to_current_state() arms.set_joint_value_target(state0.joint_state) traj = arms.plan() trajs.append(traj) if not traj: rospy.logwarn("Unable to plan to initial position.") continue arms.set_start_state(state0) arms.set_pose_reference_frame(frame_id) wp_1 = [r1_p12, r1_p12, r1_p03] wp_2 = [r2_p1, r2_p2, r2_p3] traj, fraction = arms.compute_cartesian_path(zip(wp_1, wp_2), (r1_ik_link, r2_ik_link), jump_threshold=2, avoid_collisions=False) if fraction < 1: rospy.logwarn("Unable to interpolate polish move") continue enable_collisions = [("t3_desk", "r1_gripper"), ("t3_desk", "r2_gripper")] if not services.check_trajectory( state0, traj, enable_collisions, wait_timeout=1): rospy.logwarn("Found trajectory is in collision!") continue trajs.append(traj) break print len(trajs) arms.set_robot_speed(0.05) arms.execute(trajs[0]) arms.execute(trajs[1])