def __init__(self,
hand,
obj,
dof_values=None,
dof_indices=None,
name=None,
precondition=None,
postcondition=None):
"""
@param hand: hand to grab the object with
@param obj: object to grab
@param dof_values: configuration to move the hand to before the grab
if None, defaults to a fully closed configuration
@param dof_indices: indices of the dofs specified in dof_values
if None defaults to only finger dofs
@param name: name of the action
@param precondition: Validator that validates preconditions
@param postcondition: Validator that validates postconditions
"""
if dof_values is None:
dof_values = self.BARRETT_CLOSE_CONFIGURATION
if dof_indices is None:
dof_indices = hand.GetIndices()[self.BARRETT_FINGER_INDICES]
super(GrabObjectAction, self).__init__(hand,
dof_values=dof_values,
dof_indices=dof_indices,
name=name,
precondition=precondition,
postcondition=postcondition)
self._manipulator = to_key(hand.manipulator)
self._obj = to_key(obj)
def __init__(self,
manipulator,
obj,
push_distance=0.1,
required=True,
name=None,
args=None,
kwargs=None):
"""
@param manipulator: OpenRAVE manipulator to use to push the object
@param obj: object being pushed
@param push_distance: distance to push the object
@param required: if True, and a plan cannot be found, raise an
ActionError. If False, and a plan cannot be found, return an empty
solution so execution can proceed normally.
@param name: name of the action
@param args: additional arguments to pass to PlanToEndEffectorOffset
@param kwargs: additional keyword arguments to pass to
PlanToEndEffectorOffset
"""
super(PushObjectAction, self).__init__(name=name)
self._manipulator = to_key(manipulator)
self._object = to_key(obj)
self.push_distance = push_distance
self.required = required
self.args = args if args is not None else list()
self.kwargs = kwargs if kwargs is not None else dict()
def __init__(self,
robot,
active_indices,
active_manipulator,
method,
args=None,
kwargs=None,
planner=None,
name=None,
checkpoint=False):
"""
@param robot: robot
@param active_indices: indices of the robot that should be active during
planning
@param active_manipulator: manipulator of the robot that should be
active during planning
@param method: planning method to call (e.g. 'PlanToConfiguration')
@param args: arguments to pass to the planning method
@param kwargs: keyword arguments to pass to the planning method
@param planner: specific planner to look up the planning method from
if None, robot.planner is used
@param name: name of the action
@param checkpoint: True if this action is a checkpoint
"""
super(PlanAction, self).__init__(name=name, checkpoint=checkpoint)
self._robot = to_key(robot)
self.active_indices = active_indices
self._active_manipulator = to_key(active_manipulator)
self.method = method
# TODO: Some entries in here need to be wrapped with to_key.
self.args = args if args is not None else list()
self.kwargs = kwargs if kwargs is not None else dict()
self.planner = planner
def __init__(self, hand, obj, dof_values, dof_indices, name=None):
"""
@param hand: hand to release the object with
@param obj: object to release
@param dof_values: configuration to move the hand to before releasing
@param dof_indices: indices of the dofs specified in dof_values
@param name: name of the action
"""
super(ReleaseObjectAction, self).__init__(hand,
dof_values=dof_values,
dof_indices=dof_indices,
name=name)
self._obj = to_key(obj)
self._manipulator = to_key(hand.manipulator)
def __init__(self, obj, xyz_offset, robot=None, name=None, **kwargs):
"""
@param obj: object to teleport
@param xyz: relative xyz offset (3x1 vector)
@param robot: OpenRAVE robot
if not None, have the robot grab the object after teleporting
@param name: name of the action
"""
super(TeleportAction, self).__init__(name=name)
if robot is None:
self._robot = None
else:
self._robot = to_key(robot)
self._obj = to_key(obj)
self.xyz_offset = xyz_offset
def __init__(self, action, objs, dof_values, dof_indices):
"""
@param action: Action that generated this Solution
@param objs: list of objects that should be released
@param dof_values: configuration to move the hand to before releasing
@param dof_indices: indices of the dofs specified in dof_values
"""
super(ReleaseObjectsSolution, self).__init__(action, dof_values,
dof_indices)
self._objs = [to_key(o) for o in objs]
def __init__(self,
robot,
obj,
on_obj,
active_indices,
active_manipulator,
height=0.04,
allowed_tilt=0.0,
args=None,
kwargs=None,
planner=None,
name=None):
"""
@param robot: robot
@param obj: object to place
@param on_obj: object 'obj' should be placed on
@param active_indices: indices of the robot that should be active during
planning
@param active_manipulator: manipulator of the robot that should be
active during planning
@param height: height relative to on_obj to place the object (meters)
@param allowed_tilt: amount of allowed tilt in the object when placing
it (radians)
@param args: extra arguments to pass to the planner
@param kwargs: extra keyword arguments to pass to the planner
@param planner: specific planner to use
if None, robot.planner is used
@param name: name of the action
"""
super(PlaceObjectAction, self).__init__(robot,
active_indices,
active_manipulator,
method='PlanToTSR',
args=args,
kwargs=kwargs,
planner=planner,
name=name)
self._obj = to_key(obj)
self._on_obj = to_key(on_obj)
self.orig_args = args if args is not None else list()
self.height = height
self.allowed_tilt = allowed_tilt
def __init__(self,
manipulator,
obj,
on_obj,
goal_pose,
goal_epsilon=0.02,
movables=None,
required=True,
name=None,
kwargs=None):
"""
@param manipulator: OpenRAVE manipulator to use to push the object
@param obj: object being pushed
@param on_obj: support surface to push the object along
@param goal_pose: [x,y] final pose of the object
@param goal_epsilon: maximum distance away from pose the object can be
at the end of the trajectory (meters)
@param movables: other objects in the environment that can be moved
while attempting to achieve the goal
@param required: if True, and a plan cannot be found, raise an
ActionError. If False, and a plan cannot be found, return an empty
solution so execution can proceed normally.
@param name: name of the action
@param kwargs: additional keyword arguments to be passed to the planner
generating the pushing trajectory
"""
super(RearrangeObjectAction, self).__init__(name=name)
self._manipulator = to_key(manipulator)
self._object = to_key(obj)
self._on_object = to_key(on_obj)
if movables is None:
movables = []
self._movables = [to_key(m) for m in movables]
self.goal_pose = goal_pose
self.goal_epsilon = goal_epsilon
self.required = required
self.kwargs = kwargs if kwargs is not None else dict()
self.ompl_path = None
self.planner_params = None
def __init__(self,
manipulator,
direction,
min_distance,
max_distance,
target_bodies,
force_magnitude=5.,
torque=None,
planner=None,
name=None):
"""
@param manipulator: manipulator to move
@param direction: 3 dim vector - [x,y,z] in world coordinates
@param min_distance: min distance to move
@param max_distance: max distance to move
@param target_bodies: any bodies that should be disabled during planning
@param force_magnitude: max allowable magnitude of force before
considering the manipulator to be touching something
@param torque: the max allowable torque before considering the
manipulator to be touching something
@param planner: planner to use to generate the trajectory
if None, defaults to robot.planner
@param name: name of the action
"""
super(MoveUntilTouchAction, self).__init__(name=name)
self._manipulator = to_key(manipulator)
self._target_bodies = [to_key(body) for body in target_bodies]
self.direction = np.array(direction, dtype='float')
self.min_distance = float(min_distance)
self.max_distance = float(max_distance)
self.force_magnitude = float(force_magnitude)
self.planner = planner
if torque is not None:
self.torque = np.array(torque, dtype='float')
else:
self.torque = self.TORQUE_MAGNITUDE
def save_env(env):
"""
Crude implementation of serializing an environment.
@param env: OpenRAVE environment
@return a dictionary serialization of the environment
"""
saved_env = {}
for obj in env.GetBodies():
obj_key = to_key(obj)
saved_env[obj_key] = {'pose': obj.GetTransform()}
if isinstance(obj, openravepy.Robot):
saved_env[obj_key]['dof_values'] = obj.GetDOFValues()
return saved_env
def __init__(self,
action,
obj,
dof_values,
dof_indices,
precondition=None,
postcondition=None):
"""
@param action: Action that generated this Solution
@param obj: object to grab
@param dof_values: configuration to move the hand to before the grab
@param dof_indices: indices of the dofs specified in dof_values
@param precondition: Validator that validates preconditions
@param postcondition: Validator that validates postconditions
"""
super(GrabObjectSolution,
self).__init__(action, dof_values, dof_indices, precondition,
postcondition)
self._obj = to_key(obj)
def __init__(self,
objects,
wrapped_action,
padding_only=False,
name=None,
checkpoint=False):
"""
@param objects: list of KinBodies to disable
@param wrapped_action: Action to call while objects are disabled
@param padding_only: if True, only disable padding on the objects
@param name: name of the action
@param checkpoint: True if the action is a checkpoint
"""
super(DisableAction,
self).__init__(name=name,
precondition=wrapped_action.precondition,
postcondition=wrapped_action.postcondition,
checkpoint=checkpoint)
self.action = wrapped_action
self._objects = [to_key(o) for o in objects]
self.padding_only = padding_only
def __init__(self, infile, robot, dof_indices, name='GraspValidator'):
"""
@param infile: csvfile containing grasp data used to build the
classifier
@param robot: OpenRAVE robot to validate
@param dof_indices: dof indices to validate
@param name: name of the validator
"""
super(GraspValidator, self).__init__(name=name)
data = []
# Read in the csv file
with open(infile, 'r') as f:
reader = csv.reader(f, delimiter=' ')
for row in reader:
data.append(row)
# Chop off the header
data = data[1:]
# Convert the classification bit to binary
for row in data:
if row[-1] == 'y':
row[-1] = 1
else:
row[-1] = 0
# Convert everything else to float
row[:-1] = [float(v) for v in row[:-1]]
# Now build the classifier
self.data = np.array(data)
self.X = self.data[:, :-2]
self.y = self.data[:, -1]
self.clf = svm.SVC()
self.clf.fit(self.X, self.y)
self._robot = to_key(robot)
self.dof_indices = dof_indices
def __init__(self,
robot,
obj,
tsr_name,
active_indices,
active_manipulator,
tsr_args=None,
args=None,
kwargs=None,
planner=None,
name=None):
"""
@param robot: robot
@param obj: object the TSR is defined on
@param tsr_name: name of the TSR
@param active_indices: indices of the robot that should be active during
planning
@param active_manipulator: manipulator of the robot that should be
active during planning
@param tsr_args: extra arguments to pass to the TSR library for
computing the TSR
@param args: arguments to pass to the planning method
@param kwargs: keyword arguments to pass to the planning method
@param planner: specific planner to look up the planning method from
if None, robot.planner is used
@param name: name of the action
"""
super(PlanToTSRAction, self).__init__(robot,
active_indices,
active_manipulator,
method='PlanToTSR',
args=args,
kwargs=kwargs,
planner=planner,
name=name)
self._obj = to_key(obj)
self.tsr_name = tsr_name
self.orig_args = args if args is not None else list()
self.tsr_args = tsr_args if tsr_args is not None else dict()
def __init__(self,
hand,
dof_values,
dof_indices,
name=None,
precondition=None,
postcondition=None):
"""
Move hand to a desired configuration
@param hand: EndEffector to move
@param dof_values: list of dof values to move the hand to
@param dof_indices: indices of the dofs specified in dof_values
@param name: name of the action
@param precondition: Validator that validates preconditions
@param postcondition: Validator that validates postconditions
"""
super(MoveHandAction, self).__init__(name=name,
precondition=precondition,
postcondition=postcondition)
self._hand = to_key(hand)
self.dof_values = np.array(dof_values, dtype='float')
self.dof_indices = np.array(dof_indices, dtype='int')
