def test_clip_triangle():
sim = simulator.Simulator(view=True)
view_frustum = ViewFrustum((.6,.1,0), (.8,.2,.5),
(.4,.1,0), (.2,.2,.5),
(.4,-.1,0), (.2,-.2,.5),
(.6,-.1,0), (.8,-.2,.5))
view_frustum.plot(sim, color=(1,0,0), fill=True, alpha=.25)
for halfspace in view_frustum.halfspaces:
halfspace.plot(sim, color=(0,0,1))
# triangle = Triangle([.5, 0, .25], [.5, 0, .6], [.52, .02, .25])
# triangle = Triangle([.5, 0, .25], [.7, .5, .5], [.9, .1, .6])
triangle = Triangle([.6, .1, -.2], [.7, .2, .3], [.5, 0, .1])
triangle.plot(sim, color=(0,0,1))
print('Original triangle, press enter to clip')
raw_input()
# sim.clear_plots(3)
clipped_triangles = view_frustum.clip_triangle(triangle)
print('Number of clipped triangles: {0}'.format(len(clipped_triangles)))
for tri in clipped_triangles:
tri.plot(sim, color=(0,1,0))
print('Press enter to exit')
raw_input()
def test_pyramid_sd():
sim = simulator.Simulator(view=True)
trunc_pyramid = TruncatedPyramid((.8,.2,1), (.8,.2,1.5),
(.2,.2,1), (.2,.2,1.5),
(.2,-.2,1), (.2,-.2,1.5))
trunc_pyramid.plot(sim, frame='base_link', color=(0,1,0), fill=True, alpha=.25)
for halfspace in trunc_pyramid.halfspaces:
halfspace.plot(sim, frame='base_link', color=(0,0,1))
pos_step = .01
delta_position = {'a' : [0, pos_step, 0],
'd' : [0, -pos_step, 0],
'w' : [pos_step, 0, 0],
'x' : [-pos_step, 0, 0],
'+' : [0, 0, pos_step],
'-' : [0, 0, -pos_step]}
point = np.array((.7,.2,1), dtype=float)
print('Move point around with keyboard to test signed-distance')
char = ''
while char != 'q':
char = utils.Getch.getch()
sim.clear_plots(1)
if delta_position.has_key(char):
point += np.array(delta_position[char])
sim.plot_point(sim.transform_from_to(point, 'base_link', 'world'), size=.02, color=(0,0,1))
is_inside = trunc_pyramid.is_inside(point)
sd = trunc_pyramid.signed_distance(point)
print('is inside: {0}'.format(is_inside))
print('sd: {0}\n'.format(sd))
def test_return_from_grasp_planner():
sim = simulator.Simulator(view=True)
rarm = arm.Arm('right', sim=sim)
p = planner.Planner('right', sim=sim)
rarm.set_posture('mantis')
curr_pose = rarm.get_pose()
curr_pose.orientation = tfx.tb_angles(0, 0, 180)
rarm.set_pose(curr_pose)
target_pose = rarm.get_pose() + [0.5, 0.4, -0.3]
target_pose.orientation *= tfx.tb_angles(25, -20, 10)
sim.plot_transform(target_pose.matrix)
start_joints = rarm.get_joints()
n_steps = 40
joint_traj = p.get_return_from_grasp_joint_trajectory(start_joints,
target_pose,
n_steps=n_steps)
poses = [tfx.pose(rarm.fk(joints)) for joints in joint_traj]
for pose in poses:
sim.plot_transform(pose.matrix)
print('Press enter to exit')
raw_input()
def test_grasp_planner():
sim = simulator.Simulator(view=True)
rarm = arm.Arm('right', sim=sim)
p = planner.Planner('right', sim=sim)
rarm.set_posture('mantis')
curr_pose = rarm.get_pose()
curr_pose.orientation = tfx.tb_angles(0, 0, 180)
rarm.set_pose(curr_pose)
target_pose = rarm.get_pose() + [0.5, 0.4, -0.3]
target_pose.orientation *= tfx.tb_angles(25, -20, 10)
sim.plot_transform(target_pose.matrix)
start_joints = rarm.get_joints()
n_steps = 20
joint_traj = p.get_grasp_joint_trajectory(
start_joints,
target_pose,
n_steps=n_steps,
ignore_orientation=True,
link_name='r_gripper_center_frame')
poses = [tfx.pose(rarm.fk(joints)) for joints in joint_traj]
for pose in poses:
sim.plot_transform(pose.matrix)
for n in xrange(n_steps - 1):
angle = (poses[n].orientation.inverse() * poses[-1].orientation).angle
print('angle[{0}]: {1}'.format(n, angle))
#IPython.embed()
print('Press enter to exit')
raw_input()
def test_ik():
sim = simulator.Simulator(view=True)
larm = arm.Arm('left', sim=sim)
larm.set_posture('mantis')
rarm = arm.Arm('right', sim=sim)
rarm.set_posture('mantis')
pose = rarm.get_pose()
position = pose.position
while True:
point = pose.position + np.array([
np.random.uniform(0, 1),
np.random.uniform(-.3, .3),
np.random.uniform(-.5, -.2)
])
sim.plot_point(sim.transform_from_to(point.array, 'base_link',
'world'),
color=(0, 1, 0),
size=.1)
joints = rarm.ik_lookat(position, point)
if joints is not None:
rarm.set_joints(joints)
else:
print('Joints is None')
sim.plot_transform(
sim.transform_from_to(np.array(rarm.get_pose().matrix),
'base_link', 'world'))
raw_input()
sim.clear_plots()
def __init__(self, arm_name, sim=None, interact=False):
"""
:param arm_name: "left" or "right"
:param sim: OpenRave simulator (or create if None)
:param interact: enable trajopt viewer
"""
assert arm_name == 'left' or arm_name == 'right'
self.sim = sim
if self.sim is None:
self.sim = simulator.Simulator()
self.robot = self.sim.robot
self.manip = self.sim.larm if arm_name == 'left' else self.sim.rarm
wrist_flex_index = self.robot.GetJointIndex(arm_name[0] +
'_wrist_flex_joint')
lower, upper = self.robot.GetDOFLimits()
lower[wrist_flex_index], upper[
wrist_flex_index] = -np.pi / 2., np.pi / 2.
self.robot.SetDOFLimits(lower, upper)
if interact:
trajoptpy.SetInteractive(True)
self.tool_frame = '{0}_gripper_tool_frame'.format(arm_name[0])
self.joint_indices = self.manip.GetArmIndices()
def test_box():
sim = simulator.Simulator(view=True)
pose = tfx.pose([1,0,0], tfx.tb_angles(0,45,0))
extents = [0.1,0.1,0.1]
sim.add_box(pose.matrix, extents)
print('Press enter to exit')
raw_input()
def test_mantis():
sim = simulator.Simulator(view=True)
rarm = arm.Arm('right', sim=sim)
larm = arm.Arm('left', sim=sim)
rarm.go_to_posture('mantis', block=False)
larm.go_to_posture('mantis', block=False)
rarm.open_gripper()
larm.open_gripper()
def test_gripper():
sim = simulator.Simulator(view=True)
rarm = arm.Arm('right', sim=sim)
larm = arm.Arm('left', sim=sim)
# rarm.close_gripper()
# larm.close_gripper()
rarm.open_gripper()
larm.open_gripper()
def __init__(self):
self.pc = None
self.pc_sub = rospy.Subscriber('/cloud_pcd', sm.PointCloud2,
self._pc_callback)
self.sim = simulator.Simulator(view=True)
self.rarm = arm.Arm('right', sim=self.sim)
self.larm = arm.Arm('left', sim=self.sim)
self.cam = camera.Camera(self.arm, self.sim)
self.rarm.set_posture('mantis')
self.larm.set_posture('mantis')
def test_distance_to_plot():
sim = simulator.Simulator(view=True)
t = Triangle(np.random.rand(3), np.random.rand(3), np.random.rand(3))
t.plot(sim)
p = 2*np.random.rand(3)
closest_pt = t.closest_point_to(p)
sim.plot_point(p, color=(0,0,1))
sim.plot_point(closest_pt)
IPython.embed()
def __init__(self, arm_name, sim=None, default_speed=.05):
"""
:param arm_name: "left" or "right"
:param sim: OpenRave simulator (or create if None)
:param default_speed: speed in meters/second
"""
assert arm_name == 'left' or arm_name == 'right'
self.arm_name = arm_name
self.default_speed = default_speed
self.tool_frame = '{0}_gripper_tool_frame'.format(arm_name[0])
self.joint_names = [
'{0}{1}'.format(arm_name[0], joint_name)
for joint_name in self.joint_name_suffixes
]
self.gripper_joint_name = '{0}_gripper_joint'.format(arm_name[0])
self.min_grasp, self.max_grasp = -.01, .1
self.default_max_effort = 40 # Newtons
self.current_joints, self.current_grasp = None, None
self.joint_state_sub = rospy.Subscriber('/joint_states', sm.JointState,
self._joint_state_callback)
rospy.loginfo('Waiting for /joint_states...')
while not rospy.is_shutdown(
) and self.current_joints is None or self.current_grasp is None:
rospy.sleep(.01)
self.joint_command_pub = rospy.Publisher(
'{0}_arm_controller/command'.format(arm_name[0]),
tm.JointTrajectory)
self.joint_command_client = actionlib.SimpleActionClient(
'/{0}_arm_controller/joint_trajectory_action'.format(arm_name[0]),
pcm.JointTrajectoryAction)
rospy.loginfo('Waiting for joint command server...')
self.joint_command_client.wait_for_server()
#self.gripper_command_pub = rospy.Publisher('{0}_gripper_controller/command'.format(arm_name[0]), pcm.Pr2GripperCommand)
self.gripper_command_client = actionlib.SimpleActionClient(
'{0}_gripper_controller/gripper_action'.format(arm_name[0]),
pcm.Pr2GripperCommandAction)
rospy.loginfo('Waiting for gripper command server...')
self.gripper_command_client.wait_for_server()
self.sim = sim
if self.sim is None:
self.sim = simulator.Simulator()
self.manip = self.sim.larm if arm_name == 'left' else self.sim.rarm
rospy.sleep(1)
def __init__(self):
self.pc = None
self.pc_sub = rospy.Subscriber('/cloud_pcd', sm.PointCloud2,
self._pc_callback)
self.handle_pose = None
self.handle_pose_sub = rospy.Subscriber(
'/handle_detector/avg_handle_poses', gm.PoseArray,
self._avg_handle_poses_callback)
self.sim = simulator.Simulator(view=True)
self.arm = arm.Arm('right', sim=self.sim)
self.cam = camera.Camera(self.arm, self.sim)
self.planner = planner.Planner('right', sim=self.sim)
def test_gripper_center_frame():
sim = simulator.Simulator(view=True)
larm = arm.Arm('left', sim=sim)
larm.set_posture('mantis')
rarm = arm.Arm('right', sim=sim)
rarm.set_posture('mantis')
print('Showing r_gripper_tool_frame')
sim.plot_transform(
sim.robot.GetLink('r_gripper_tool_frame').GetTransform())
raw_input()
print('Showing r_gripper_center_frame')
sim.plot_transform(
sim.robot.GetLink('r_gripper_center_frame').GetTransform())
raw_input()
def __init__(self):
self.graspable_points = None
self.table_pose = None
self.table_extents = None
self.avg_handle_poses = None
self.is_reset_kinfu = False
self.home_pose = None
min_positions = np.array([0.25, -1, 0.3])
max_positions = np.array([1, 1, 1.5])
self.point_cloud_filter = lambda point: min(
min_positions < point) and min(point < max_positions)
# self.point_cloud_filter = lambda point: True
self.graspable_points_sub = rospy.Subscriber(
'/kinfu/graspable_points', sm.PointCloud2,
self._graspable_points_callback)
self.table_sub = rospy.Subscriber('/kinfu/plane_bounding_box',
pcl_utils.msg.BoundingBox,
self._table_callback)
self.avg_handle_poses_sub = rospy.Subscriber(
'/handle_detector/avg_handle_poses', gm.PoseArray,
self._avg_handle_poses_callback)
self.reset_kinfu_sub = rospy.Subscriber('/kinfu/reset',
std_msgs.msg.Empty,
self._reset_kinfu_callback)
self.home_pose_sub = rospy.Subscriber('/bsp/home_pose', gm.PoseStamped,
self._home_pose_callback)
self.grasp_joint_traj_pub = rospy.Publisher(
'/check_handle_grasp/grasp_joint_trajectory', tm.JointTrajectory)
self.return_grasp_joint_traj_pub = rospy.Publisher(
'/check_handle_grasp/return_grasp_joint_trajectory',
tm.JointTrajectory)
self.sim = simulator.Simulator(view=True)
self.arm = arm.Arm('right', sim=self.sim)
self.planner = planner.Planner('right', sim=self.sim)
self.arm.set_posture('mantis')
self.home_pose = self.arm.get_pose()
rospy.sleep(0.5)
self.sim.update()
def __init__(self):
#self.grasp_traj = None
#self.return_grasp_traj = None
#self.grasp_traj_sub = rospy.Subscriber('/check_handle_grasps/grasp_trajectory', gm.PoseArray, self._grasp_traj_callback)
#self.return_grasp_traj_sub = rospy.Subscriber('/check_handle_grasps/return_grasp_trajectory', gm.PoseArray, self._return_grasp_traj_callback)
self.grasp_joint_traj = None
self.return_grasp_joint_traj = None
self.grasp_joint_traj_sub = rospy.Subscriber(
'/check_handle_grasp/grasp_joint_trajectory', tm.JointTrajectory,
self._grasp_joint_traj_callback)
self.return_grasp_joint_traj_sub = rospy.Subscriber(
'/check_handle_grasp/return_grasp_joint_trajectory',
tm.JointTrajectory, self._return_grasp_joint_traj_callback)
self.sim = simulator.Simulator(view=True)
self.arm = arm.Arm('right', sim=self.sim)
self.speed = 0.06
def test_kinect():
sim = simulator.Simulator(view=True)
larm = arm.Arm('left', sim=sim)
rarm = arm.Arm('right', sim=sim)
sim.add_box(rarm.get_pose(), [.5, .5, .5])
larm.set_posture('mantis')
rarm.set_posture('mantis')
manip_links = [
l for l in sim.robot.GetLinks()
if l not in rarm.manip.GetIndependentLinks()
]
while True:
print('Press "q" to exit')
if (utils.Getch.getch() == 'q'):
break
rarm.teleop()
is_coll = max([sim.env.CheckCollision(l) for l in manip_links])
print('Is in collision: {0}\n'.format(is_coll))
def test_plotting():
sim = simulator.Simulator(view=True)
sim.plot_triangle(([1,0,1],[1,0,2],[1.5,0,1.5]),color=(1,0,0),alpha=0.5)
view_frustum = ViewFrustum((.6,.1,0), (.8,.2,.5),
(.4,.1,0), (.2,.2,.5),
(.4,-.1,0), (.2,-.2,.5),
(.6,-.1,0), (.8,-.2,.5))
view_frustum.plot(sim, color=(1,0,0), fill=True, alpha=.25)
for halfspace in view_frustum.halfspaces:
halfspace.plot(sim, color=(0,0,1))
trunc_pyramid = TruncatedPyramid((.6,.1,1), (.8,.2,1.5),
(.4,.1,1), (.2,.2,1.5),
(.4,-.1,1), (.2,-.2,1.5))
trunc_pyramid.plot(sim, color=(0,1,0), fill=True, alpha=.25)
for halfspace in trunc_pyramid.halfspaces:
halfspace.plot(sim, color=(0,0,1))
print('Press enter to exit')
raw_input()
def test_joints():
sim = simulator.Simulator(view=True)
rarm = arm.Arm('right', sim=sim)
larm = arm.Arm('left', sim=sim)
print('rarm joints: {0}'.format(rarm.get_joints()))
def test_gradient_sd():
sim = simulator.Simulator(view=True)
larm = arm.Arm('left', sim=sim)
larm.set_posture('mantis')
rarm = arm.Arm('right', sim=sim)
rarm.set_posture('mantis')
cam = camera.Camera(rarm, sim)
# triangles3d = []
# triangles3d = [geometry3d.Triangle([.7,0,.8],[.7,0,1.1],[.7,-.3,.7])]
# triangles3d = [geometry3d.Triangle([.5,0,.5],[.8,0,.6],[.5,-.3,.9])]
# triangles3d = [geometry3d.Triangle([np.random.uniform(.2,.5), np.random.uniform(-.5,0), np.random.uniform(.25,.75)],
# [np.random.uniform(.2,.5), np.random.uniform(-.5,0), np.random.uniform(.25,.75)],
# [np.random.uniform(.2,.5), np.random.uniform(-.5,0), np.random.uniform(.25,.75)]) for _ in xrange(3)]
table_center = np.array([.2, .7, .5])
triangles3d = [ #geometry3d.Triangle(table_center, table_center+np.array([.5,-1.4,0]), table_center+np.array([.5,0,0])),
#geometry3d.Triangle(table_center, table_center+np.array([0,-1.4,0]), table_center+np.array([.5,-1.4,0])),
geometry3d.Triangle(table_center + np.array([.25, -.7, 0]),
table_center + np.array([.25, -.7, .2]),
table_center + np.array([.25, -1.2, 0])),
geometry3d.Triangle(table_center + np.array([.25, -1.2, 0]),
table_center + np.array([.25, -.7, .2]),
table_center + np.array([.25, -1.2, .2]))
]
# point = np.array([0.5, 0.7, 0.66]) # sd: 0.115138180967
# point = np.array([0.5, -2, 0.66])
point = np.array([0.640, -0.170, 0.560]) # sd: 0.0189611173538
j = rarm.get_joints()
while True:
sim.clear_plots()
sim.plot_point(sim.transform_from_to(point, 'base_link', 'world'),
size=.02,
color=(0, 0, 1))
cam.plot(frame='base_link')
for tri3d in triangles3d:
tri3d.plot(sim, frame='base_link', fill=True)
sd = 0 #signed_distance(j, point, triangles3d, rarm, cam, plot=False)
start = time.time()
sd_grad = signed_distance_grad(j, point, triangles3d, rarm, cam)
elapsed = time.time() - start
start = time.time()
sd_grad_fast = fast_signed_distance_grad(j, point, triangles3d, rarm,
cam)
fast_elapsed = time.time() - start
print('sd: {0}'.format(sd))
print('sd_grad: {0}'.format(sd_grad))
print('sd_grad_fast: {0}'.format(sd_grad_fast))
print('sd_grad diff: {0}'.format(np.linalg.norm(sd_grad -
sd_grad_fast)))
print('sd_grad time: {0}'.format(elapsed))
print('sd_grad_fast time: {0}'.format(fast_elapsed))
print('Press enter to continue (or "q" to exit)')
char = utils.Getch.getch()
if char == 'q':
break
j -= (np.pi / 64.) * (sd_grad / np.linalg.norm(sd_grad))
rarm.set_joints(j)