def get_rrt_traj(env, robot, active_dof, init_dof, end_dof): # assert body in env.GetRobot() active_dofs = robot.GetActiveDOFIndices() robot.SetActiveDOFs(active_dof) robot.SetActiveDOFValues(init_dof) params = Planner.PlannerParameters() params.SetRobotActiveJoints(robot) params.SetGoalConfig(end_dof) # set goal to all ones # # forces parabolic planning with 40 iterations # import ipdb; ipdb.set_trace() params.SetExtraParameters("""<_postprocessing planner="parabolicsmoother"> <_nmaxiterations>20</_nmaxiterations> </_postprocessing>""") planner = RaveCreatePlanner(env, 'birrt') planner.InitPlan(robot, params) traj = RaveCreateTrajectory(env, '') result = planner.PlanPath(traj) if result == False: robot.SetActiveDOFs(active_dofs) return None traj_list = [] for i in range(traj.GetNumWaypoints()): # get the waypoint values, this holds velocites, time stamps, etc data = traj.GetWaypoint(i) # extract the robot joint values only dofvalues = traj.GetConfigurationSpecification().ExtractJointValues( data, robot, robot.GetActiveDOFIndices()) # raveLogInfo('waypint %d is %s'%(i,np.round(dofvalues, 3))) traj_list.append(np.round(dofvalues, 3)) robot.SetActiveDOFs(active_dofs) return np.array(traj_list)
def get_ompl_rrtconnect_traj(env, robot, active_dof, init_dof, end_dof): # assert body in env.GetRobot() dof_inds = robot.GetActiveDOFIndices() robot.SetActiveDOFs(active_dof) robot.SetActiveDOFValues(init_dof) params = Planner.PlannerParameters() params.SetRobotActiveJoints(robot) params.SetGoalConfig(end_dof) # set goal to all ones # forces parabolic planning with 40 iterations planner = RaveCreatePlanner(env, 'OMPL_RRTConnect') planner.InitPlan(robot, params) traj = RaveCreateTrajectory(env, '') planner.PlanPath(traj) traj_list = [] for i in range(traj.GetNumWaypoints()): # get the waypoint values, this holds velocites, time stamps, etc data = traj.GetWaypoint(i) # extract the robot joint values only dofvalues = traj.GetConfigurationSpecification().ExtractJointValues( data, robot, robot.GetActiveDOFIndices()) # raveLogInfo('waypint %d is %s'%(i,np.round(dofvalues, 3))) traj_list.append(np.round(dofvalues, 3)) robot.SetActiveDOFs(dof_inds) return traj_list
def test_get_joint(self): """ This test creates a path in openrave format and test the class method get_joint to return a known joint. """ path = blade_coverage.Path(self.rays) ind = str() for i in range(self.robot.GetDOF()): ind += str(i) + ' ' cs = ConfigurationSpecification() _ = cs.AddGroup('joint_values ' + self.robot.GetName() + ' ' + ind, len(self.robot.GetActiveDOFIndices()), 'cubic') cs.AddDerivativeGroups(1, False) cs.AddDerivativeGroups(2, False) _ = cs.AddDeltaTimeGroup() traj = RaveCreateTrajectory(self.turbine.env, '') traj.Init(cs) for i in range(len(self.joints[:1])): waypoint = list(self.joints[i]) waypoint.extend(list(self.joints[i])) waypoint.extend(list(self.joints[i])) waypoint.extend([0]) traj.Insert(traj.GetNumWaypoints(), waypoint) path.data = [traj] joint = path.get_joint(self.robot, 0, 0) self.assertTrue(linalg.norm(joint - self.joints[0]) <= 1e-5, msg='get_joint failed')
def create_trajectory(turbine, joints, joints_vel, joints_acc, times): """ This method creates the trajectory specification in OpenRave format and insert the waypoints: joints - vels - accs - times DOF - DOF - DOF - DOF - 1 Args: turbine: (@ref Turbine) turbine object joints: (float[n<SUB>i</SUB>][nDOF]) joints to create the trajectory joints_vel: (float[n<SUB>i</SUB>][nDOF]) joints_vel to create the trajectory joints_acc: (float[n<SUB>i</SUB>][nDOF]) joints_acc to create the trajectory times: (float[n<SUB>i</SUB>]) deltatimes Returns: trajectory: OpenRave object. """ robot = turbine.robot ind = str() for i in range(robot.GetDOF()): ind += str(i) + ' ' cs = ConfigurationSpecification() _ = cs.AddGroup('joint_values ' + robot.GetName() + ' ' + ind, len(robot.GetActiveDOFIndices()), 'cubic') cs.AddDerivativeGroups(1, False) cs.AddDerivativeGroups(2, False) _ = cs.AddDeltaTimeGroup() traj = RaveCreateTrajectory(turbine.env, '') traj.Init(cs) for i in range(len(joints)): waypoint = list(joints[i]) waypoint.extend(list(joints_vel[i])) waypoint.extend(list(joints_acc[i])) waypoint.extend([times[i]]) traj.Insert(traj.GetNumWaypoints(), waypoint) return traj
def test_serialize(self): """ This test verifies if the class can serialize a path in openrave format. """ ind = str() for i in range(self.robot.GetDOF()): ind += str(i) + ' ' cs = ConfigurationSpecification() _ = cs.AddGroup('joint_values ' + self.robot.GetName() + ' ' + ind, len(self.robot.GetActiveDOFIndices()), 'cubic') traj = RaveCreateTrajectory(self.turbine.env, '') traj.Init(cs) for i in range(len(self.joints)): waypoint = list(self.joints[i]) traj.Insert(traj.GetNumWaypoints(), waypoint) path = blade_coverage.Path(self.rays) path.data = [traj] try: path.serialize('test_serialize') shutil.rmtree('test_serialize') except: self.assertTrue(False, msg='Data can not be serialized')
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 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 FollowVectorField(self, robot, fn_vectorfield, fn_terminate, integration_time_interval=10.0, timelimit=5.0, sampling_func=util.SampleTimeGenerator, norm_order=2, **kw_args): """ Follow a joint space vectorfield to termination. @param robot @param fn_vectorfield a vectorfield of joint velocities @param fn_terminate custom termination condition @param integration_time_interval The time interval to integrate over. @param timelimit time limit before giving up @param sampling_func sample generator to compute validity checks Note: Function will terminate as soon as invalid configuration is encountered. No more samples will be requested from the sampling_func after this occurs. @param norm_order order of norm to use for collision checking @param kw_args keyword arguments to be passed to fn_vectorfield @return traj """ from .exceptions import ( CollisionPlanningError, SelfCollisionPlanningError, ) from openravepy import CollisionReport, RaveCreateTrajectory from ..util import GetLinearCollisionCheckPts import time import scipy.integrate CheckLimitsAction = openravepy.KinBody.CheckLimitsAction # This is a workaround to emulate 'nonlocal' in Python 2. nonlocals = { 'exception': None, 't_cache': None, # Must be negative so we check the start of the trajectory. 't_check': -1., } env = robot.GetEnv() active_indices = robot.GetActiveDOFIndices() # Create a new trajectory matching the current # robot's joint configuration specification cspec = robot.GetActiveConfigurationSpecification('linear') cspec.AddDeltaTimeGroup() cspec.ResetGroupOffsets() path = RaveCreateTrajectory(env, '') path.Init(cspec) time_start = time.time() def fn_wrapper(t, q): """ The integrator will try to solve this equation at each time step. Note: t is the integration time and is non-monotonic. """ # Set the joint values, without checking the joint limits robot.SetActiveDOFValues(q, CheckLimitsAction.Nothing) return fn_vectorfield() 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 (throws an exception on collision) robot_checker.VerifyCollisionFree() # Check the termination condition. status = fn_terminate() if Status.DoesCache(status): nonlocals['t_cache'] = t if Status.DoesTerminate(status): raise TerminationError() def fn_callback(t, q): """ This is called at every successful integration step. """ try: # Add the waypoint to the trajectory. waypoint = numpy.zeros(cspec.GetDOF()) cspec.InsertDeltaTime(waypoint, t - path.GetDuration()) cspec.InsertJointValues(waypoint, q, robot, active_indices, 0) path.Insert(path.GetNumWaypoints(), waypoint) # Run constraint checks at DOF resolution. if path.GetNumWaypoints() == 1: checks = [(t, q)] else: # This returns collision checks for the entire trajectory, # including the part that has already been checked. These # duplicate checks will be filtered out below. checks = GetLinearCollisionCheckPts(robot, path, norm_order=norm_order, sampling_func=sampling_func) for t_check, q_check in checks: # TODO: It would be more efficient to only generate checks # for the new part of the trajectory. if t_check <= nonlocals['t_check']: continue # Record the time of this check so we continue checking # from where we left off. We do this first, just in case # fn_status_callback raises an exception. nonlocals['t_check'] = t_check fn_status_callback(t_check, q_check) return 0 # Keep going. except PlanningError as e: nonlocals['exception'] = e return -1 # Stop. with self.robot_checker_factory(robot) as robot_checker, \ robot.CreateRobotStateSaver(Robot.SaveParameters.LinkTransformation): # Integrate the vector field to get a configuration space path. # # TODO: Tune the integrator parameters. # # Integrator: 'dopri5' # DOPRI (Dormand & Prince 1980) is an explicit method for solving ODEs. # It is a member of the Runge-Kutta family of solvers. integrator = scipy.integrate.ode(f=fn_wrapper) integrator.set_integrator(name='dopri5', first_step=0.1, atol=1e-3, rtol=1e-3) # Set function to be called at every successful integration step. integrator.set_solout(fn_callback) integrator.set_initial_value(y=robot.GetActiveDOFValues(), t=0.) integrator.integrate(t=integration_time_interval) t_cache = nonlocals['t_cache'] exception = nonlocals['exception'] if t_cache is None: raise exception or PlanningError( 'An unknown error has occurred.', deterministic=True) elif exception: logger.warning('Terminated early: %s', str(exception)) # Remove any parts of the trajectory that are not cached. This also # strips the (potentially infeasible) timing information. output_cspec = robot.GetActiveConfigurationSpecification('linear') output_path = RaveCreateTrajectory(env, '') output_path.Init(output_cspec) # Add all waypoints before the last integration step. GetWaypoints does # not include the upper bound, so this is safe. cached_index = path.GetFirstWaypointIndexAfterTime(t_cache) output_path.Insert(0, path.GetWaypoints(0, cached_index), cspec) # Add a segment for the feasible part of the last integration step. output_path.Insert(output_path.GetNumWaypoints(), path.Sample(t_cache), cspec) util.SetTrajectoryTags(output_path, { Tags.SMOOTH: True, Tags.CONSTRAINED: True, Tags.DETERMINISTIC_TRAJECTORY: True, Tags.DETERMINISTIC_ENDPOINT: True, }, append=True) return output_path
def FollowVectorField(self, robot, fn_vectorfield, fn_terminate, integration_time_interval=10.0, timelimit=5.0, **kw_args): """ Follow a joint space vectorfield to termination. @param robot @param fn_vectorfield a vectorfield of joint velocities @param fn_terminate custom termination condition @param integration_time_interval The time interval to integrate over. @param timelimit time limit before giving up @param kw_args keyword arguments to be passed to fn_vectorfield @return traj """ from .exceptions import ( CollisionPlanningError, SelfCollisionPlanningError, ) from openravepy import CollisionReport, RaveCreateTrajectory from ..util import GetCollisionCheckPts import time import scipy.integrate CheckLimitsAction = openravepy.KinBody.CheckLimitsAction # This is a workaround to emulate 'nonlocal' in Python 2. nonlocals = { 'exception': None, 't_cache': None, 't_check': 0., } env = robot.GetEnv() active_indices = robot.GetActiveDOFIndices() # Create a new trajectory matching the current # robot's joint configuration specification cspec = robot.GetActiveConfigurationSpecification('linear') cspec.AddDeltaTimeGroup() cspec.ResetGroupOffsets() path = RaveCreateTrajectory(env, '') path.Init(cspec) time_start = time.time() def fn_wrapper(t, q): """ The integrator will try to solve this equation at each time step. Note: t is the integration time and is non-monotonic. """ # Set the joint values, without checking the joint limits robot.SetActiveDOFValues(q, CheckLimitsAction.Nothing) return fn_vectorfield() 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 fn_callback(t, q): """ This is called at every successful integration step. """ try: # Add the waypoint to the trajectory. waypoint = numpy.zeros(cspec.GetDOF()) cspec.InsertDeltaTime(waypoint, t - path.GetDuration()) cspec.InsertJointValues(waypoint, q, robot, active_indices, 0) path.Insert(path.GetNumWaypoints(), waypoint) # Run constraint checks at DOF resolution. if path.GetNumWaypoints() == 1: checks = [(t, q)] else: # TODO: This should start at t_check. Unfortunately, a bug # in GetCollisionCheckPts causes this to enter an infinite # loop. checks = GetCollisionCheckPts(robot, path, include_start=False) # start_time=nonlocals['t_check']) for t_check, q_check in checks: fn_status_callback(t_check, q_check) # Record the time of this check so we continue checking at # DOF resolution the next time the integrator takes a step. nonlocals['t_check'] = t_check return 0 # Keep going. except PlanningError as e: nonlocals['exception'] = e return -1 # Stop. # Integrate the vector field to get a configuration space path. # # TODO: Tune the integrator parameters. # # Integrator: 'dopri5' # DOPRI (Dormand & Prince 1980) is an explicit method for solving ODEs. # It is a member of the Runge-Kutta family of solvers. integrator = scipy.integrate.ode(f=fn_wrapper) integrator.set_integrator(name='dopri5', first_step=0.1, atol=1e-3, rtol=1e-3) # Set function to be called at every successful integration step. integrator.set_solout(fn_callback) # Initial conditions integrator.set_initial_value(y=robot.GetActiveDOFValues(), t=0.) integrator.integrate(t=integration_time_interval) t_cache = nonlocals['t_cache'] exception = nonlocals['exception'] if t_cache is None: raise exception or PlanningError('An unknown error has occurred.') elif exception: logger.warning('Terminated early: %s', str(exception)) # Remove any parts of the trajectory that are not cached. This also # strips the (potentially infeasible) timing information. output_cspec = robot.GetActiveConfigurationSpecification('linear') output_path = RaveCreateTrajectory(env, '') output_path.Init(output_cspec) # Add all waypoints before the last integration step. GetWaypoints does # not include the upper bound, so this is safe. cached_index = path.GetFirstWaypointIndexAfterTime(t_cache) output_path.Insert(0, path.GetWaypoints(0, cached_index), cspec) # Add a segment for the feasible part of the last integration step. output_path.Insert(output_path.GetNumWaypoints(), path.Sample(t_cache), cspec) # Flag this trajectory as constrained. util.SetTrajectoryTags(output_path, { Tags.CONSTRAINED: 'true', Tags.SMOOTH: 'true' }, append=True) return output_path