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 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)
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 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])
