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)
if _DEBUG: print args # Initialize node rospy.init_node(_NODE_NAME, anonymous=True) # Initialize robot robot = ClopemaRobotCommander("arms") trajs = read_msgs(args['<file_name>'], RobotTrajectory) # Check speed if args['--speed'] is not None: print "Speed ", "%+3.2f" % float(args['--speed']) else: print "Speed ", "%+3.2f" % robot.get_robot_speed() if not args['--force']: r = ask("Execute trajectory? ", {'Y':'Yes','n':'No'}) else: r = 'y' if r == 'y': if args['--speed'] is not None: robot.set_robot_speed(float(args['--speed'])) for traj in trajs: robot.execute(traj) if not args['--no-sof']: robot.set_servo_power_off(True)