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 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 main():
# Process cammand line arguments
argv = rospy.myargv(argv=sys.argv)
opt = docopt(__doc__, argv=argv[1:])
opt_debug = opt['--debug']
opt_trajectory_file = opt['<trajectory_file>']
opt_force = opt['--force']
opt_no_confirm = opt['--no-confirm']
opt_xtion = opt['--xtion']
opt_output = opt['--output']
if opt_debug:
print opt
# Initialise node
rospy.init_node(NODE_NAME, anonymous=NODE_ANONYMOUS)
# Initialise robot
robot = ClopemaRobotCommander(ROBOT_GROUP)
# Initialise tf buffer
tfb = get_tf2_buffer()
# Load trajectories
trajs = read_msgs(opt_trajectory_file, RobotTrajectory)
# Infer what xtion we are calibrating
if opt_xtion is None:
xtion = get_xtion(infer_xtion(trajs))
else:
xtion = get_xtion(int(opt_xtion))
# Check output directory
if opt_output is not None:
output_path = opt_output
if not prepare_output_directory(output_path, opt_force):
rospy.logerr(
"Output directory is not empty, try adding -f if you want to overwrite it."
)
return
else:
output_path = prepare_unique_directory("%s_calib" % xtion.name)
# Initialise measurement
calib = CameraCalib(robot, tfb, xtion, output_path)
calib.confirm = not opt_no_confirm
# Run calibration capture
calib.run(trajs)
# Turn servo off
robot.set_servo_power_off()
def main():
# Initialize node
rospy.init_node(node_name, anonymous=node_anonymous)
# Initialize robot commander
arms = ClopemaRobotCommander("arms")
# Move to start position
move_to_start_state()
# Compute polish move
traj = compute_polish_move(p0, p1, length=0.2, frame_id=frame_id)
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)
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)
--eef-step STEP Step of the end effector [default: 0.01].
The '--' is required if you are using negative numbers.
"""
import rospy
from clopema_libs import docopt
from clopema_libs.ui import ask
from clopema_robot import ClopemaRobotCommander
from copy import deepcopy
if __name__ == '__main__':
rospy.init_node("move_pose", anonymous=True)
args = docopt(__doc__, options_first=True)
group = ClopemaRobotCommander.get_group_for_ee(args['--link'])
robot = ClopemaRobotCommander(group)
link_id = args['--link']
link = robot.robot.get_link(link_id)
pose = link.pose().pose
pose_ = deepcopy(pose)
eef_step = float(args['--eef-step'])
# Print info
print "group: ", group
print "link_id: ", link_id
print "eef_step: ", eef_step
if args['X'] is not None and args['X'] is not '-':
pose_.position.x = float(args['X'])
if args['Y'] is not None and args['Y'] is not '-':
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 __name__ == "__main__":
# Parse command line
args = docopt(__doc__)
_DEBUG = args['--debug']
# Print all arguments if debug
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':
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])
def move_to_start_state():
ext = ClopemaRobotCommander("ext")
ext.set_named_target("ext_minus_90")
traj = ext.plan()
ext.execute(traj)
#!/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_robot import ClopemaRobotCommander
if __name__ == '__main__':
ClopemaRobotCommander.set_servo_power_off()
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)
#!/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()
self.view.set_body(urwid.Filler(text, 'top'))
self.update_alarm = self.loop.set_alarm_in(self.refresh, self.update)
def exit_on_q(self, input):
if input in ('q', 'Q'):
raise urwid.ExitMainLoop()
def format_val(self, name, value):
if name in self.values:
rec = self.values[name]
if np.allclose(rec[0], value):
s = ("body", "% -8.3f" % value)
else:
s = ("warn", "% -8.3f" % value)
if rec[1] > 2 / self.refresh:
self.values[name] = (value, 0)
else:
self.values[name] = (rec[0], rec[1] + 1)
else:
s = ("body", "% -8.3f" % value)
self.values[name] = (value, 0)
return s
if __name__ == "__main__":
rospy.init_node("robot_monitor", anonymous=True)
robot = ClopemaRobotCommander('arms')
monitor = Monitor(robot)
The S,L,U,R,B,T arguments are joint radiuses in degrees it can also be '-' and
that means that the joint stays the same.
The '--' is required if you are using negative numbers.
"""
from math import radians, degrees
from clopema_libs import docopt
from clopema_libs.ui import ask
from clopema_robot import ClopemaRobotCommander
if __name__ == '__main__':
args = docopt(__doc__, options_first=True)
robot = ClopemaRobotCommander("r2_arm")
if args['S'] is not None and args['S'] is not '-':
robot.set_joint_value_target("r2_joint_s", radians(float(args['S'])))
if args['L'] is not None and args['L'] is not '-':
robot.set_joint_value_target("r2_joint_l", radians(float(args['L'])))
if args['U'] is not None and args['U'] is not '-':
robot.set_joint_value_target("r2_joint_u", radians(float(args['U'])))
if args['R'] is not None and args['R'] is not '-':
robot.set_joint_value_target("r2_joint_r", radians(float(args['R'])))
if args['B'] is not None and args['B'] is not '-':
robot.set_joint_value_target("r2_joint_b", radians(float(args['B'])))
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 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)