def table_edge(robot, table, **kwargs):
"""
This creates a TSR that allows you to sample poses from either
long edge of the table.
@param robot The robot (unused)
@param table The table
"""
table_in_world = table.GetTransform()
# Extents of the table - y-axis is normal to table surface
xdim = 0.93175 # half extent
zdim = 0.3805 # half extent
ydim = 0.785 # full extent
# We want to create a set of TSRs, one for each edge of the table
Bw_1 = numpy.zeros((6, 2))
Bw_1[0, :] = [-xdim, xdim]
Tw_e1 = numpy.array([[1., 0., 0., 0.], [0., 1., 0., ydim],
[0., 0., 1, zdim], [0., 0., 0., 1.]])
tsr1 = TSR(T0_w=table_in_world, Tw_e=Tw_e1, Bw=Bw_1)
tsr1_chain = TSRChain(TSR=tsr1)
Bw_2 = numpy.zeros((6, 2))
Bw_2[0, :] = [-xdim, xdim]
Tw_e2 = numpy.array([[-1., 0., 0., 0.], [0., -1., 0., ydim],
[0., 0., 1, -zdim], [0., 0., 0., 1.]])
tsr2 = TSR(T0_w=table_in_world, Tw_e=Tw_e2, Bw=Bw_2)
tsr2_chain = TSRChain(TSR=tsr2)
return [tsr1_chain, tsr2_chain]
def fuze_grasp(robot, fuze, manip=None):
'''
@param robot The robot performing the grasp
@param fuze The bottle to grasp
@param manip The manipulator to perform the grasp, if None
the active manipulator on the robot is used
'''
if manip is None:
manip_idx = robot.GetActiveManipulatorIndex()
else:
manip.SetActive()
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
T0_w = fuze.GetTransform()
ee_to_palm = 0.15
palm_to_bottle_center = 0.045
total_offset = ee_to_palm + palm_to_bottle_center
Tw_e = numpy.array([[ 0., 0., -1., -total_offset],
[ 0., 1., 0., 0.],
[ 1., 0., 0., 0.08], #bottle height
[ 0., 0., 0., 1.]])
Bw = numpy.zeros((6,2))
Bw[2,:] = [0.0, 0.02] # Allow a little vertical movement
Bw[5,:] = [-numpy.pi/2, numpy.pi/2] # Allow any orientation
grasp_tsr = TSR(T0_w = T0_w, Tw_e = Tw_e, Bw = Bw, manip = manip_idx)
grasp_chain = TSRChain(sample_start=False, sample_goal = True,
constrain=False, TSR = grasp_tsr)
return [grasp_chain]
def present_obj(robot, transform, manip=None):
"""
@param robot The robot performing the presentation gesture
@param transform The location of where the robot the presenting
@param manip The manipulator to present. This must be the right arm.
"""
(manip, manip_idx) = GetManipulatorIndex(robot, manip)
#Compute T0_w
T0_w = transform
#Compute TW_e with respect to right arm
TW_e = numpy.array([[0.92647484, 0.26522822, -0.26701753, -0.09831361],
[-0.37616512, 0.62994351, -0.67946374, -0.18565347],
[-0.012007, 0.72994874, 0.68339642, 0.18895879],
[0., 0., 0., 1.]])
#Compute Bw
Bw = numpy.zeros((6, 2))
Bw[5, :] = [-numpy.pi, numpy.pi]
T = TSR(T0_w=T0_w, Tw_e=TW_e, Bw=Bw, manip=manip_idx)
chain = TSRChain(TSRs=[T],
sample_goal=True,
sample_start=False,
constrain=False)
return [chain]
def bowl_grasp(robot, glass, manip=None):
'''
@param robot The robot performing the grasp
@param glass The glass to grasp
@param manip The manipulator to perform the grasp, if None
the active manipulator on the robot is used
'''
if manip is None:
manip_idx = robot.GetActiveManipulatorIndex()
else:
manip.SetActive()
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
T0_w = glass.GetTransform()
Tw_e = numpy.array([[ 0.,-1., 0., 0.08], #bowl width
[ 1., 0., 0., 0.],
[ 0., 0., 1., 0.22], #vertical hand offset
[ 0., 0., 0., 1.]])
Bw = numpy.zeros((6,2))
Bw[2,:] = [-0.02, 0.02] # Allow a little vertical movement
Bw[5,:] = [-numpy.pi, numpy.pi] # Allow any orientation
grasp_tsr = TSR(T0_w = T0_w, Tw_e = Tw_e, Bw = Bw, manip = manip_idx)
grasp_chain = TSRChain(sample_start=False, sample_goal = True,
constrain=False, TSR = grasp_tsr)
return [grasp_chain]
def present_obj(robot, transform, manip=None):
"""
@param robot The robot performing the presentation gesture
@param transform The location of where the robot the presenting
@param manip The manipulator to present. This must be the right arm.
"""
(manip, manip_idx) = GetManipulatorIndex(robot, manip)
if manip.GetName() != 'right':
raise prpy.exceptions.PrpyException(
'Presenting is only defined for the right arm.')
#Compute T0_w
T0_w = transform
#Compute TW_e with respect to right arm
TW_e = numpy.array([[-0.42480713, 0.81591405, 0.39220297, -0.10103751],
[0.08541525, -0.39518032, 0.91462383, -0.2806636],
[0.90124533, 0.42203884, 0.09818392, 0.16018878],
[0., 0., 0., 1.]])
#Compute Bw
Bw = numpy.zeros((6, 2))
Bw[5, :] = [-numpy.pi, numpy.pi]
T = TSR(T0_w=T0_w, Tw_e=TW_e, Bw=Bw, manip=manip_idx)
chain = TSRChain(TSRs=[T],
sample_goal=True,
sample_start=False,
constrain=False)
return [chain]
def point_on(robot, block_bin, manip=None, padding=0.04):
'''
This creates a TSR that allows you to sample poses on the tray.
The samples from this TSR should be used to find points for
object placement. They are directly on the tray, and thus not
suitable as an end-effector pose. Grasp specific calculations are
necessary to find a suitable end-effector pose.
@param robot The robot performing the grasp
@param tray The tray to sample poses on
@param manip The manipulator to perform the grasp, if None
the active manipulator on the robot is used
@param padding The amount of space around the edge to exclude
from sampling. If using this to place an object, this would
be the maximum radius of the object
@param handle_padding If true add extra padding along the edges
of the tray that have the handles to prevent choosing a pose
too near the handle of the tray
'''
if manip is None:
manip_idx = robot.GetActiveManipulatorIndex()
else:
manip.SetActive()
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
T0_w = block_bin.GetTransform() # Coordinate system on bottom of bin
Tw_e = numpy.eye(4)
Tw_e[2, 3] = 0.17 # set the object on top of the bin - bin is 13cm high
Bw = numpy.zeros((6, 2))
xdim = max(0.085 - padding, 0.0)
ydim = max(0.135 - padding, 0.0)
Bw[0, :] = [-xdim,
xdim] # move along x, y directions to get any point on tray
Bw[1, :] = [-ydim, ydim]
Bw[2, :] = [-0.02, 0.04] # verticle movement
Bw[5, :] = [
-numpy.pi, numpy.pi
] # allow any rotation around z - which is the axis normal to the tray top
manip_tsr = TSR(T0_w=T0_w, Tw_e=Tw_e, Bw=Bw, manip=manip_idx)
tsr_chain = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=manip_tsr)
return [tsr_chain]
def pills_transport(robot,
pills,
manip=None,
roll_epsilon=0.2,
pitch_epsilon=0.2,
yaw_epsilon=0.2):
'''
Generates a trajectory-wide constraint for transporting the object with little roll, pitch or yaw
Assumes the object has already been grasped and is in the proper
configuration for transport.
@param robot The robot grasping the glass
@param pills The grasped object
@param manip the manipulator grasping the object, if None the active manipulator
of the robot is used
@param roll_epsilon The amount to let the object roll during transport (object frame)
@param pitch_epsilon The amount to let the object pitch during transport (object frame)
@param yaw_epsilon The amount to let the object yaw during transport (object frame)
'''
if manip is None:
manip_idx = robot.GetActiveManipulatorIndex()
manip = robot.GetActiveManipulator()
else:
manip.SetActive()
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
ee_in_glass = numpy.dot(numpy.linalg.inv(pills.GetTransform()),
manip.GetEndEffectorTransform())
Bw = numpy.array([
[-100., 100.], # bounds that cover full reachability of manip
[-100., 100.],
[-100., 100.],
[-roll_epsilon, roll_epsilon],
[-pitch_epsilon, pitch_epsilon],
[-yaw_epsilon, yaw_epsilon]
])
transport_tsr = TSR(T0_w=glass.GetTransform(),
Tw_e=ee_in_glass,
Bw=Bw,
manip=manip_idx)
transport_chain = TSRChain(sample_start=False,
sample_goal=False,
constrain=True,
TSR=transport_tsr)
return [transport_chain]
def point_obj(robot, transform, manip=None):
"""
@param robot The robot performing the point
@param transform The location of where the robot is pointing to
@param manip The manipulator to point with. This must be the right arm.
"""
(manip, manip_idx) = GetManipulatorIndex(robot, manip)
if manip.GetName() != 'right':
raise prpy.exceptions.PrPyException(
'Pointing is only defined on the right arm.')
# compute T_ow
T0_w_0 = transform
T0_w_1 = numpy.identity(4)
# compute T_we with respect to right arm.
TW_e_0 = numpy.array([[0.07277, -0.71135, 0.69905, -0.41425],
[0.0336, 0.70226, 0.71111, -0.44275],
[-0.99678, -0.02825, 0.07501, -0.04314],
[0., 0., 0., 1.]])
TW_e_1 = numpy.identity(4)
# compute B_w
Bw_0 = numpy.zeros((6, 2))
Bw_0[3, :] = [-numpy.pi, numpy.pi]
Bw_0[4, :] = [0, numpy.pi]
Bw_0[5, :] = [-numpy.pi, numpy.pi]
Bw_1 = numpy.zeros((6, 2))
Bw_1[2, :] = [-0.5, 0.5]
T_0 = TSR(T0_w=T0_w_0, Tw_e=TW_e_0, Bw=Bw_0, manip=manip_idx)
T_1 = TSR(T0_w=T0_w_1, Tw_e=TW_e_1, Bw=Bw_1, manip=manip_idx)
chain = TSRChain(TSRs=[T_0, T_1],
sample_goal=True,
sample_start=False,
constrain=False)
return [chain]
def point_on(robot, table, manip=None, padding=0, **kwargs):
"""
This creates a TSR that allows you to sample poses on the table.
The sampled poses will have z-axis point up, normal to the table top.
The xy-plane of the sampled of the sample poses will be parallel to the table surface.
The samples from this TSR should be used to find points for object placement.
They are directly on the table, and thus not suitable as an end-effector pose.
Grasp specific calculations are necessary to find a suitable end-effector pose.
@param robot The robot performing the grasp
@param pitcher The pitcher to grasp
@param manip The manipulator to perform the grasp, if None
the active manipulator on the robot is used
@param padding The min distance of the sampled point from any edge of the table
"""
if manip is None:
manip_idx = robot.GetActiveManipulatorIndex()
else:
manip.SetActive()
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
T0_w = table.GetTransform()
# The frame is set on the table such that the y-axis is normal to the table surface
Tw_e = numpy.array([[1., 0., 0., 0.], [0., 0., 1., 0.75],
[0., -1., 0., 0.], [0., 0., 0., 1.]])
Bw = numpy.zeros((6, 2))
Bw[0, :] = [-0.93 + padding, 0.93 - padding
] # move along x and z directios to get any point on table
Bw[2, :] = [-0.38 + padding, 0.38 - padding]
Bw[4, :] = [
-numpy.pi, numpy.pi
] # allow any rotation around y - which is the axis normal to the table top
table_top_tsr = TSR(T0_w=T0_w, Tw_e=Tw_e, Bw=Bw, manip=manip_idx)
table_top_chain = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=table_top_tsr)
return [table_top_chain]
def func(robot, obj):
manip_idx = robot.GetActiveManipulatorIndex()
all_tsrs = []
for tsr in chain['TSRs']:
T0_w = obj.GetTransform()
Tw_e = numpy.array(tsr['Tw_e'])
Bw = numpy.array(tsr['Bw'])
yaml_tsr = TSR(T0_w=T0_w,
Tw_e=Tw_e,
Bw=Bw,
manip=manip_idx)
all_tsrs.append(yaml_tsr)
yaml_chain = TSRChain(sample_start=sample_start,
sample_goal=sample_goal,
constrain=constrain,
TSRs=all_tsrs)
return [yaml_chain]
def point_obj(robot, transform, manip=None):
"""
@param robot The robot performing the point
@param transform The location of where the robot is pointing to
@param manip The manipulator to point with. This must be the right arm.
"""
(manip, manip_idx) = GetManipulatorIndex(robot, manip)
# compute T_ow
T0_w_0 = transform
T0_w_1 = numpy.identity(4)
# compute T_we with respect to right arm.
TW_e_0 = numpy.array([[0.92647484, 0.26522822, -0.26701753, -0.09831361],
[-0.37616512, 0.62994351, -0.67946374, -0.18565347],
[-0.012007, 0.72994874, 0.68339642, 0.18895879],
[0., 0., 0., 1.]])
TW_e_1 = numpy.identity(4)
# compute B_w
Bw_0 = numpy.zeros((6, 2))
Bw_0[3, :] = [-numpy.pi, numpy.pi]
Bw_0[4, :] = [0, numpy.pi]
Bw_0[5, :] = [-numpy.pi, numpy.pi]
Bw_1 = numpy.zeros((6, 2))
Bw_1[2, :] = [-0.5, 0.5]
T_0 = TSR(T0_w=T0_w_0, Tw_e=TW_e_0, Bw=Bw_0, manip=manip_idx)
T_1 = TSR(T0_w=T0_w_1, Tw_e=TW_e_1, Bw=Bw_1, manip=manip_idx)
chain = TSRChain(TSRs=[T_0, T_1],
sample_goal=True,
sample_start=False,
constrain=False)
return [chain]
def pills_on_table(robot, pills, pose_tsr_chain, manip=None):
'''
Generates end-effector poses for placing the pill bottle on the table.
This factory assumes the pill bottle is grasped at the time it is called.
@param robot The robot grasping the glass
@param pills bottle The grasped object
@param pose_tsr_chain The tsr chain for sampling placement poses for the glass
@param manip The manipulator grasping the object, if None the active
manipulator of the robot is used
'''
if manip is None:
manip_idx = robot.GetActiveManipulatorIndex()
manip = robot.GetActiveManipulator()
else:
manip.SetActive()
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
ee_in_glass = numpy.dot(numpy.linalg.inv(pills.GetTransform()),
manip.GetEndEffectorTransform())
ee_in_glass[2, 3] += 0.04 # Let go slightly above table
Bw = numpy.zeros((6, 2))
Bw[2, :] = [0., 0.04] # Allow some lateral movement
for tsr in pose_tsr_chain.TSRs:
if tsr.manipindex != manip_idx:
raise Exception(
'pose_tsr_chain defined for a different manipulator.')
grasp_tsr = TSR(Tw_e=ee_in_glass, Bw=Bw, manip=manip_idx)
all_tsrs = list(pose_tsr_chain.TSRs) + [grasp_tsr]
place_chain = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSRs=all_tsrs)
return [place_chain]
def _pills_grasp(robot, pills, manip=None, push_distance=0.0, **kw_args):
"""
@param robot The robot performing the grasp
@param pills The pill bottle to grasp
@param manip The manipulator to perform the grasp, if None
the active manipulator on the robot is used
@param push_distance The offset distance for pushing
"""
if manip is None:
manip_idx = robot.GetActiveManipulatorIndex()
else:
manip.SetActive()
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
T0_w = pills.GetTransform()
ee_to_palm = 0.18
palm_to_glass_center = .04
total_offset = ee_to_palm + palm_to_glass_center + push_distance
Tw_e = numpy.array([
[0., 0., 1., -total_offset],
[1., 0., 0., 0.],
[0., 1., 0., 0.06], # bottle height
[0., 0., 0., 1.]
])
Bw = numpy.zeros((6, 2))
Bw[2, :] = [0.0, 0.02] # Allow a little vertical movement
Bw[5, :] = [-numpy.pi, numpy.pi] # Allow any orientation
grasp_tsr = TSR(T0_w=T0_w, Tw_e=Tw_e, Bw=Bw, manip=manip_idx)
grasp_chain = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=grasp_tsr)
return [grasp_chain]
def sweep_objs(robot, transform, manip=None):
"""
This motion sweeps from where the manipulator currently is
to the transform given as input.
@param robot The robot performing the point
@param transform The location of end of the sweeping motion
@param manip The manipulator to sweep.
"""
(manip, manip_idx) = GetManipulatorIndex(robot, manip)
#TSR for the goal
ee_offset = 0.15
obj_position = transform
start_position = manip.GetEndEffectorTransform()
end_position = manip.GetEndEffectorTransform()
end_position[0, 3] = obj_position[0, 3] - ee_offset
end_position[1, 3] = obj_position[1, 3]
Bw = numpy.zeros((6, 2))
epsilon = 0.05
Bw[0, :] = [-epsilon, epsilon]
Bw[1, :] = [-epsilon, epsilon]
Bw[2, :] = [-epsilon, epsilon]
Bw[4, :] = [-epsilon, epsilon]
tsr_goal = TSR(T0_w=end_position,
Tw_e=numpy.eye(4),
Bw=Bw,
manip=manip_idx)
goal_tsr_chain = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSRs=[tsr_goal])
goal_in_start = numpy.dot(numpy.linalg.inv(start_position), end_position)
#TSR that constrains the movement
Bw_constrain = numpy.zeros((6, 2))
Bw_constrain[:, 0] = -epsilon
Bw_constrain[:, 1] = epsilon
if goal_in_start[0, 3] < 0:
Bw_constrain[0, :] = [-epsilon + goal_in_start[0, 3], epsilon]
else:
Bw_constrain[0, :] = [-epsilon, epsilon + goal_in_start[0, 3]]
if goal_in_start[1, 3] < 0:
Bw_constrain[1, :] = [-epsilon + goal_in_start[1, 3], epsilon]
else:
Bw_constrain[1, :] = [-epsilon, epsilon + goal_in_start[1, 3]]
if goal_in_start[2, 3] < 0:
Bw_constrain[2, :] = [-epsilon + goal_in_start[2, 3], epsilon]
else:
Bw_constrain[2, :] = [-epsilon, epsilon + goal_in_start[2, 3]]
tsr_constraint = TSR(T0_w=start_position,
Tw_e=numpy.eye(4),
Bw=Bw_constrain,
manip=manip_idx)
movement_chain = TSRChain(sample_start=False,
sample_goal=False,
constrain=True,
TSRs=[tsr_constraint])
return [goal_tsr_chain, movement_chain]
def lift_obj(robot,
transform=numpy.eye(4),
manip=None,
distance=0.1,
epsilon=0.05):
"""
This creates a TSR for lifting an object a specified distance.
It is assumed that when called, the robot is grasping the object.
This assumes that the object can be lifted with one arm.
@param robot The robot to perform the lift
@param transform The transform of the object to lift
@param manip The manipulator to lift
@param distance The distance to lift the bottle
"""
if manip is None:
manip = robot.GetActiveManipulator()
manip_idx = robot.GetActiveManipulatorIndex()
else:
manip.SetActive()
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
#TSR for the goal
start_position = manip.GetEndEffectorTransform()
end_position = manip.GetEndEffectorTransform()
end_position[2, 3] += distance
Bw = numpy.zeros((6, 2))
Bw[0, :] = [-epsilon, epsilon]
Bw[1, :] = [-epsilon, epsilon]
Bw[4, :] = [-epsilon, epsilon]
tsr_goal = TSR(T0_w=end_position,
Tw_e=numpy.eye(4),
Bw=Bw,
manip=manip_idx)
goal_tsr_chain = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSRs=[tsr_goal])
#TSR that constrains the movement
Bw_constrain = numpy.zeros((6, 2))
Bw_constrain[:, 0] = -epsilon
Bw_constrain[:, 1] = epsilon
if distance < 0:
Bw_constrain[1, :] = [-epsilon + distance, epsilon]
else:
Bw_constrain[1, :] = [-epsilon, epsilon + distance]
tsr_constraint = TSR(T0_w=start_position,
Tw_e=numpy.eye(4),
Bw=Bw_constrain,
manip=manip_idx)
movement_chain = TSRChain(sample_start=False,
sample_goal=False,
constrain=True,
TSRs=[tsr_constraint])
return [goal_tsr_chain, movement_chain]
def pitcher_pour(robot,
pitcher,
min_tilt=1.4,
max_tilt=1.57,
manip=None,
grasp_transform=None,
pitcher_pose=None,
**kw_args):
'''
@param robot The robot whose active manipulator should be used to pour
@param pitcher The pitcher to pour
@param min_tilt The minimum amount to tilt the pitcher in the pouring motion
@param max_tilt The maximum amount to tilt the pitcher in the pouring motion
@param grasp_transform The pose of the end-effector in world frame when grasping the pitcher,
if this parameter is not provided, it is assumed the robot is currently
grasping the object and the current end-effector transform is used
'''
if manip is None:
manip = robot.GetActiveManipulator()
manip_idx = robot.GetActiveManipulatorIndex()
else:
manip.SetActive()
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
if pitcher_pose is None:
pitcher_pose = pitcher.GetTransform()
pitcher_in_world = pitcher.GetTransform()
# spout in pitcher
spout_in_pitcher = numpy.array([[-0.7956, 0.6057, 0., 0.],
[-0.6057, -0.7956, 0., 0.],
[0., 0., 1., 0.0599], [0., 0., 0., 1.]])
spout_offset = 0.113
spout_in_pitcher[:2, 3] = spout_offset * spout_in_pitcher[:2, 0]
# end-effector relative to spout
ee_in_world = manip.GetEndEffectorTransform()
ee_in_pitcher = numpy.dot(numpy.linalg.inv(pitcher_in_world), ee_in_world)
ee_in_spout = numpy.dot(numpy.linalg.inv(spout_in_pitcher), ee_in_pitcher)
Bw_pour = numpy.zeros((6, 2))
Bw_pour[4, :] = [0, 2 * numpy.pi]
Bw_pour[3, :] = [-10 * numpy.pi / 180, 10 * numpy.pi / 180]
tsr_0 = TSR(T0_w=pitcher_pose,
Tw_e=spout_in_pitcher,
Bw=numpy.zeros((6, 2)),
manip=manip_idx)
tsr_1_constraint = TSR(Tw_e=ee_in_spout, Bw=Bw_pour, manip=manip_idx)
pour_chain = TSRChain(sample_start=False,
sample_goal=False,
constrain=True,
TSRs=[tsr_0, tsr_1_constraint])
Bw_goal = numpy.zeros((6, 2))
Bw_goal[4, :] = [min_tilt, max_tilt]
tsr_1_goal = TSR(Tw_e=ee_in_spout, Bw=Bw_goal, manip=manip_idx)
pour_goal = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSRs=[tsr_0, tsr_1_goal])
return [pour_chain, pour_goal], min_tilt, max_tilt
