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