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 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 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 cap_palm_push(cap, **kw_args):
"""
@param cap The cap to grasp
@param push_distance The distance to push before grasping
"""
epsilon = 0.005
T0_w = cap.get_transform()
chain_list = []
# Base of cap (opposite side of head)
Tw_e_front1 = numpy.array([
[0., 0., -1., 0.04],
[0., 1., 0., 0],
[1., 0., 0., 0.027], # Downward
[0., 0., 0., 1.]
])
Bw_yz = numpy.zeros((6, 2))
Bw_yz[1, :] = [-epsilon, epsilon]
Bw_yz[2, :] = [-epsilon, epsilon]
Bw_yz[5, :] = [-math.pi, math.pi]
front_tsr1 = TSR(T0_w=T0_w, Tw_e=Tw_e_front1, Bw=Bw_yz)
grasp_chain_front1 = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=front_tsr1)
chain_list += [grasp_chain_front1] # AROUND
# Each chain in the list can also be rotated by 180 degrees around z
rotated_chain_list = []
for c in chain_list:
rval = numpy.pi
R = numpy.array([[numpy.cos(rval), -numpy.sin(rval), 0., 0.],
[numpy.sin(rval),
numpy.cos(rval), 0., 0.], [0., 0., 1., 0.],
[0., 0., 0., 1.]])
tsr = c.TSRs[0]
Tw_e = tsr.Tw_e
Tw_e_new = numpy.dot(Tw_e, R)
tsr_new = TSR(T0_w=tsr.T0_w, Tw_e=Tw_e_new, Bw=tsr.Bw)
tsr_chain_new = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=tsr_new)
rotated_chain_list += [tsr_chain_new]
return chain_list + rotated_chain_list
def CreateTSRChains(cls, robot, direction, distance):
direction = numpy.array(direction, dtype=float)
if direction.shape != (3, ):
raise ValueError('Direction must be a three-dimensional vector.')
if not (distance >= 0):
raise ValueError(
'Distance must be non-negative; got {:f}.'.format(distance))
manip, manip_index = GetManipulatorIndex(robot)
H_world_ee = manip.GetEndEffectorTransform()
direction = direction / numpy.linalg.norm(direction)
# 'object frame w' is at ee, z pointed along direction to move
H_world_w = H_from_op_diff(H_world_ee[0:3, 3], direction)
H_w_ee = numpy.dot(invert_H(H_world_w), H_world_ee)
Hw_end = numpy.eye(4)
Hw_end[2, 3] = distance
goal_chain = TSRChain(sample_goal=True,
TSR=TSR(T0_w=numpy.dot(H_world_w, Hw_end),
Tw_e=H_w_ee,
Bw=numpy.zeros((6, 2)),
manipindex=manip_index))
constraint_chain = TSRChain(
constrain=True,
TSR=TSR(T0_w=H_world_w,
Tw_e=H_w_ee,
Bw=numpy.array([[-cls.epsilon, cls.epsilon],
[-cls.epsilon, cls.epsilon],
[min(0.0, distance),
max(0.0, distance)],
[-cls.epsilon, cls.epsilon],
[-cls.epsilon, cls.epsilon],
[-cls.epsilon, cls.epsilon]]),
manipindex=manip_index))
return [goal_chain, constraint_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=pills.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_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)
# move along x, y directions to get any point on tray
Bw[0, :] = [-xdim, xdim]
Bw[1, :] = [-ydim, ydim]
Bw[2, :] = [-0.02, 0.04] # verticle movement
# allow any rotation around z - which is the axis normal to the tray top
Bw[5, :] = [-numpy.pi, numpy.pi]
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 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))
# move along x and z directios to get any point on table
Bw[0, :] = [-0.93 + padding, 0.93 - padding]
Bw[2, :] = [-0.38 + padding, 0.38 - padding]
# allow any rotation around y - which is the axis normal to the table top
Bw[4, :] = [-numpy.pi, numpy.pi]
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 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 block_on_surface(robot, block, pose_tsr_chain, manip=None):
'''
Generates end-effector poses for placing the block on a surface.
This factory assumes the block is grasped at the time it is called.
@param robot The robot grasping the block
@param block The grasped object
@param pose_tsr_chain The tsr chain for sampling placement
poses for the block
@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()
block_pose = block.GetTransform()
block_pose[:3, :3] = numpy.eye(3) # ignore orientation
ee_in_block = numpy.dot(numpy.linalg.inv(block_pose),
manip.GetEndEffectorTransform())
Bw = numpy.zeros((6, 2))
Bw[2, :] = [0., 0.04] # Allow some vertical movement
for tsr in pose_tsr_chain.TSRs:
if tsr.manipindex != manip_idx:
raise ValueError(
'pose_tsr_chain defined for a different manipulator.')
grasp_tsr = TSR(Tw_e=ee_in_block, 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 block_at_pose(robot, block, position, manip=None):
'''
Generates end-effector poses for placing the block on another object
@param robot The robot grasping the block
@param block The block being grasped
@param position The position to place the block [x,y,z]
@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()
# Block on the object
T0_w = numpy.eye(4)
T0_w[:3, 3] = position
Bw = numpy.zeros((6, 2))
Bw[5, :] = [-numpy.pi, numpy.pi]
place_tsr = TSR(T0_w=T0_w, Tw_e=numpy.eye(4), Bw=Bw, manip=manip_idx)
ee_in_block = numpy.dot(numpy.linalg.inv(block.GetTransform()),
manip.GetEndEffectorTransform())
ee_tsr = TSR(
T0_w=numpy.eye(4), #ignored
Tw_e=ee_in_block,
Bw=numpy.zeros((6, 2)),
manip=manip_idx)
place_tsr_chain = TSRChain(sample_start=False,
sample_goal=True,
TSRs=[place_tsr, ee_tsr])
return [place_tsr_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 block_grasp(robot, block, manip=None, **kw_args):
"""
Generates end-effector poses for moving the arm near the block
@param robot The robot grasping the block
@param block The block being grasped
@param manip The manipulator to move near the block, 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()
offset = 0.01 #vertical offset relative to block
alpha = 0.8 # orientation of end-effector relative to block
block_in_world = block.GetTransform()
block_in_world[:3, :3] = numpy.eye(3) # ignore orientation
ee_in_block = numpy.array([[
numpy.cos(alpha), 0., -numpy.sin(alpha), 0.3 * numpy.sin(alpha) + 0.04
], [0., -1, 0,
0.], [-numpy.sin(alpha), 0., -numpy.cos(alpha), 0.25 + offset],
[0., 0., 0., 1.]])
Bw = numpy.zeros((6, 2))
Bw[5, :] = [-numpy.pi, numpy.pi]
pose_tsr = TSR(T0_w=block_in_world,
Tw_e=ee_in_block,
Bw=Bw,
manip=manip_idx)
pose_tsr_chain = TSRChain(sample_start=False,
sample_goal=True,
TSRs=[pose_tsr])
return [pose_tsr_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
def cylinder_handle_grasp(tool, push_distance=0.0,
width_offset=0.0,
**kw_args):
"""
@param tool The tool to grasp
@param push_distance The distance to push before grasping
"""
ee_to_palm_distance = 0.098
lateral_offset=ee_to_palm_distance + push_distance
T0_w = tool.get_transform()
chain_list = []
# Base of tool
Tw_e_head = numpy.array([[ 0., 0., -1., lateral_offset],
[ 0., 1., 0., 0.0],
[ 1., 0., 0., 0.0],
[ 0., 0., 0., 1.]])
Bw_yz = numpy.zeros((6,2))
Bw_yz[0, :] = [-0.03, 0]
Bw_yz[1, :] = [-width_offset, width_offset]
Bw_yz[2, :] = [-width_offset, width_offset]
Bw_yz[3, :] = [-math.pi, math.pi]
Bw_yz[4, :] = [-math.pi/2.0, math.pi/2.0]
head_tsr = TSR(T0_w = T0_w, Tw_e = Tw_e_head, Bw = Bw_yz)
grasp_chain_head = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=head_tsr)
chain_list += [ grasp_chain_head ]
# Sides
Tw_e_side = numpy.array([[ 1., 0., 0., 0.0],
[ 0.,-1., 0., 0.0],
[ 0., 0., -1., lateral_offset],
[ 0., 0., 0., 1.]])
Bw_side = numpy.zeros((6,2))
Bw_side[0, :] = [-0.08, 0.0]
Bw_side[1, :] = [-width_offset, width_offset]
Bw_side[3, :] = [-math.pi, math.pi]
Bw_side[4,:] = [-math.pi/4.0, math.pi/4.0]
side_tsr = TSR(T0_w = T0_w, Tw_e = Tw_e_side, Bw = Bw_side)
grasp_chain_side = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=side_tsr)
chain_list += [ grasp_chain_side ]
# Each chain in the list can also be rotated by 180 degrees around z
rotated_chain_list = []
for c in chain_list:
rval = numpy.pi
R = numpy.array([[numpy.cos(rval), -numpy.sin(rval), 0., 0.],
[numpy.sin(rval), numpy.cos(rval), 0., 0.],
[ 0., 0., 1., 0.],
[ 0., 0., 0., 1.]])
tsr = c.TSRs[0]
Tw_e = tsr.Tw_e
Tw_e_new = numpy.dot(Tw_e, R)
tsr_new = TSR(T0_w = tsr.T0_w, Tw_e=Tw_e_new, Bw=tsr.Bw)
tsr_chain_new = TSRChain(sample_start=False, sample_goal=True, constrain=False,
TSR=tsr_new)
rotated_chain_list += [ tsr_chain_new ]
return chain_list + rotated_chain_list
def handle_grasp(tool, push_distance=0.0,
width_offset=0.009, #0.008,
toolLength=0.08,
**kw_args):
"""
@param tool The tool to grasp
@param push_distance The distance to push before grasping
"""
ee_to_palm_distance = 0.1 #0.098
lateral_offset=ee_to_palm_distance + push_distance
T0_w = tool.get_transform()
chain_list = []
# Base of tool - horizontal (opposite side of head)
Tw_e_horiz = numpy.array([[ 0., 0., -1., lateral_offset],
[ 0., 1., 0., 0.0],
[ 1., 0., 0., 0.0],
[ 0., 0., 0., 1.]])
Bw_h = numpy.zeros((6,2))
Bw_h[0, :] = [-0.03, 0]
Bw_h[1, :] = [-width_offset, width_offset] #XXX
Bw_h[2, :] = [-width_offset, width_offset] #XXX
#Bw_h[4, :] = [-math.pi/2.0, math.pi/2.0]
horiz_tsr = TSR(T0_w=T0_w, Tw_e=Tw_e_horiz, Bw=Bw_h)
grasp_chain_horiz = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=horiz_tsr)
chain_list += [ grasp_chain_horiz ] #XXX
# Base of tool - vertical (opposite side of head)
Tw_e_vert = numpy.array([[ 0., 0., -1., lateral_offset],
[ 1., 0., 0., 0.0],
[ 0., -1., 0., 0.0],
[ 0., 0., 0., 1.]])
Bw_v = numpy.zeros((6,2))
Bw_v[0, :] = [-0.03, 0]
Bw_v[1, :] = [-width_offset, width_offset] #XXX
Bw_v[2, :] = [-width_offset, width_offset] #XXX
#Bw_v[5, :] = [-math.pi/2.0, math.pi/2.0]
vert_tsr = TSR(T0_w=T0_w, Tw_e=Tw_e_vert, Bw = Bw_v)
grasp_chain_vert = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=vert_tsr)
chain_list += [ grasp_chain_vert ] #XXX
# Top and Bottom sides
Tw_e_side1 = numpy.array([[ 1., 0., 0., 0.0],
[ 0.,-1., 0., 0.0],
[ 0., 0., -1., lateral_offset],
[ 0., 0., 0., 1.]])
Tw_e_side2 = numpy.array([[ 1., 0., 0., 0.0],
[ 0., 1., 0., 0.0],
[ 0., 0., 1., -lateral_offset],
[ 0., 0., 0., 1.]])
Bw_side = numpy.zeros((6,2))
Bw_side[0, :] = [-toolLength, 0.0]
Bw_side[2, :] = [-width_offset, width_offset] #XXX
#Bw_side[4,:] = [-math.pi/4.0, math.pi/4.0]
side_tsr1 = TSR(T0_w = T0_w, Tw_e = Tw_e_side1, Bw = Bw_side)
grasp_chain_side1 = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=side_tsr1)
chain_list += [ grasp_chain_side1 ]
side_tsr2 = TSR(T0_w = T0_w, Tw_e = Tw_e_side2, Bw = Bw_side)
grasp_chain_side2 = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=side_tsr2)
chain_list += [ grasp_chain_side2 ]
# Two side faces
Tw_e_bottom1 = numpy.array([[ 1., 0., 0., 0.],
[ 0., 0., -1., lateral_offset],
[ 0., 1., 0., 0.0],
[ 0., 0., 0., 1.]])
Tw_e_bottom2 = numpy.array([[ 1., 0., 0., 0.],
[ 0., 0., 1., -lateral_offset],
[ 0., -1., 0., 0.0],
[ 0., 0., 0., 1.]])
Bw_topbottom = numpy.zeros((6,2))
Bw_topbottom[0, :] = [-toolLength, 0.0]
Bw_topbottom[2, :] = [-width_offset, width_offset] #XXX
#Bw_topbottom[5, :] = [-math.pi/4.0, math.pi/4.0]
bottom_tsr1 = TSR(T0_w = T0_w, Tw_e = Tw_e_bottom1, Bw = Bw_topbottom)
grasp_chain_bottom1 = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=bottom_tsr1)
chain_list += [ grasp_chain_bottom1 ]
bottom_tsr2 = TSR(T0_w = T0_w, Tw_e = Tw_e_bottom2, Bw = Bw_topbottom)
grasp_chain_bottom2 = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=bottom_tsr2)
chain_list += [ grasp_chain_bottom2 ]
# Each chain in the list can also be rotated by 180 degrees around z
rotated_chain_list = []
for c in chain_list:
rval = numpy.pi
R = numpy.array([[numpy.cos(rval), -numpy.sin(rval), 0., 0.],
[numpy.sin(rval), numpy.cos(rval), 0., 0.],
[ 0., 0., 1., 0.],
[ 0., 0., 0., 1.]])
tsr = c.TSRs[0]
Tw_e = tsr.Tw_e
Tw_e_new = numpy.dot(Tw_e, R)
tsr_new = TSR(T0_w = tsr.T0_w, Tw_e=Tw_e_new, Bw=tsr.Bw)
tsr_chain_new = TSRChain(sample_start=False, sample_goal=True, constrain=False,
TSR=tsr_new)
rotated_chain_list += [ tsr_chain_new ]
return chain_list + rotated_chain_list
def handle_grasp(tool, push_distance=0.0, width_offset=0.0, **kw_args):
"""
@param tool The tool to grasp
@param push_distance The distance to push before grasping
"""
ee_to_palm_distance = 0.1175
lateral_offset = ee_to_palm_distance + push_distance
T0_w = tool.get_transform()
chain_list = []
# Base of tool (opposite side of head)
Tw_e_front1 = numpy.array([[0., 0., -1., lateral_offset],
[0., 1., 0., 0.0], [1., 0., 0., 0.0],
[0., 0., 0., 1.]])
Bw_yz = numpy.zeros((6, 2))
Bw_yz[1, :] = [-width_offset, width_offset]
Bw_yz[2, :] = [-width_offset, width_offset]
front_tsr1 = TSR(T0_w=T0_w, Tw_e=Tw_e_front1, Bw=Bw_yz)
grasp_chain_front = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=front_tsr1)
chain_list += [grasp_chain_front]
# Top and Bottom sides
Tw_e_side1 = numpy.array([[0., 1., 0., 0.0], [1., 0., 0., 0.0],
[0., 0., -1., lateral_offset], [0., 0., 0., 1.]])
Tw_e_side2 = numpy.array([[0., 1., 0., 0.0], [1., 0., 0., 0.02],
[0., 0., 1., -lateral_offset], [0., 0., 0., 1.]])
Bw_side = numpy.zeros((6, 2))
Bw_side[0, :] = [-0.08, 0.0]
Bw_side[1, :] = [-width_offset, width_offset]
side_tsr1 = TSR(T0_w=T0_w, Tw_e=Tw_e_side1, Bw=Bw_side)
grasp_chain_side1 = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=side_tsr1)
chain_list += [grasp_chain_side1]
side_tsr2 = TSR(T0_w=T0_w, Tw_e=Tw_e_side2, Bw=Bw_side)
grasp_chain_side2 = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=side_tsr2)
chain_list += [grasp_chain_side2]
# Two side faces
Tw_e_bottom1 = numpy.array([[0., -1., 0., 0.],
[0., 0., -1., lateral_offset],
[-1., 0., 0., 0.0], [0., 0., 0., 1.]])
Tw_e_bottom2 = numpy.array([[0., -1., 0., 0.],
[0., 0., 1., -lateral_offset],
[-1., 0., 0., 0.0], [0., 0., 0., 1.]])
Bw_topbottom = numpy.zeros((6, 2))
Bw_topbottom[0, :] = [-0.08, 0.0]
Bw_topbottom[2, :] = [-width_offset, width_offset]
bottom_tsr1 = TSR(T0_w=T0_w, Tw_e=Tw_e_bottom1, Bw=Bw_topbottom)
grasp_chain_bottom1 = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=bottom_tsr1)
chain_list += [grasp_chain_bottom1]
bottom_tsr2 = TSR(T0_w=T0_w, Tw_e=Tw_e_bottom2, Bw=Bw_topbottom)
grasp_chain_bottom2 = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=bottom_tsr2)
chain_list += [grasp_chain_bottom2]
# Each chain in the list can also be rotated by 180 degrees around z
rotated_chain_list = []
for c in chain_list:
rval = numpy.pi
R = numpy.array([[numpy.cos(rval), -numpy.sin(rval), 0., 0.],
[numpy.sin(rval),
numpy.cos(rval), 0., 0.], [0., 0., 1., 0.],
[0., 0., 0., 1.]])
tsr = c.TSRs[0]
Tw_e = tsr.Tw_e
Tw_e_new = numpy.dot(Tw_e, R)
tsr_new = TSR(T0_w=tsr.T0_w, Tw_e=Tw_e_new, Bw=tsr.Bw)
tsr_chain_new = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=tsr_new)
rotated_chain_list += [tsr_chain_new]
return chain_list + rotated_chain_list
def bar_grasp(box, push_distance=0.0,
width_offset=0.0,
**kw_args):
"""
@param box The box to grasp
@param push_distance The distance to push before grasping
"""
ee_to_palm_distance = 0.098
lateral_offset=ee_to_palm_distance + push_distance
epsilon = 0.01
T0_w = box.get_transform()
chain_list = []
# Base of box (opposite side of head)
Tw_e_front1 = numpy.array([[ 0., 0., -1., lateral_offset],
[-1., 0., 0., 0.0],
[ 0., 1., 0., 0.0],
[ 0., 0., 0., 1.]])
Tw_e_front2 = numpy.array([[ 0., 0., 1., -lateral_offset],
[ 1., 0., 0., 0.0],
[ 0., 1., 0., 0.0],
[ 0., 0., 0., 1.]])
Bw_yz = numpy.zeros((6,2))
Bw_yz[0, :] = [-epsilon, epsilon]
Bw_yz[1, :] = [-0.2, 0.2]
front_tsr1 = TSR(T0_w = T0_w, Tw_e = Tw_e_front1, Bw = Bw_yz)
grasp_chain_front1 = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=front_tsr1)
chain_list += [ grasp_chain_front1 ]
front_tsr2 = TSR(T0_w = T0_w, Tw_e = Tw_e_front2, Bw = Bw_yz)
grasp_chain_front2 = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=front_tsr2)
chain_list += [ grasp_chain_front2 ]
# Top and Bottom sides
Tw_e_side3 = numpy.array([[ 0., 1., 0., 0.0],
[ 1., 0., 0., 0.0],
[ 0., 0., -1., lateral_offset],
[ 0., 0., 0., 1.]])
Tw_e_side4 = numpy.array([[ 0., 1., 0., 0.0],
[-1., 0., 0., 0.0],
[ 0., 0., 1., -lateral_offset-0.05],
[ 0., 0., 0., 1.]])
Bw_side = numpy.zeros((6,2))
Bw_side[0, :] = [-epsilon, epsilon]
Bw_side[1, :] = [-0.2, 0.2]
side_tsr3 = TSR(T0_w = T0_w, Tw_e = Tw_e_side3, Bw = Bw_side)
grasp_chain_side3 = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=side_tsr3)
chain_list += [ grasp_chain_side3 ]
side_tsr4 = TSR(T0_w = T0_w, Tw_e = Tw_e_side4, Bw = Bw_side)
grasp_chain_side4 = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=side_tsr4)
chain_list += [ grasp_chain_side4 ]
# Two side faces
Tw_e_bottom1 = numpy.array([[ 0., -1., 0., 0.],
[ 0., 0., -1., lateral_offset+0.19],
[ 1., 0., 0., 0.0],
[ 0., 0., 0., 1.]])
Tw_e_bottom2 = numpy.array([[ 0., 1., 0., 0.],
[ 0., 0., 1., -lateral_offset-0.19],
[ 1., 0., 0., 0.0],
[ 0., 0., 0., 1.]])
Tw_e_bottom3 = numpy.array([[ 1., 0., 0., 0.],
[ 0., 0., 1., -lateral_offset-0.19],
[ 0., -1., 0., 0.0],
[ 0., 0., 0., 1.]])
Tw_e_bottom4 = numpy.array([[ 1., 0., 0., 0.],
[ 0., 0., -1., lateral_offset+0.19],
[ 0., 1., 0., 0.0],
[ 0., 0., 0., 1.]])
Bw_topbottom = numpy.zeros((6,2))
Bw_topbottom[1, :] = [-epsilon, epsilon]
bottom_tsr1 = TSR(T0_w = T0_w, Tw_e = Tw_e_bottom1, Bw = Bw_topbottom)
grasp_chain_bottom1 = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=bottom_tsr1)
chain_list += [ grasp_chain_bottom1 ]
bottom_tsr2 = TSR(T0_w = T0_w, Tw_e = Tw_e_bottom2, Bw = Bw_topbottom)
grasp_chain_bottom2 = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=bottom_tsr2)
chain_list += [ grasp_chain_bottom2 ]
bottom_tsr3 = TSR(T0_w = T0_w, Tw_e = Tw_e_bottom3, Bw = Bw_topbottom)
grasp_chain_bottom3 = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=bottom_tsr3)
chain_list += [ grasp_chain_bottom3 ]
bottom_tsr4 = TSR(T0_w = T0_w, Tw_e = Tw_e_bottom4, Bw = Bw_topbottom)
grasp_chain_bottom4 = TSRChain(sample_start=False, sample_goal=True,
constrain=False, TSR=bottom_tsr4)
chain_list += [ grasp_chain_bottom4 ]
# Each chain in the list can also be rotated by 180 degrees around z
rotated_chain_list = []
for c in chain_list:
rval = numpy.pi
R = numpy.array([[numpy.cos(rval), -numpy.sin(rval), 0., 0.],
[numpy.sin(rval), numpy.cos(rval), 0., 0.],
[ 0., 0., 1., 0.],
[ 0., 0., 0., 1.]])
tsr = c.TSRs[0]
Tw_e = tsr.Tw_e
Tw_e_new = numpy.dot(Tw_e, R)
tsr_new = TSR(T0_w = tsr.T0_w, Tw_e=Tw_e_new, Bw=tsr.Bw)
tsr_chain_new = TSRChain(sample_start=False, sample_goal=True, constrain=False,
TSR=tsr_new)
rotated_chain_list += [ tsr_chain_new ]
return chain_list #+ rotated_chain_list
def bottle_grasp(cap, push_distance=0.0, **kw_args):
"""
@param cap The cap to grasp
@param push_distance The distance to push before grasping
"""
epsilon = 0.005
ee_to_palm_distance = 0.105
lateral_offset = ee_to_palm_distance + push_distance
T0_w = cap.get_transform()
chain_list = []
# Base of cap (opposite side of head)
Tw_e_front1 = numpy.array([[0., 0., -1., lateral_offset],
[0., 1., 0., 0.0], [1., 0., 0., 0.0],
[0., 0., 0., 1.]])
Bw_yz = numpy.zeros((6, 2))
Bw_yz[1, :] = [-epsilon, epsilon]
Bw_yz[2, :] = [-0.05, 0.05]
Bw_yz[5, :] = [-math.pi, math.pi]
front_tsr1 = TSR(T0_w=T0_w, Tw_e=Tw_e_front1, Bw=Bw_yz)
grasp_chain_front1 = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=front_tsr1)
chain_list += [grasp_chain_front1] # AROUND
# Top and Bottom sides
Tw_e_side1 = numpy.array([[1., 0., 0., 0.0], [0., -1., 0., 0.0],
[0., 0., -1., lateral_offset], [0., 0., 0., 1.]])
Tw_e_side2 = numpy.array([[1., 0., 0., 0.0], [0., 1., 0., 0.0],
[0., 0., 1., -lateral_offset], [0., 0., 0., 1.]])
Bw_side = numpy.zeros((6, 2))
Bw_side[0, :] = [-epsilon, epsilon]
Bw_side[1, :] = [-epsilon, epsilon]
Bw_side[5, :] = [-math.pi, math.pi]
side_tsr1 = TSR(T0_w=T0_w, Tw_e=Tw_e_side1, Bw=Bw_side)
grasp_chain_side1 = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=side_tsr1)
chain_list += [grasp_chain_side1] #TOP GRASP
side_tsr2 = TSR(T0_w=T0_w, Tw_e=Tw_e_side2, Bw=Bw_side)
grasp_chain_side2 = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=side_tsr2)
chain_list += [grasp_chain_side2] #BOTTOM GRASP
# Each chain in the list can also be rotated by 180 degrees around z
rotated_chain_list = []
for c in chain_list:
rval = numpy.pi
R = numpy.array([[numpy.cos(rval), -numpy.sin(rval), 0., 0.],
[numpy.sin(rval),
numpy.cos(rval), 0., 0.], [0., 0., 1., 0.],
[0., 0., 0., 1.]])
tsr = c.TSRs[0]
Tw_e = tsr.Tw_e
Tw_e_new = numpy.dot(Tw_e, R)
tsr_new = TSR(T0_w=tsr.T0_w, Tw_e=Tw_e_new, Bw=tsr.Bw)
tsr_chain_new = TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=tsr_new)
rotated_chain_list += [tsr_chain_new]
return chain_list + rotated_chain_list
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.2
obj_position = transform
start_position = manip.GetEndEffectorTransform()
end_position = manip.GetEndEffectorTransform()
end_position[0, 3] = obj_position[0, 3]
end_position[1, 3] = obj_position[1, 3] - ee_offset
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]
