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)
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)
#!/usr/bin/env python # Copyright (c) CloPeMa, EU-FP7 - All Rights Reserved # # Author: Libor Wagner <*****@*****.**> # Institute: Czech Technical University in Prague # Created on: Oct 15, 2013 from clopema_libs.ui import ask from clopema_robot import ClopemaRobotCommander if __name__ == '__main__': group = ClopemaRobotCommander("arms") group.set_named_target('home_arms') group.plan() r = ask("Execute trajectory", {'Y': "Yes", "n": "No"}) if r == 'y': group.go() group = ClopemaRobotCommander("ext") group.set_named_target('home_ext') group.plan() r = ask("Execute trajectory", {'Y': "Yes", "n": "No"}) if r == 'y': group.go()
if args['T'] is not None and args['T'] is not '-': robot.set_joint_value_target("r1_joint_t", radians(float(args['T']))) traj = robot.plan() # Print some info print "From: ", " ".join([ "% +3.2f" % degrees(r) for r in traj.joint_trajectory.points[0].positions ]) print "To: ", " ".join([ "% +3.2f" % degrees(r) for r in traj.joint_trajectory.points[-1].positions ]) print "Length: ", len(traj.joint_trajectory.points) if args['--speed'] is not None: print "Speed ", "%+3.2f" % float(args['--speed']) else: print "Speed ", "%+3.2f" % robot.get_robot_speed() r = ask("Execute trajectory [Y,n]? ", ['y', 'n'], 'y') if r == 'y': if args['--speed'] is not None: robot.set_robot_speed(float(args['--speed'])) robot.go() if args['--sof']: robot.set_servo_power_off(True)