def StackCups(robot, table, cup, stack, cups_stacked, manip=None):
"""
@param robot The robot performing the stacking
@param cup The cup that is being stacked
@param stack The stack of cups the cup is going to be placed on
@param cups_stacked A list of the cups that have been stacked
@param manip The arm being used to stack cups
"""
if manip == None:
manip = robot.GetActiveManipulator()
env = robot.GetEnv()
with env:
aabb_cup = cup.ComputeAABB()
aabb_stack = stack.ComputeAABB()
stack_height = 2*aabb_stack.extents()[2] + (len(cups_stacked)+1)*0.03
herb_in_world = robot.GetTransform()
cup_in_world = cup.GetTransform()
cup_in_herb = numpy.dot(numpy.linalg.inv(herb_in_world), cup_in_world)
theta = numpy.arctan2(cup_in_herb[1,0], cup_in_herb[0,0])
if manip == robot.left_arm:
yaw_in_herb = numpy.array([-numpy.pi, 0])
else:
yaw_in_herb = numpy.array([0, numpy.pi])
yaw_in_cup = yaw_in_herb - theta
robot.PushGrasp(cup, yaw_range=yaw_in_cup)
from prpy.rave import Disabled
with Disabled(table):
#Lift up cup
manip.PlanToEndEffectorOffset(direction=[0, 0, 1], distance = stack_height, position_tolerance=0.1, execute=True)
with env:
aabb_cup = cup.ComputeAABB()
aabb_stack = stack.ComputeAABB()
direc = numpy.array([0., 0., 0.])
direc[:2] = aabb_stack.pos()[:2] - aabb_cup.pos()[:2]
dist = numpy.linalg.norm(direc)
manip.PlanToEndEffectorOffset(direction=direc, distance = dist, position_tolerance=0.05, execute=True)
#Move cup down
with Disabled(stack):
manip.PlanToEndEffectorOffset(direction=[0, 0, -1], distance = 0.02, position_tolerance=0.1, execute=True)
#Release cup
manip.hand.OpenHand()
with env:
robot.Release(cup)
cup.SetTransform(stack.GetTransform())
#Add cup to list of cups stacked
cups_stacked.append(cup)
return cups_stacked
def MoveCupAndPour(robot, table, manip_pitcher, manip_cup, cup, pitcher):
"""
@param robot The robot performing the pouring
@param table The table the objects are on
@param manip_pitcher The arm that is manipulating the pitcher
@param manip_cup The arm that is manipulating the cup
@param cup The cup to pour into
@param pitcher The pitcher to pour
"""
env = robot.GetEnv()
with env:
pitcher_start = pitcher.GetTransform()
pitcher_aabb = pitcher.ComputeAABB()
cup_pose = cup.GetTransform()
cup_aabb = cup.ComputeAABB()
manip_cup_pose = manip_cup.GetEndEffectorTransform()
manip_pitcher_pose = manip_pitcher.GetEndEffectorTransform()
p_tsr, min_tilt, max_tilt = robot.tsrlibrary(pitcher, 'pour')
traj = manip_pitcher.PlanToTSR(p_tsr, execute=False)
cspec = traj.GetConfigurationSpecification()
wpt = traj.GetWaypoint(traj.GetNumWaypoints() - 1)
config = cspec.ExtractJointValues(wpt, robot,
manip_pitcher.GetArmIndices())
with env:
with robot.CreateRobotStateSaver(
openravepy.KinBody.SaveParameters.LinkTransformation):
manip_pitcher.SetDOFValues(config)
desired_cup_in_world_x = pitcher_start[0, 3] + (
2 * pitcher_aabb.extents()[0])
desired_cup_in_world_y = pitcher_start[1, 3] - (
2 * pitcher_aabb.extents()[1])
cup_in_world_x = cup_pose[0, 3]
cup_in_world_y = cup_pose[1, 3]
move_x = desired_cup_in_world_x - cup_in_world_x + cup_aabb.extents(
)[0] / 2
move_y = desired_cup_in_world_y - cup_in_world_y - cup_aabb.extents(
)[1]
move_cup = math.sqrt(move_x**2 + move_y**2)
from prpy.rave import Disabled
with Disabled(table):
manip_cup.PlanToEndEffectorOffset(direction=[move_x, move_y, 0],
distance=move_cup,
position_tolerance=0.1,
execute=True,
timelimit=10)
manip_cup.PlanToEndEffectorOffset(direction=[0, 0, -1],
distance=0.007,
position_tolerance=0.1,
execute=True,
timelimit=10)
manip_cup.hand.OpenHand()
with env:
robot.Release(cup)
manip_cup.PlanToEndEffectorOffset(direction=-1 * manip_cup_pose[:3, 2],
distance=0.1,
position_tolerance=0.1,
execute=True,
timelimit=10)
manip_cup.PlanToNamedConfiguration('home', execute=True)
# Pour
# slow the arm down
v = numpy.array([0.75, 0.75, 2., 2., 2.5, 2.5, 2.5])
with robot.CreateRobotStateSaver(
openravepy.KinBody.SaveParameters.JointMaxVelocityAndAcceleration):
manip_pitcher.SetVelocityLimits(0.5 * v, 0.3)
#Gets position of the pitcher and pours
traj = robot.PostProcessPath(traj)
robot.ExecuteTrajectory(traj)
time.sleep(1) #To let the water pour from pitcher into cup
# Tilts pitcher back to original position
robot.ExecuteTrajectory(openravepy.planningutils.ReverseTrajectory(traj))
manip_pitcher.PlanToEndEffectorOffset(direction=-1 *
manip_pitcher_pose[:3, 0],
distance=0.1,
position_tolerance=0.1,
execute=True)
def _GrabBlock(robot, blocks, table, manip=None, preshape=None, **kw_args):
"""
@param robot The robot performing the grasp
@param block The block to grab
@param table The table, or object, the block is resting on
@param manip The manipulator to use to grab the block
(if None active manipulator is used)
@param preshape The shape (dof_values) to move the hand to before
executing the grab
"""
from prpy.rave import AllDisabled, Disabled
from prpy.viz import RenderTSRList, RenderVector
env = robot.GetEnv()
block = None
if manip is None:
with env:
manip = robot.GetActiveManipulator()
if preshape is None:
preshape = [1.7, 1.7, 0.2, 2.45]
# First move the hand to the right preshape
manip.hand.MoveHand(*preshape)
block_tsr_list = []
with env:
for b in blocks:
# Get a TSR to move near the block.
tsr_list = robot.tsrlibrary(b, 'grasp', manip=manip)
block_tsr_list += tsr_list
# Plan to a pose above the block
with RenderTSRList(block_tsr_list, robot.GetEnv()):
with Disabled(table, padding_only=True):
manip.PlanToTSR(block_tsr_list, execute=True)
with manip.GetRobot().GetEnv():
ee_pose = manip.GetEndEffectorTransform()
block_idxs = [
idx for idx, tsr_chain in enumerate(block_tsr_list)
if tsr_chain.contains(ee_pose)
]
if len(block_idxs) == 0:
raise NoTSRException("Failed to find the TSR PlanToTSR planned to")
block = blocks[block_idxs[0]]
try:
with AllDisabled(env, [table] + blocks, padding_only=True):
# Move down until touching the table
with env:
start_point = manip.GetEndEffectorTransform()[0:3, 3]
table_aabb = ComputeEnabledAABB(table)
#table_height = table_aabb.pos()[2] + table_aabb.extents()[2]
# 0.14 is the distance from finger tip to end-effector frame
#current_finger_height = manip.GetEndEffectorTransform()[2, 3] - 0.14
min_distance = 0.10 # current_finger_height - table_height
down_direction = [0., 0., -1.]
with RenderVector(start_point, down_direction, min_distance, env):
manip.MoveUntilTouch(direction=down_direction,
timelimit=5,
distance=min_distance,
max_distance=min_distance + 0.05,
ignore_collisions=blocks + [table])
# Move parallel to the table to funnel the block into the fingers
# by projecting the -x direction of end-effector onto the xy-plane.
with env:
start_point = manip.GetEndEffectorTransform()[0:3, 3]
funnel_direction = -1. * manip.GetEndEffectorTransform()[:3, 0]
funnel_direction[2] = 0.
# TODO: We should only have to disable the block for this. Why does
# this fail if we do not disable the table?
with AllDisabled(env, blocks + [table]):
with RenderVector(start_point, funnel_direction, 0.1, env):
manip.PlanToEndEffectorOffset(direction=funnel_direction,
distance=0.08,
max_distance=0.12,
timelimit=5.,
execute=True)
# Close the finger to grab the block
manip.hand.MoveHand(f3=1.7)
# Compute the pose of the block in the hand frame
with env:
local_p = [0.01, 0, 0.24, 1.0]
hand_pose = manip.GetEndEffectorTransform()
world_p = numpy.dot(hand_pose, local_p)
block_pose = block.GetTransform()
block_pose[:, 3] = world_p
block_relative = numpy.dot(numpy.linalg.inv(hand_pose), block_pose)
# Now lift the block up off the table
with AllDisabled(env, blocks + [table]):
manip.PlanToEndEffectorOffset(direction=[0, 0, 1],
distance=0.05,
timelimit=5,
execute=True)
# OpenRAVE trick to hallucinate the block into the correct
# pose relative to the hand
with env:
hand_pose = manip.GetEndEffectorTransform()
block_pose = numpy.dot(hand_pose, block_relative)
block.SetTransform(block_pose)
manip.GetRobot().Grab(block)
return block
except PlanningError as e:
logger.error('Failed to complete block grasp')
raise
glass = env.ReadKinBodyXMLFile(glass_path)
env.Add(glass)
glass_transform = numpy.eye(4)
glass_transform[:3, 3] = [0.6239455840637041, -0.4013916328109689, 0.75]
glass.SetTransform(glass_transform)
# Goal pose for the object
import numpy
goal_in_table = [0.15, 0., -0.03, 1.]
goal_in_world = numpy.dot(table.GetTransform(), goal_in_table)
goal_pose = goal_in_world[:2]
goal_radius = 0.1
from prpy.rave import Disabled
from prpy.util import ComputeEnabledAABB
with Disabled(table, padding_only=True):
table_aabb = ComputeEnabledAABB(table)
table_height = 0.01 + table_aabb.pos()[2] + table_aabb.extents()[2]
pts = []
for a in numpy.arange(0, 2.*numpy.pi, 0.1):
pts.append([goal_pose[0] + goal_radius*numpy.sin(a),
goal_pose[1] + goal_radius*numpy.cos(a),
table_height])
h = env.drawlinestrip(points=numpy.array(pts), linewidth=5.0,
colors=numpy.array([0.0, 1., 0.]))
# Plan to the start configuration
try:
push_arm.SetActive()
start_pose = [4.49119545, -1.59899798, -0.6, 1.65274406, -1.7742985, -0.63854765, -1.23051631]
def PushToPoseOnTable(robot,
obj,
table,
goal_position,
goal_radius,
manip=None,
max_plan_duration=30.0,
shortcut_time=3.,
render=True,
search=True,
tracker=None,
**kw_args):
"""
@param robot The robot performing the push
@param obj The object to push
@param table The table kinbody the object is resting on
@param goal_position The desired (x,y) position of the object in
world coordinates
@param goal_radius The max distance from goal_position for the object
to be to still consider the goal achieved
@param manip The manipulator to use for the push - if None the active
manipulator is used
@param max_plan_duration The max time to run the planner
@param shortcut_time The amount of time to spend shortcutting,
if 0. no shortcutting is performed
@param render If true, render the trajectory while executing
@param tracker The perception module to use to track the object
during pushing
"""
# Get a push planner
try:
if search:
from or_pushing.discrete_search_push_planner import DiscreteSearchPushPlanner
planner = DiscreteSearchPushPlanner(robot.GetEnv())
else:
from or_pushing.push_planner import PushPlanner
planner = PushPlanner(robot.GetEnv())
except ImportError:
raise ActionError(
"Unable to create PushPlanner. Is the randomized_rearrangement_planning"
"repository checked out in your workspace?")
# Get the manipulator
if manip is None:
with robot.GetEnv():
manip = robot.GetActiveManipulator()
with robot.GetEnv():
from prpy.rave import Disabled
from prpy.util import ComputeEnabledAABB
with Disabled(table, padding_only=True):
table_aabb = ComputeEnabledAABB(table)
ee_pushing_transform = manip.GetEndEffectorTransform()
# Make the state bounds be at the edges of the table
table_pos = table_aabb.pos()
table_extents = table_aabb.extents()
sbounds = {
'high': [
table_pos[0] + table_extents[0], table_pos[1] + table_extents[1],
2. * numpy.pi
],
'low':
[table_pos[0] - table_extents[0], table_pos[1] - table_extents[1], 0]
}
# Assume we want to keep the current orientation and height of the
# manipulator throughout the push
ee_pushing_transform[:2, 3] = [0., 0.] #ignore x,y pose
# Compute the goal pose
#table_height = table_pos[2] + table_extents[2]
goal_pose = [goal_position[0], goal_position[1]]
with robot.CreateRobotStateSaver():
traj = planner.PushToPose(
robot,
obj,
goal_pose,
state_bounds=sbounds,
pushing_manifold=ee_pushing_transform.flatten().tolist(),
max_plan_duration=max_plan_duration,
goal_radius=goal_radius,
**kw_args)
if traj is None:
raise PlanningError('Failed to find pushing plan')
# Execute
from prpy.viz import RenderTrajectory
with RenderTrajectory(robot, traj, color=[1, 0, 0, 1], render=render):
if shortcut_time > 0:
traj = planner.ShortcutPath(timelimit=shortcut_time)
with RenderTrajectory(robot, traj, color=[0, 0, 1, 1], render=render):
if tracker is not None:
tracker.StartTrackObject(robot, obj.GetName())
try:
if manip.simulated:
# Use the push planner code to simulate path execution.
# This simulates the pushing of the objects during
# execution of the trajectory.
planner.ExecutePlannedPath()
else:
# Execute the trajectory
robot.ExecuteTrajectory(traj)
# During execution, object pushes won't be simulated.
# In the OpenRAVE world, the robot will instead move
# through the objects and probably be in collision at
# the end. This call sets all the objects to their
# expected poses at the end of the trajectory.
# If execution was successful, this should resolve
# collisions.
planner.SetFinalObjectPoses()
finally:
if tracker is not None:
tracker.StopTrackObject(robot, obj.GetName())
return traj