def fn_status_callback(t, q):
"""
Check joint-limits and collisions for a specific joint
configuration. This is called multiple times at DOF
resolution in order to check along the entire length of the
trajectory.
Note: This is called by fn_callback, which is currently
called after each integration time step, which means we are
doing more checks than required.
"""
if time.time() - time_start >= timelimit:
raise TimeLimitError()
# Check joint position limits.
# We do this before setting the joint angles.
util.CheckJointLimits(robot, q)
robot.SetActiveDOFValues(q)
# Check collision.
report = CollisionReport()
if env.CheckCollision(robot, report=report):
raise CollisionPlanningError.FromReport(report)
elif robot.CheckSelfCollision(report=report):
raise SelfCollisionPlanningError.FromReport(report)
# Check the termination condition.
status = fn_terminate()
if Status.DoesCache(status):
nonlocals['t_cache'] = t
if Status.DoesTerminate(status):
raise TerminationError()
def close_finger(finger, offset, tol=.01):
"""Close a finger joint by up to a given amount, checking for collision.
Note that this implementation is slow...
"""
link = finger.GetHierarchyChildLink()
#Make a pose for the robot
pose = _oh.Pose(finger.GetParent())
initial = pose[finger]
env = finger.GetParent().GetEnv()
report = CollisionReport()
step = offset
while abs(step) > tol:
pose[finger] += step
pose.send()
collision = env.CheckCollision(link, report=report)
if collision:
pose[finger] -= step
pose.send()
step /= 2.
elif abs(pose[finger] - initial) >= offset:
#Met or exceeded desired offset without collision
break
return offset
def run(self,auto=False,extra=[]):
if auto:
self.activate(extra)
response=self.problem.SendCommand(self.Serialize())
if len(response)>0:
collisions=CollisionReport()
if self.robot.CheckSelfCollision(collisions):
print "Self-collision between links {} and {}!".format(collisions.plink1,collisions.plink2)
return False
if self.robot.GetEnv().CheckCollision(self.robot,collisions):
print "Environment collision between links {} and {}!".format(collisions.plink1,collisions.plink2)
return False
self.soln=[float(x) for x in response[:-1].split(' ')]
return True
def resample_rcollides(pred, negated, t, plan):
# Variable that needs to added to BoundExpr and latter pass to the planner
JOINT_STEP = 20
STEP_DECREASE_FACTOR = 1.5
ATTEMPT_SIZE = 7
LIN_SAMP_RANGE = 5
attr_inds = OrderedDict()
res = []
robot, rave_body = pred.robot, pred._param_to_body[pred.robot]
body = rave_body.env_body
manip = body.GetManipulator("right_arm")
arm_inds = manip.GetArmIndices()
lb_limit, ub_limit = body.GetDOFLimits()
step_factor = JOINT_STEP
joint_step = (ub_limit[arm_inds] - lb_limit[arm_inds]) / step_factor
original_pose, arm_pose = robot.rArmPose[:, t].copy(
), robot.rArmPose[:, t].copy()
rave_body.set_pose([0, 0, robot.pose[:, t]])
rave_body.set_dof({
"lArmPose": robot.lArmPose[:, t].flatten(),
"lGripper": robot.lGripper[:, t].flatten(),
"rArmPose": robot.rArmPose[:, t].flatten(),
"rGripper": robot.rGripper[:, t].flatten()
})
## Determine the range we should resample
pred_list = [
act_pred['active_timesteps'] for act_pred in plan.actions[0].preds
if act_pred['pred'].spacial_anchor == True
]
start, end = 0, plan.horizon - 1
for action in plan.actions:
if action.active_timesteps[0] <= t and action.active_timesteps[1] > t:
for act_pred in plan.actions[0].preds:
if act_pred['pred'].spacial_anchor == True:
if act_pred['active_timesteps'][0] + act_pred[
'pred'].active_range[0] > t:
end = min(
end, act_pred['active_timesteps'][0] +
act_pred['pred'].active_range[0])
if act_pred['active_timesteps'][1] + act_pred[
'pred'].active_range[1] < t:
start = max(
start, act_pred['active_timesteps'][1] +
act_pred['pred'].active_range[1])
desired_end_pose = robot.rArmPose[:, end]
current_end_pose = robot.rArmPose[:, t]
col_report = CollisionReport()
collisionChecker = RaveCreateCollisionChecker(plan.env, 'pqp')
count = 1
while (body.CheckSelfCollision()
or collisionChecker.CheckCollision(body, report=col_report)
or col_report.minDistance <= pred.dsafe):
step_sign = np.ones(len(arm_inds))
step_sign[np.random.choice(len(arm_inds),
len(arm_inds) / 2,
replace=False)] = -1
# Ask in collision pose to randomly move a step, hopefully out of collision
arm_pose = original_pose + np.multiply(step_sign, joint_step)
rave_body.set_dof({"rArmPose": arm_pose})
# arm_pose = body.GetActiveDOFValues()[arm_inds]
if not count % ATTEMPT_SIZE:
step_factor = step_factor / STEP_DECREASE_FACTOR
joint_step = (ub_limit[arm_inds] -
lb_limit[arm_inds]) / step_factor
count += 1
# For Debug
rave_body.set_pose([0, 0, robot.pose[:, t]])
add_to_attr_inds_and_res(t, attr_inds, res, robot,
[('rArmPose', arm_pose)])
robot._free_attrs['rArmPose'][:, t] = 0
start, end = max(start, t - LIN_SAMP_RANGE), min(t + LIN_SAMP_RANGE, end)
rcollides_traj = np.hstack([
lin_interp_traj(robot.rArmPose[:, start], arm_pose, t - start),
lin_interp_traj(arm_pose, robot.rArmPose[:, end], end - t)[:, 1:]
]).T
i = start + 1
for traj in rcollides_traj[1:-1]:
add_to_attr_inds_and_res(i, attr_inds, res, robot,
[('rArmPose', traj)])
i += 1
return np.array(res), attr_inds
def PlanWorkspacePath(self,
robot,
traj,
timelimit=5.0,
min_waypoint_index=None,
norm_order=2,
**kw_args):
"""
Plan a configuration space path given a workspace path.
All timing information is ignored.
@param robot
@param traj workspace trajectory
represented as OpenRAVE AffineTrajectory
@param min_waypoint_index minimum waypoint index to reach
@param timelimit timeout in seconds
@param norm_order: 1 ==> The L1 norm
2 ==> The L2 norm
inf ==> The L_infinity norm
Used to determine the resolution of collision checked waypoints
in the trajectory
@return qtraj configuration space path
"""
env = robot.GetEnv()
from .exceptions import (TimeoutPlanningError, CollisionPlanningError,
SelfCollisionPlanningError)
from openravepy import (CollisionOptions, CollisionOptionsStateSaver,
CollisionReport)
p = openravepy.KinBody.SaveParameters
with robot, CollisionOptionsStateSaver(env.GetCollisionChecker(),
CollisionOptions.ActiveDOFs), \
robot.CreateRobotStateSaver(p.ActiveDOF | p.LinkTransformation):
manip = robot.GetActiveManipulator()
robot.SetActiveDOFs(manip.GetArmIndices())
# Create a new trajectory starting at current robot location.
qtraj = openravepy.RaveCreateTrajectory(env, '')
qtraj.Init(manip.GetArmConfigurationSpecification('linear'))
qtraj.Insert(0, robot.GetActiveDOFValues())
# Initial search for workspace path timing: one huge step.
t = 0.
dt = traj.GetDuration()
q_resolutions = robot.GetActiveDOFResolutions()
# Smallest CSpace step at which to give up
min_step = numpy.linalg.norm(robot.GetActiveDOFResolutions() /
100.,
ord=norm_order)
ik_options = openravepy.IkFilterOptions.CheckEnvCollisions
start_time = time.time()
epsilon = 1e-6
try:
while t < traj.GetDuration() + epsilon:
# Check for a timeout.
# TODO: This is not really deterministic because we do not
# have control over CPU time. However, it is exceedingly
# unlikely that running the query again will change the
# outcome unless there is a significant change in CPU load.
current_time = time.time()
if (timelimit is not None
and current_time - start_time > timelimit):
raise TimeoutPlanningError(timelimit,
deterministic=True)
# Hypothesize new configuration as closest IK to current
qcurr = robot.GetActiveDOFValues() # Configuration at t.
qnew = manip.FindIKSolution(openravepy.matrixFromPose(
traj.Sample(t + dt)[0:7]),
ik_options,
ikreturn=False,
releasegil=True)
# Check if the step was within joint DOF resolution.
infeasible_step = True
if qnew is not None:
# Found an IK
steps = abs(qnew - qcurr) / q_resolutions
norm = numpy.linalg.norm(steps, ord=norm_order)
if (norm < min_step) and qtraj:
raise PlanningError('Not making progress.')
infeasible_step = norm > 1.0
if infeasible_step:
# Backtrack and try half the step
dt = dt / 2.0
else:
# Move forward to new trajectory time.
robot.SetActiveDOFValues(qnew)
qtraj.Insert(qtraj.GetNumWaypoints(), qnew)
t = min(t + dt, traj.GetDuration())
dt = dt * 2.0
except PlanningError as e:
# Compute the min acceptable time from the min waypoint index.
if min_waypoint_index is None:
min_waypoint_index = traj.GetNumWaypoints() - 1
cspec = traj.GetConfigurationSpecification()
wpts = [
traj.GetWaypoint(i) for i in range(min_waypoint_index + 1)
]
dts = [cspec.ExtractDeltaTime(wpt) for wpt in wpts]
min_time = numpy.sum(dts)
# Throw an error if we haven't reached the minimum waypoint.
if t < min_time:
# FindIKSolutions is slower than FindIKSolution, so call
# this only to identify error when there is no solution.
ik_solutions = manip.FindIKSolutions(
openravepy.matrixFromPose(
traj.Sample(t + dt * 2.0)[0:7]),
openravepy.IkFilterOptions.IgnoreSelfCollisions,
ikreturn=False,
releasegil=True)
collision_error = None
# update collision_error to contain collision info.
with robot.CreateRobotStateSaver(p.LinkTransformation):
for q in ik_solutions:
robot.SetActiveDOFValues(q)
cr = CollisionReport()
if env.CheckCollision(robot, report=cr):
collision_error = \
CollisionPlanningError.FromReport(
cr, deterministic=True)
elif robot.CheckSelfCollision(report=cr):
collision_error = \
SelfCollisionPlanningError.FromReport(
cr, deterministic=True)
else:
collision_error = None
if collision_error is not None:
raise collision_error
else:
raise
# Otherwise we'll gracefully terminate.
else:
logger.warning('Terminated early at time %f < %f: %s', t,
traj.GetDuration(), str(e))
SetTrajectoryTags(qtraj, {
Tags.CONSTRAINED: True,
Tags.DETERMINISTIC_TRAJECTORY: True,
Tags.DETERMINISTIC_ENDPOINT: True,
},
append=True)
return qtraj
def _get_trajectory(self, robot, q_goal, dof_indices):
"""
Generates a hand trajectory that attempts to move the specified
dof_indices to the configuration indicated in q_goal. The trajectory
will move the hand until q_goal is reached or an invalid configuration
(usually collision) is detected.
@param robot: OpenRAVE robot
@param q_goal: goal configuration (dof values)
@param dof_indices: indices of the dofs specified in q_goal
@return hand trajectory
"""
def collision_callback(report, is_physics):
"""
Callback for collision detection. Add the links that are in
collision to the colliding_links set in the enclosing frame.
@param report: collision report
@param is_physics: whether collision is from physics
@return ignore collision action
"""
colliding_links.update([report.plink1, report.plink2])
return CollisionAction.Ignore
env = robot.GetEnv()
collision_checker = env.GetCollisionChecker()
dof_indices = np.array(dof_indices, dtype='int')
report = CollisionReport()
handle = env.RegisterCollisionCallback(collision_callback)
with \
robot.CreateRobotStateSaver(
Robot.SaveParameters.ActiveDOF
| Robot.SaveParameters.LinkTransformation), \
CollisionOptionsStateSaver(
collision_checker,
CollisionOptions.ActiveDOFs):
robot.SetActiveDOFs(dof_indices)
q_prev = robot.GetActiveDOFValues()
# Create the output trajectory.
cspec = robot.GetActiveConfigurationSpecification('linear')
cspec.AddDeltaTimeGroup()
traj = RaveCreateTrajectory(env, '')
traj.Init(cspec)
# Velocity that each joint will be moving at while active.
qd = np.sign(q_goal - q_prev) * robot.GetActiveDOFMaxVel()
# Duration required for each joint to reach the goal.
durations = (q_goal - q_prev) / qd
durations[qd == 0.] = 0.
t_prev = 0.
events = np.concatenate((
[0.],
durations,
np.arange(0., durations.max(), 0.01),
))
mask = np.array([True] * len(q_goal), dtype='bool')
for t_curr in np.unique(events):
robot.SetActiveDOFs(dof_indices[mask])
# Disable joints that have reached the goal.
mask = np.logical_and(mask, durations >= t_curr)
# Advance the active DOFs.
q_curr = q_prev.copy()
q_curr[mask] += (t_curr - t_prev) * qd[mask]
robot.SetDOFValues(q_curr, dof_indices)
# Check for collision. This only operates on the active DOFs
# because the ActiveDOFs flag is set. We use a collision
# callback to build a set of all links in collision.
colliding_links = set()
env.CheckCollision(robot, report=report)
robot.CheckSelfCollision(report=report)
# Check which DOF(s) are involved in the collision.
mask = np.logical_and(mask, [
not any(
robot.DoesAffect(dof_index, link.GetIndex())
for link in colliding_links)
for dof_index in dof_indices
])
# Revert the DOFs that are in collision.
# TODO: This doesn't seem to take the links out of collision.
q_curr = q_prev.copy()
q_curr[mask] += (t_curr - t_prev) * qd[mask]
# Add a waypoint to the output trajectory.
waypoint = np.empty(cspec.GetDOF())
cspec.InsertDeltaTime(waypoint, t_curr - t_prev)
cspec.InsertJointValues(waypoint, q_curr, robot, dof_indices,
0)
traj.Insert(traj.GetNumWaypoints(), waypoint)
t_prev = t_curr
q_prev = q_curr
# Terminate if all joints are inactive.
if not mask.any():
break
del handle
return traj
def PlanWorkspacePath(self,
robot,
traj,
timelimit=5.0,
min_waypoint_index=None,
**kw_args):
"""
Plan a configuration space path given a workspace path.
All timing information is ignored.
@param robot
@param traj workspace trajectory
represented as OpenRAVE AffineTrajectory
@param min_waypoint_index minimum waypoint index to reach
@param timelimit timeout in seconds
@return qtraj configuration space path
"""
from .exceptions import (TimeoutPlanningError, CollisionPlanningError,
SelfCollisionPlanningError)
from openravepy import CollisionReport
p = openravepy.KinBody.SaveParameters
with robot:
manip = robot.GetActiveManipulator()
robot.SetActiveDOFs(manip.GetArmIndices())
# Create a new trajectory starting at current robot location.
qtraj = openravepy.RaveCreateTrajectory(self.env, '')
qtraj.Init(manip.GetArmConfigurationSpecification('linear'))
qtraj.Insert(0, robot.GetActiveDOFValues())
# Initial search for workspace path timing: one huge step.
t = 0.
dt = traj.GetDuration()
# Smallest CSpace step at which to give up
min_step = min(robot.GetActiveDOFResolutions()) / 100.
ik_options = openravepy.IkFilterOptions.CheckEnvCollisions
start_time = time.time()
epsilon = 1e-6
try:
while t < traj.GetDuration() + epsilon:
# Check for a timeout.
current_time = time.time()
if (timelimit is not None
and current_time - start_time > timelimit):
raise TimeoutPlanningError(timelimit)
# Hypothesize new configuration as closest IK to current
qcurr = robot.GetActiveDOFValues() # Configuration at t.
qnew = manip.FindIKSolution(openravepy.matrixFromPose(
traj.Sample(t + dt)[0:7]),
ik_options,
ikreturn=False,
releasegil=True)
# Check if the step was within joint DOF resolution.
infeasible_step = True
if qnew is not None:
# Found an IK
step = abs(qnew - qcurr)
if (max(step) < min_step) and qtraj:
raise PlanningError('Not making progress.')
infeasible_step = \
any(step > robot.GetActiveDOFResolutions())
if infeasible_step:
# Backtrack and try half the step
dt = dt / 2.0
else:
# Move forward to new trajectory time.
robot.SetActiveDOFValues(qnew)
qtraj.Insert(qtraj.GetNumWaypoints(), qnew)
t = min(t + dt, traj.GetDuration())
dt = dt * 2.0
except PlanningError as e:
# Compute the min acceptable time from the min waypoint index.
if min_waypoint_index is None:
min_waypoint_index = traj.GetNumWaypoints() - 1
cspec = traj.GetConfigurationSpecification()
wpts = [
traj.GetWaypoint(i) for i in range(min_waypoint_index + 1)
]
dts = [cspec.ExtractDeltaTime(wpt) for wpt in wpts]
min_time = numpy.sum(dts)
# Throw an error if we haven't reached the minimum waypoint.
if t < min_time:
# FindIKSolutions is slower than FindIKSolution, so call
# this only to identify error when there is no solution.
ik_solutions = manip.FindIKSolutions(
openravepy.matrixFromPose(
traj.Sample(t + dt * 2.0)[0:7]),
openravepy.IkFilterOptions.IgnoreSelfCollisions,
ikreturn=False,
releasegil=True)
collision_error = None
# update collision_error to contain collision info.
with robot.CreateRobotStateSaver(p.LinkTransformation):
for q in ik_solutions:
robot.SetActiveDOFValues(q)
cr = CollisionReport()
if self.env.CheckCollision(robot, report=cr):
collision_error = \
CollisionPlanningError.FromReport(cr)
elif robot.CheckSelfCollision(report=cr):
collision_error = \
SelfCollisionPlanningError.FromReport(cr)
else:
collision_error = None
if collision_error is not None:
raise collision_error
else:
raise
# Otherwise we'll gracefully terminate.
else:
logger.warning('Terminated early at time %f < %f: %s', t,
traj.GetDuration(), str(e))
# Return as much of the trajectory as we have solved.
SetTrajectoryTags(qtraj, {Tags.CONSTRAINED: True}, append=True)
return qtraj
def MoveUntilTouch(manipulator, direction, distance, max_distance=None,
max_force=5.0, max_torque=None, ignore_collisions=None,
velocity_limit_scale=0.25, **kw_args):
"""Execute a straight move-until-touch action.
This action stops when a sufficient force is is felt or the manipulator
moves the maximum distance. The motion is considered successful if the
end-effector moves at least distance. In simulation, a move-until-touch
action proceeds until the end-effector collids with the environment.
@param direction unit vector for the direction of motion in the world frame
@param distance minimum distance in meters
@param max_distance maximum distance in meters
@param max_force maximum force in Newtons
@param max_torque maximum torque in Newton-Meters
@param ignore_collisions collisions with these objects are ignored when
planning the path, e.g. the object you think you will touch
@param velocity_limit_scale A multiplier to use to scale velocity limits
when executing MoveUntilTouch ( < 1 in most cases).
@param **kw_args planner parameters
@return felt_force flag indicating whether we felt a force.
"""
from contextlib import nested
from openravepy import CollisionReport, KinBody, Robot, RaveCreateTrajectory
from prpy.planning.exceptions import CollisionPlanningError
delta_t = 0.01
robot = manipulator.GetRobot()
env = robot.GetEnv()
dof_indices = manipulator.GetArmIndices()
direction = numpy.array(direction, dtype='float')
# Default argument values.
if max_distance is None:
max_distance = 1.
warnings.warn(
'MoveUntilTouch now requires the "max_distance" argument.'
' This will be an error in the future.',
DeprecationWarning)
if max_torque is None:
max_torque = numpy.array([100.0, 100.0, 100.0])
if ignore_collisions is None:
ignore_collisions = []
with env:
# Compute the expected force direction in the hand frame.
hand_pose = manipulator.GetEndEffectorTransform()
force_direction = numpy.dot(hand_pose[0:3, 0:3].T, -direction)
# Disable the KinBodies listed in ignore_collisions. We backup the
# "enabled" state of all KinBodies so we can restore them later.
body_savers = [
body.CreateKinBodyStateSaver() for body in ignore_collisions]
robot_saver = robot.CreateRobotStateSaver(
Robot.SaveParameters.ActiveDOF
| Robot.SaveParameters.ActiveManipulator
| Robot.SaveParameters.LinkTransformation)
with robot_saver, nested(*body_savers) as f:
manipulator.SetActive()
robot_cspec = robot.GetActiveConfigurationSpecification()
for body in ignore_collisions:
body.Enable(False)
path = robot.PlanToEndEffectorOffset(direction=direction,
distance=distance, max_distance=max_distance, **kw_args)
# Execute on the real robot by tagging the trajectory with options that
# tell the controller to stop on force/torque input.
if not manipulator.simulated:
raise NotImplementedError('MoveUntilTouch not yet implemented under ros_control.')
# Forward-simulate the motion until it hits an object.
else:
traj = robot.PostProcessPath(path)
is_collision = False
traj_cspec = traj.GetConfigurationSpecification()
new_traj = RaveCreateTrajectory(env, '')
new_traj.Init(traj_cspec)
robot_saver = robot.CreateRobotStateSaver(
Robot.SaveParameters.LinkTransformation)
with env, robot_saver:
for t in numpy.arange(0, traj.GetDuration(), delta_t):
waypoint = traj.Sample(t)
dof_values = robot_cspec.ExtractJointValues(
waypoint, robot, dof_indices, 0)
manipulator.SetDOFValues(dof_values)
# Terminate if we detect collision with the environment.
report = CollisionReport()
if env.CheckCollision(robot, report=report):
logger.info('Terminated from collision: %s',
str(CollisionPlanningError.FromReport(report)))
is_collision = True
break
elif robot.CheckSelfCollision(report=report):
logger.info('Terminated from self-collision: %s',
str(CollisionPlanningError.FromReport(report)))
is_collision = True
break
# Build the output trajectory that stops in contact.
if new_traj.GetNumWaypoints() == 0:
traj_cspec.InsertDeltaTime(waypoint, 0.)
else:
traj_cspec.InsertDeltaTime(waypoint, delta_t)
new_traj.Insert(new_traj.GetNumWaypoints(), waypoint)
if new_traj.GetNumWaypoints() > 0:
robot.ExecuteTrajectory(new_traj)
return is_collision
def VerifyCollisionFree(self):
report = CollisionReport()
if self.env.CheckCollision(self.robot, report=report):
raise CollisionPlanningError.FromReport(report)
elif self.robot.CheckSelfCollision(report=report):
raise SelfCollisionPlanningError.FromReport(report)
def VerifyCollisionFree(self):
report = CollisionReport()
if self.CheckCollision(report=report):
raise CollisionPlanningError.FromReport(report)
def fn(q):
body.SetActiveDOFValues(q)
report = CollisionReport()
collision = env.CheckCollision(body, report=report)
return collision, report.minDistance