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)