def __init__(self, robot, traj, num_samples=20, linewidth=2, color=[1, 0, 0, 1], render=True): self.env = robot.GetEnv() self.robot = robot self.handles = list() self.render = render # Rendering options. self.num_samples = num_samples self.linewidth = linewidth self.color = numpy.array(color, dtype='float') # Clone and retime the trajectory. self.traj = openravepy.RaveCreateTrajectory(self.env, 'GenericTrajectory') self.traj.Clone(traj, 0) openravepy.planningutils.RetimeTrajectory(self.traj)
def compute_rave_trajectory(self, robot): """Compute an OpenRAVE trajectory equivalent to this trajectory. Parameters ---------- robot: Openrave robot. Returns ------- trajectory: Equivalent openrave trajectory. """ traj = orpy.RaveCreateTrajectory(robot.GetEnv(), "") spec = robot.GetActiveConfigurationSpecification("cubic") spec.AddDerivativeGroups(1, False) spec.AddDerivativeGroups(2, True) traj.Init(spec) deltas = [0] for i in range(len(self.ss_waypoints) - 1): deltas.append(self.ss_waypoints[i + 1] - self.ss_waypoints[i]) if len(self.ss_waypoints) == 1: q = self(0) qd = self(0, 1) qdd = self(0, 2) traj.Insert(traj.GetNumWaypoints(), list(q) + list(qd) + list(qdd) + [0]) else: qs = self(self.ss_waypoints) qds = self(self.ss_waypoints, 1) qdds = self(self.ss_waypoints, 2) for (q, qd, qdd, dt) in zip(qs, qds, qdds, deltas): traj.Insert( traj.GetNumWaypoints(), q.tolist() + qd.tolist() + qdd.tolist() + [dt], ) return traj
def trajectory_from_waypoints(robot, waypoints): """ Generate an OpenRAVE trajectory using the given waypoints. Parameters ---------- robot: orpy.Robot The OpenRAVE robot waypoints: list List of waypoints (joint configurations) Returns ------- traj: orpy.Trajectory Resulting OpenRAVE trajectory. """ env = robot.GetEnv() traj = orpy.RaveCreateTrajectory(env, '') traj.Init(robot.GetActiveConfigurationSpecification()) for i, q in enumerate(waypoints): traj.Insert(i, q) return traj
def ConvertPlanToTrajectory(self, plan): # Create a trajectory traj = openravepy.RaveCreateTrajectory(self.robot.GetEnv(), 'GenericTrajectory') config_spec = self.robot.GetActiveConfigurationSpecification() traj.Init(config_spec) idx = 0 for pt in plan: traj.Insert(idx, pt) idx = idx + 1 openravepy.planningutils.RetimeActiveDOFTrajectory( traj, self.robot, maxvelmult=1, maxaccelmult=1, hastimestamps=False, plannername='ParabolicTrajectoryRetimer') return traj
def _Plan(self, robot, goals, maxiter=500, continue_planner=False, or_args=None, **kw_args): # Get rid of default postprocessing extraParams = '<_postprocessing planner=""><_nmaxiterations>0</_nmaxiterations></_postprocessing>' # Maximum planner iterations extraParams += '<_nmaxiterations>{:d}</_nmaxiterations>'.format(maxiter) if or_args is not None: for key, value in or_args.iteritems(): extraParams += '<{k:s}>{v:s}</{k:s}>'.format(k=str(key), v=str(value)) params = openravepy.Planner.PlannerParameters() params.SetRobotActiveJoints(robot) params.SetGoalConfig(goals) params.SetExtraParameters(extraParams) traj = openravepy.RaveCreateTrajectory(self.env, 'GenericTrajectory') try: self.env.Lock() # Plan. if (not continue_planner) or not self.setup: self.planner.InitPlan(robot, params) self.setup = True status = self.planner.PlanPath(traj, releasegil=True) if status not in [openravepy.PlannerStatus.HasSolution, openravepy.PlannerStatus.InterruptedWithSolution]: raise PlanningError('Planner returned with status {:s}.' .format(str(status))) except Exception as e: raise PlanningError('Planning failed with error: {:s}'.format(e)) finally: self.env.Unlock() return traj
def plan_to_joint_configuration(robot, qgoal, planner='birrt', max_planner_iterations=20, max_postprocessing_iterations=40): env = robot.GetEnv() rave_planner = orpy.RaveCreatePlanner(env, planner) params = orpy.Planner.PlannerParameters() params.SetRobotActiveJoints(robot) params.SetGoalConfig(qgoal) params.SetMaxIterations(max_planner_iterations) params.SetPostProcessing( 'ParabolicSmoother', '<_nmaxiterations>{0}</_nmaxiterations>'.format( max_postprocessing_iterations)) success = rave_planner.InitPlan(robot, params) if not success: return None # Plan a trajectory traj = orpy.RaveCreateTrajectory(env, '') status = rave_planner.PlanPath(traj) if status != orpy.PlannerStatus.HasSolution: return None return traj
def PlanToConfigurations(self, robot, goal_configs, **kw_args): params = self.defaults._replace(**kw_args) # serialize parameters orparams = openravepy.Planner.PlannerParameters() orparams.SetRobotActiveJoints(robot) goal_configs_stacked = [] for goal_config in goal_configs: goal_configs_stacked.extend(goal_config) orparams.SetGoalConfig(goal_configs_stacked) xml = self.xml_from_params(params) orparams.SetExtraParameters('\n'.join(xml)) self.planner.InitPlan(robot, orparams) # plan path traj = openravepy.RaveCreateTrajectory(self.env, '') status = self.planner.PlanPath(traj) if status == openravepy.PlannerStatus.HasSolution: return traj else: raise prpy.planning.base.PlanningError( 'LEMUR status: {}'.format(status))
def test_consistency(request, env, traj_string): "Check the consistency between the OpenRAVE trajectory and the interpolator." robot = env.GetRobots()[0] active_indices = robot.GetActiveDOFIndices() traj = orpy.RaveCreateTrajectory(env, "") traj.deserialize(traj_string) path = toppra.RaveTrajectoryWrapper(traj, robot) spec = traj.GetConfigurationSpecification() N = 100 ss = np.linspace(0, path.get_duration(), N) # Openrave samples qs_rave = [] qds_rave = [] qdds_rave = [] for s in ss: data = traj.Sample(s) qs_rave.append(spec.ExtractJointValues(data, robot, active_indices, 0)) qds_rave.append(spec.ExtractJointValues(data, robot, active_indices, 1)) qdds_rave.append( spec.ExtractJointValues(data, robot, active_indices, 2)) # Interpolator samples qs_ra = path.eval(ss) qds_ra = path.evald(ss) if path._interpolation != "quadratic": qdds_ra = path.evaldd(ss) # plt.plot(qs_ra) # plt.title(request.node.name) # plt.show() np.testing.assert_allclose(qs_rave, qs_ra, atol=1e-8) np.testing.assert_allclose(qds_rave, qds_ra, atol=1e-8) if path._interpolation != "quadratic": np.testing.assert_allclose(qdds_rave, qdds_ra, atol=1e-8)
def test_Simplifier(self): with self.env: # Setup simplifier = openravepy.RaveCreatePlanner(self.env, 'OMPL_Simplifier') params = openravepy.Planner.PlannerParameters() cspec = self.robot.GetActiveConfigurationSpecification() traj = openravepy.RaveCreateTrajectory(self.env, '') traj.deserialize(self.TRAJECTORY_XML) # Act simplifier.InitPlan(self.robot, params) result = simplifier.PlanPath(traj) # Assert self.assertEqual(result, openravepy.PlannerStatus.HasSolution) self.assertGreaterEqual(traj.GetNumWaypoints(), 1) numpy.testing.assert_array_almost_equal(traj.GetWaypoint(0, cspec), self.START_CONFIG) numpy.testing.assert_array_almost_equal( traj.GetWaypoint(traj.GetNumWaypoints() - 1, cspec), self.GOAL_CONFIG)
def GenerateRetimedTrajectory(self, robot, traj, f=lambda t: t, num_wp_new=None, factor=1): '''Retimes input trajectory for robot according to the given function f. Here f maps new trajectory time to source trajectory time, where inputs and outputs are proportions (ie, they are in the range [0, 1]). @param robot the robot @param traj the source trajectory @param f the function mapping source trajectory time to output traj time @param num_wp_new the number of waypoints in output traj @return new_traj the retimed trajectory ''' num_wp_src = traj.GetNumWaypoints() num_wp_new = num_wp_src if not num_wp_new else num_wp_new dur, spec = traj.GetDuration(), traj.GetConfigurationSpecification() # Make sure that input function does not change total trajectory duration assert abs(f(1) - 1.0) < .01 assert abs(f(0) - 0.0) < .01 new_traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), '') new_traj.Init(spec) t_interval = factor * (dur / (num_wp_new - 1)) beginning, end = traj.GetWaypoint(0), traj.GetWaypoint(num_wp_src - 1) # The start waypoint remains the same new_traj.Insert(0, beginning) for i in range(1, num_wp_new-1): wp = traj.Sample(max(dur * f(float(i) / num_wp_new), 0)) spec.InsertDeltaTime(wp, t_interval) new_traj.Insert(i, wp) # End waypoint remains the same spec.InsertDeltaTime(end, t_interval) new_traj.Insert(num_wp_new - 1, end) # Retime velocities retime_result = openravepy.planningutils.RetimeTrajectory(new_traj, hastimestamps=True) return new_traj
def Generate(self, robot, num_batches, **kw_args): params = self.defaults._replace(max_batches=num_batches, **kw_args) # lock env, save robot state # and create family with robot, type(self).AddedFamilyModule(self.env, robot) as family: setcache_path = self.get_setcache_path(family, params.roadmap, read_only=False) # disable any link that moves due to non-active dofs openravepy.raveLogInfo('[Generate] Computing self-checked set cache for these links:') for linkindex,link in enumerate(robot.GetLinks()): inactive_affected = False for dofindex in range(robot.GetDOF()): if dofindex in robot.GetActiveDOFIndices(): continue joint = robot.GetJointFromDOFIndex(dofindex) jointindex = robot.GetJoints().index(joint) if robot.DoesAffect(jointindex, linkindex): inactive_affected = True if inactive_affected: link.Enable(False) else: openravepy.raveLogInfo('[Generate] [{}] {} ({} geoms)'.format( linkindex, link.GetName(), len(link.GetGeometries()))) # save self-check family.SendCommand('Let Self = $live') openravepy.raveLogInfo('[Generate] Current family:') family.SendCommand('PrintCurrentFamily') self_header = family.SendCommand('GetHeaderFromSet Self') openravepy.raveLogInfo('[Generate] Self header:') for line in self_header.rstrip('\n').split('\n'): openravepy.raveLogInfo('[Generate] {}'.format(line)) # create a planner planner = openravepy.RaveCreatePlanner(self.env, 'FamilyPlanner') if planner is None: raise prpy.planning.base.UnsupportedPlanningError('Unable to create FamilyPlanner planner.') # create params orparams = openravepy.Planner.PlannerParameters() orparams.SetRobotActiveJoints(robot) orparams.SetInitialConfig([]) orparams.SetGoalConfig([]) xml = self.xml_from_params(params) xml.append('<solve_all>true</solve_all>') xml.append('<family_module>{}</family_module>'.format(family.SendCommand('GetInstanceId'))) xml.append('<family_setcaches><setcache><name>Self</name><filename>{}</filename></setcache></family_setcaches>'.format(setcache_path)) orparams.SetExtraParameters('\n'.join(xml)) success = planner.InitPlan(robot, orparams) if not success: raise RuntimeError('InitPlan failed!') result = planner.PlanPath(None) if result != openravepy.PlannerStatus.HasSolution: raise RuntimeError('Planning failed!') # ensure directories exist # from http://stackoverflow.com/a/5032238/5228520 try: os.makedirs(os.path.dirname(setcache_path)) except OSError as exception: if exception.errno != errno.EEXIST: raise openravepy.raveLogInfo('[Generate] Saving set cache ...') planner.SendCommand('SaveSetCaches') # return dummy trajectory traj = openravepy.RaveCreateTrajectory(self.env, '') traj.Init(robot.GetActiveConfigurationSpecification()) return traj
def test_ros_trajectory_from_openrave(self): np.random.seed(123) robot = self.robot qgoal = ru.kinematics.random_joint_values(robot) traj = ru.planning.plan_to_joint_configuration(robot, qgoal, pname='BiRRT') ros_traj = ru.planning.ros_trajectory_from_openrave(robot.GetName(), traj) # Check trajs durations ros_traj_duration = ros_traj.points[-1].time_from_start.to_sec() np.testing.assert_almost_equal(ros_traj_duration, traj.GetDuration()) # Check num of waypoints self.assertEqual(len(ros_traj.points), traj.GetNumWaypoints()) # Send trajectory with repeated waypoints waypoints = [] for i in range(5): q = ru.kinematics.random_joint_values(robot) waypoints.append(q) waypoints.append(q) traj = ru.planning.trajectory_from_waypoints(robot, waypoints) status = ru.planning.retime_trajectory(robot, traj, 'ParabolicTrajectoryRetimer') ros_traj = ru.planning.ros_trajectory_from_openrave(robot.GetName(), traj) # Check trajs durations ros_traj_duration = ros_traj.points[-1].time_from_start.to_sec() np.testing.assert_almost_equal(ros_traj_duration, traj.GetDuration()) # Check num of waypoints self.assertTrue(len(ros_traj.points) < traj.GetNumWaypoints()) # Test corrupted trajectories: missing deltatime env = self.env robot_name = robot.GetName() traj_corrupted = orpy.RaveCreateTrajectory(env, '') spec = traj.GetConfigurationSpecification() values_group = spec.GetGroupFromName('joint_values {0}'.format(robot_name)) velocities_group = spec.GetGroupFromName( 'joint_velocities {0}'.format(robot_name)) deltatime_group = spec.GetGroupFromName('deltatime') spec.RemoveGroups('deltatime') traj_corrupted.Init(spec) for i in xrange(traj.GetNumWaypoints()): waypoint = traj.GetWaypoint(i).tolist() waypoint.pop(deltatime_group.offset) traj_corrupted.Insert(i, waypoint) ros_traj = ru.planning.ros_trajectory_from_openrave(robot.GetName(), traj_corrupted) self.assertEqual(ros_traj, None) # Test corrupted trajectories: missing joint_velocities manip = robot.GetActiveManipulator() spec = manip.GetArmConfigurationSpecification() traj_corrupted = orpy.RaveCreateTrajectory(env, '') traj_corrupted.Init(spec) for i in xrange(traj.GetNumWaypoints()): waypoint = traj.GetWaypoint(i).tolist() values_end = values_group.offset + values_group.dof traj_corrupted.Insert(i, waypoint[values_group.offset:values_end]) ros_traj = ru.planning.ros_trajectory_from_openrave(robot.GetName(), traj_corrupted) self.assertEqual(ros_traj, None) # Test corrupted trajectories: missing joint_values traj_corrupted = orpy.RaveCreateTrajectory(env, '') spec = orpy.ConfigurationSpecification() indices = ' '.join(map(str,manip.GetArmIndices())) spec.AddGroup('joint_velocities {0} {1}'.format(robot_name, indices), robot.GetActiveDOF(), 'linear') traj_corrupted.Init(spec) for i in xrange(traj.GetNumWaypoints()): waypoint = traj.GetWaypoint(i).tolist() values_end = values_group.offset + values_group.dof traj_corrupted.Insert(i, waypoint[values_group.offset:values_end]) ros_traj = ru.planning.ros_trajectory_from_openrave(robot.GetName(), traj_corrupted) self.assertEqual(ros_traj, None)
<seed>0</seed> </roadmap> <do_timing>true</do_timing> <family_module>{}</family_module> <family_setcaches> <setcache><name>Self</name><filename>setcache-Self.txt</filename></setcache> </family_setcaches>''' .format(family.SendCommand('GetInstanceId'))) params.SetGoalConfig(q_goal) print('calling planner.InitPlan ...') success = planner.InitPlan(robot, params) if not success: raise RuntimeError('InitPlan failed!') print('calling planner.PlanPath ...') traj = openravepy.RaveCreateTrajectory(env, '') result = planner.PlanPath(traj) if result != openravepy.PlannerStatus.HasSolution: raise RuntimeError('planning failed!') print('GetTimes:', planner.SendCommand('GetTimes')) #planner.SendCommand('SolveAll') #planner.SendCommand('CacheSaveAll') raise RuntimeError('borking early!') print('AGAIN!') traj = openravepy.RaveCreateTrajectory(env, '') result = planner.PlanPath(traj) if result != openravepy.PlannerStatus.HasSolution:
def make_const_vel(bin_traj, env): g1 = openravepy.ConfigurationSpecification.Group() g1.name = "joint_values ADA 0 1 2 3 4 5 6 7" g1.interpolation = "linear" g1.dof = 8 g1.offset = 0 config_spec = openravepy.ConfigurationSpecification() config_spec.AddDeltaTimeGroup() config_spec.AddGroup(g1) new_bin_traj = openravepy.RaveCreateTrajectory(env, "") new_bin_traj.Init(config_spec) #print new_bin_traj.GetConfigurationSpecification() t = 0 timestep = 0.005 dist_allowed_per_timestep = 0.05 data = bin_traj.Sample(t) prev_joint = np.array([ data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7] ]) t += timestep final_waypoints = [] curr_index = 0 first = True while t < bin_traj.GetDuration(): data = bin_traj.Sample(t) joint = np.array([ data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7] ]) diff_vector = joint - prev_joint dist = LA.norm(diff_vector) if (dist < dist_allowed_per_timestep ): #difference is so small, get rid of this waypoint donothing = 0 elif (dist == dist_allowed_per_timestep): #good, keep this waypoint final_waypoints.append(joint) new_bin_traj.Insert(curr_index, create_waypoint(joint, first, timestep), True) curr_index += 1 prev_joint = joint else: #difference is too big, add points in between lng = np.arange(0, dist, dist_allowed_per_timestep) offset = np.outer(diff_vector / dist, lng) intermed = np.outer(prev_joint, np.ones( offset.shape[1])) + offset #add this difference to prev_j for col in range(intermed.shape[1]): final_waypoints.append(intermed[:, col]) prev_joint = intermed[:, col] new_bin_traj.Insert( curr_index, create_waypoint(intermed[:, col], first, timestep), True) if (first): first = False curr_index += 1 t += timestep return new_bin_traj
def _Snap(self, robot, goal, **kw_args): from prpy.util import CheckJointLimits from prpy.util import GetLinearCollisionCheckPts from prpy.planning.exceptions import CollisionPlanningError from prpy.planning.exceptions import SelfCollisionPlanningError # Create a two-point trajectory between the # current configuration and the goal. # (a straight line in joint space) env = robot.GetEnv() traj = openravepy.RaveCreateTrajectory(env, '') cspec = robot.GetActiveConfigurationSpecification('linear') active_indices = robot.GetActiveDOFIndices() # Check the start position is within joint limits, # this can throw a JointLimitError start = robot.GetActiveDOFValues() CheckJointLimits(robot, start, deterministic=True) # Add the start waypoint start_waypoint = numpy.zeros(cspec.GetDOF()) cspec.InsertJointValues(start_waypoint, start, robot, active_indices, False) traj.Init(cspec) traj.Insert(0, start_waypoint.ravel()) # Make the trajectory end at the goal configuration, as # long as it is not in collision and is not identical to # the start configuration. CheckJointLimits(robot, goal, deterministic=True) if not numpy.allclose(start, goal): goal_waypoint = numpy.zeros(cspec.GetDOF()) cspec.InsertJointValues(goal_waypoint, goal, robot, active_indices, False) traj.Insert(1, goal_waypoint.ravel()) # Get joint configurations to check # Note: this returns a python generator, and if the # trajectory only has one waypoint then only the # start configuration will be collisioned checked. # # Sampling function: # 'linear' # from prpy.util import SampleTimeGenerator # linear = SampleTimeGenerator # 'Van der Corput' from prpy.util import VanDerCorputSampleGenerator vdc = VanDerCorputSampleGenerator checks = GetLinearCollisionCheckPts(robot, traj, norm_order=2, sampling_func=vdc) with self.robot_checker_factory(robot) as robot_checker, \ robot.CreateRobotStateSaver(Robot.SaveParameters.LinkTransformation): # Run constraint checks at DOF resolution: for t, q in checks: # Set the joint positions # Note: the planner is using a cloned 'robot' object robot.SetActiveDOFValues(q) # Check collision (throws an exception on collision) robot_checker.VerifyCollisionFree() SetTrajectoryTags(traj, { Tags.SMOOTH: True, Tags.DETERMINISTIC_TRAJECTORY: True, Tags.DETERMINISTIC_ENDPOINT: True, }, append=True) return traj
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 move_to_joint_position(q, joint_controller, robot, planner='birrt', max_planner_iterations=20, max_postprocessing_iterations=40, dt=1.0 / 150, require_confirm=True, velocity_factor=0.3, acceleration_factor=0.3): """Move the robot to a desired joint position. The routine first plans a collision-free path in the internal environment of `robot`. Then if a collision-free path is found, it executes and waits for trajectory execution to finish before terminating. Args: q (array): Shape (6,). Goal joint position. joint_controller (JointPositionController): A connected joint controller. robot (OpenRAVE Robot): The Denso model. planner (str, optional): OpenRAVE planner to use. max_planner_iterations (int, optional): Number of planning iterations. max_postprocessing_iterations (int, optional): Number of post-processing iterations. dt (float, optional): Time step. velocity_factor (float, optional): Reduction factor. acceleration_factor (float, optional): Reduction factor. Returns: out (bool): True if successfully moves the robot to `q`. Else False. """ q_current = joint_controller.get_joint_positions() env = robot.GetEnv() vlim_original = robot.GetDOFVelocityLimits() alim_original = robot.GetDOFAccelerationLimits() robot.SetDOFVelocityLimits(vlim_original * velocity_factor) robot.SetDOFAccelerationLimits(alim_original * acceleration_factor) with env: robot.SetDOFValues( q_current, dofindices=robot.GetActiveManipulator().GetArmIndices()) rave_planner = orpy.RaveCreatePlanner(env, planner) params = orpy.Planner.PlannerParameters() robot.SetActiveDOFs(robot.GetActiveManipulator().GetArmIndices()) params.SetRobotActiveJoints(robot) params.SetGoalConfig(q) params.SetMaxIterations(max_planner_iterations) params.SetPostProcessing( 'ParabolicSmoother', '<_nmaxiterations>{0}</_nmaxiterations>'.format( max_postprocessing_iterations)) success = rave_planner.InitPlan(robot, params) if not success: return False # Plan a trajectory traj = orpy.RaveCreateTrajectory(env, '') status = rave_planner.PlanPath(traj) if status != orpy.PlannerStatus.HasSolution: return False if require_confirm: raw_input( "Trajectory planned with {}! Press [Enter] to move to\n q:= {}". format(planner, q)) robot.SetDOFVelocityLimits(vlim_original) robot.SetDOFAccelerationLimits(alim_original) if isinstance(joint_controller, JointPositionController): # Execute the trajectory spec = traj.GetConfigurationSpecification() time = 0. rate = rospy.Rate(int(1 / dt)) while time < traj.GetDuration(): trajdata = traj.Sample(time) values = spec.ExtractJointValues( trajdata, robot, robot.GetActiveManipulator().GetArmIndices(), 0) # Send joint signal joint_controller.set_joint_positions(values) robot.SetDOFValues( values, dofindices=robot.GetActiveManipulator().GetArmIndices()) time += dt rate.sleep() return True else: raise ValueError( 'controller [{}] not supported!'.format(joint_controller))
target1 = tr.quaternion_matrix([0.0, 1.0, 0.0, 0.0]) target1[:3, 3] = np.array([0.6, 0, 0.9]) roll1, pitch1, yaw1 = tr.euler_from_matrix(target1, 'syxz') print "target orientation is: ", roll1, pitch1, yaw1 print "target pose is:", target1 solutions = manipulator.FindIKSolutions(target1, orpy.IkFilterOptions.CheckEnvCollisions) print solutions planner = orpy.RaveCreatePlanner(env, 'birrt') # Using bidirectional RRT params = orpy.Planner.PlannerParameters() params.SetRobotActiveJoints(robot) params.SetGoalConfig(solutions[0]) params.SetPostProcessing('ParabolicSmoother', '<_nmaxiterations>10</_nmaxiterations>') planner.InitPlan(robot, params) # Plan a trajectory traj = orpy.RaveCreateTrajectory(env, '') planner.PlanPath(traj) # Execute the trajectory controller = robot.GetController() controller.SetPath(traj) # set the poses #robot.SetActiveDOFValues(solutions[0]) link_idx = [l.GetName() for l in robot.GetLinks()].index('tool0') link_origin = robot.GetLink('tool0').GetTransform()[:3, 3] J = np.zeros((6, 6)) q = robot.GetActiveDOFValues() #move linear J[:3, :] = robot.ComputeJacobianTranslation(link_idx, link_origin)[:, :6] J[3:, :] = robot.ComputeJacobianAxisAngle(link_idx)[:, :6]
def plan_to_joint_configuration(robot, qgoal, pname='BiRRT', max_iters=20, max_ppiters=40, try_swap=False): """ Plan a trajectory to the given `qgoal` configuration. Parameters ---------- robot: orpy.Robot The OpenRAVE robot qgoal: array_like The goal configuration pname: str Name of the planning algorithm. Available options are: `BasicRRT`, `BiRRT` max_iters: float Maximum iterations for the planning stage max_ppiters: float Maximum iterations for the post-processing stage. It will use a parabolic smoother wich short-cuts the trajectory and then smooths it try_swap: bool If set, will compute the direct and reversed trajectory. The minimum duration trajectory is used. Returns ------- traj: orpy.Trajectory Planned trajectory. If plan fails, this function returns `None`. """ qstart = robot.GetActiveDOFValues() env = robot.GetEnv() planner = orpy.RaveCreatePlanner(env, pname) params = orpy.Planner.PlannerParameters() params.SetMaxIterations(max_iters) if max_ppiters > 0: params.SetPostProcessing('ParabolicSmoother', '<_nmaxiterations>{0}</_nmaxiterations>'.format(max_ppiters)) else: params.SetPostProcessing('', '') # Plan trajectory best_traj = None min_duration = float('inf') reversed_is_better = False count = 0 for qa, qb in itertools.permutations([qstart, qgoal], 2): count += 1 with robot: robot.SetActiveDOFValues(qa) params.SetGoalConfig(qb) params.SetRobotActiveJoints(robot) initsuccess = planner.InitPlan(robot, params) if initsuccess: traj = orpy.RaveCreateTrajectory(env, '') # Plan the trajectory status = planner.PlanPath(traj) if status == orpy.PlannerStatus.HasSolution: duration = traj.GetDuration() if duration < min_duration: min_duration = duration best_traj = orpy.RaveCreateTrajectory( env, traj.GetXMLId()) best_traj.Clone(traj, 0) if count == 2: reversed_is_better = True if not try_swap: break # Check if we need to reverse the trajectory if reversed_is_better: best_traj = orpy.planningutils.ReverseTrajectory(best_traj) return best_traj
def PlanToEndEffectorOffset(self, robot, direction, distance, max_distance=None, nullspace=JointLimitAvoidance, timelimit=5.0, step_size=0.001, position_tolerance=0.01, angular_tolerance=0.15, **kw_args): """ Plan to a desired end-effector offset with move-hand-straight constraint. movement less than distance will return failure. The motion will not move further than max_distance. @param robot @param direction unit vector in the direction of motion @param distance minimum distance in meters @param max_distance maximum distance in meters @param timelimit timeout in seconds @param stepsize step size in meters for the Jacobian pseudoinverse controller @param position_tolerance constraint tolerance in meters @param angular_tolerance constraint tolerance in radians @return traj """ if distance < 0: raise ValueError('Distance must be non-negative.') elif numpy.linalg.norm(direction) == 0: raise ValueError('Direction must be non-zero') elif max_distance is not None and max_distance < distance: raise ValueError('Max distance is less than minimum distance.') elif step_size <= 0: raise ValueError('Step size must be positive.') elif position_tolerance < 0: raise ValueError('Position tolerance must be non-negative.') elif angular_tolerance < 0: raise ValueError('Angular tolerance must be non-negative.') # save all active bodies so we only check collision with those active_bodies = [] for body in self.env.GetBodies(): if body.IsEnabled(): active_bodies.append(body) # Normalize the direction vector. direction = numpy.array(direction, dtype='float') direction /= numpy.linalg.norm(direction) # Default to moving an exact distance. if max_distance is None: max_distance = distance with robot: manip = robot.GetActiveManipulator() traj = openravepy.RaveCreateTrajectory(self.env, '') traj.Init(manip.GetArmConfigurationSpecification()) active_dof_indices = manip.GetArmIndices() limits_lower, limits_upper = robot.GetDOFLimits(active_dof_indices) initial_pose = manip.GetEndEffectorTransform() q = robot.GetDOFValues(active_dof_indices) traj.Insert(0, q) start_time = time.time() current_distance = 0.0 sign_flipper = 1 last_rot_error = 9999999999.0 try: while current_distance < max_distance: # Check for a timeout. current_time = time.time() if timelimit is not None and current_time - start_time > timelimit: raise PlanningError('Reached time limit.') # Compute joint velocities using the Jacobian pseudoinverse. q_dot = self.GetStraightVelocity(manip, direction, initial_pose, nullspace, step_size, sign_flipper=sign_flipper) q += q_dot robot.SetDOFValues(q, active_dof_indices) # Check for collisions. #if self.env.CheckCollision(robot): for body in active_bodies: if self.env.CheckCollision(robot, body): raise PlanningError('Encountered collision.') if robot.CheckSelfCollision(): raise PlanningError('Encountered self-collision.') # Check for joint limits. elif not (limits_lower < q).all() or not (q < limits_upper).all(): raise PlanningError( 'Encountered joint limit during Jacobian move.') # Check our distance from the constraint. current_pose = manip.GetEndEffectorTransform() a = initial_pose[0:3, 3] p = current_pose[0:3, 3] orthogonal_proj = ( a - p) - numpy.dot(a - p, direction) * direction if numpy.linalg.norm(orthogonal_proj) > position_tolerance: raise PlanningError( 'Deviated from a straight line constraint.') # Check our orientation against the constraint. offset_pose = numpy.dot(numpy.linalg.inv(current_pose), initial_pose) offset_angle = openravepy.axisAngleFromRotationMatrix( offset_pose) offset_angle_norm = numpy.linalg.norm(offset_angle) if offset_angle_norm > last_rot_error + 0.0005: sign_flipper *= -1 last_rot_error = offset_angle_norm if offset_angle_norm > angular_tolerance: raise PlanningError( 'Deviated from orientation constraint.') traj.Insert(traj.GetNumWaypoints(), q) # Check if we've exceeded the maximum distance by projecting our # displacement along the direction. hand_pose = manip.GetEndEffectorTransform() displacement = hand_pose[0:3, 3] - initial_pose[0:3, 3] current_distance = numpy.dot(displacement, direction) except PlanningError as e: # Throw an error if we haven't reached the minimum distance. if current_distance < distance: raise # Otherwise we'll gracefully terminate. else: logger.warning('Terminated early at distance %f < %f: %s', current_distance, max_distance, e.message) SetTrajectoryTags(output_traj, {Tags.CONSTRAINED: True}, append=True) return traj
def RetimeWholeBodyTrajectory(robot, arm_traj, base_traj): affine_retimer = OpenRAVEAffineRetimer() retimer = ParabolicRetimer() smoother = HauserParabolicSmoother() simplifier = None env = arm_traj.GetEnv() cspec = arm_traj.GetConfigurationSpecification() dof_indices, _ = cspec.ExtractUsedIndices(robot) affine_dofs = (DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis) #_postprocess_envs = collections.defaultdict(openravepy.Environment) '''Separating base and arm trajectory and try to retime them individually. As openrave does not support retime both of the affine and joint_dof at same time There will be issues.As affine retiming keeps the same number of points while joint_dof retiming reduces number of points.''' postprocess_envs = robot._postprocess_envs[openravepy.RaveGetEnvironmentId(env)] retiming_options = dict() affine_retimer_options = dict() with Clone(env, clone_env = postprocess_envs) as cloned_env: cloned_robot1 = cloned_env.Cloned(robot) cloned_robot1.SetActiveDOFs(dof_indices) arm_timed_traj = retimer.RetimeTrajectory(cloned_robot1, arm_traj, **retiming_options) arm_final_traj = CopyTrajectory(arm_timed_traj, env=env) cloned_robot2 = cloned_env.Cloned(robot) cloned_robot2.SetActiveDOFs([],affine_dofs) base_timed_traj = affine_retimer.RetimeTrajectory(cloned_robot2, base_traj, **affine_retimer_options) base_final_traj = CopyTrajectory(base_timed_traj, env = env) '''Creating a whole body trajectory from arm_timed and base_timed traj The timestamp and number of waypoints are going to be different from both the trajectories so if the arm_timed_traj has higher number of waypoints, after the waypoints from base are over, every other waypoint will receive zeros for position and velocity and if the base_timed_traj has higher number of waypoints, then all the arm waypoints will receive the values of last waypoint. Openrave trajectory format now only can receive one deltatime. (Hacky way) as we dont use the affine velocity, the base time is merged in affine_velocity. Thats why it is 4''' base_num_of_way = base_final_traj.GetNumWaypoints() arm_num_of_way = arm_final_traj.GetNumWaypoints() print 'base has '+str(base_num_of_way)+' waypoints' print 'arm has '+str(arm_num_of_way)+' waypoints' a_cspec = arm_final_traj.GetConfigurationSpecification() b_cspec = base_final_traj.GetConfigurationSpecification() with env: robot.SetActiveDOFs(dof_indices, affine_dofs) cspec = robot.GetActiveConfigurationSpecification('linear') traj = openravepy.RaveCreateTrajectory(env, '') cspec.AddGroup('joint_velocities', dof= 8, interpolation='quadratic') cspec.AddGroup('affine_velocities', dof= 4, interpolation='next') cspec.AddDeltaTimeGroup() traj.Init(cspec) highest_way = higherNumber(base_num_of_way, arm_num_of_way) arm_final_waypoint = [] for i in range(highest_way): value = [] arm_waypoint = [] base_waypoint = [] dt = 0.0 if(i>=base_num_of_way): arm_waypoint = arm_final_traj.GetWaypoint(i) base_waypoint = np.zeros(7) dt = arm_waypoint[-1] elif(i>=arm_num_of_way): arm_waypoint = arm_final_waypoint base_waypoint = base_final_traj.GetWaypoint(i) dt = base_waypoint[-1] else: arm_waypoint = arm_final_traj.GetWaypoint(i) base_waypoint = base_final_traj.GetWaypoint(i) dt = arm_waypoint[-1] arm_way_point = arm_waypoint[:-1] value.extend(arm_way_point[:8]) value.extend(base_waypoint[:3]) value.extend(arm_way_point[8:]) value.extend(base_waypoint[3:]) value.extend([dt]) traj.Insert(i, value) arm_final_waypoint = arm_waypoint return traj
def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0, jointstarts=None, jointgoals=None, psample=None, tsr_chains=None, extra_args=None, **kw_args): """ @param allowlimadj If True, adjust the joint limits to include the robot's start configuration """ from openravepy import CollisionOptionsStateSaver, Robot, KinBody if timelimit is None: timelimit = self.timelimit if timelimit <= 0.: raise ValueError('Invalid value for "timelimit". Limit must be' ' non-negative; got {:f}.'.format(timelimit)) env = robot.GetEnv() problem = openravepy.RaveCreateProblem(env, 'CBiRRT') if problem is None: raise UnsupportedPlanningError('Unable to create CBiRRT module.') is_endpoint_deterministic = True is_constrained = False # TODO We may need this work-around because CBiRRT doesn't like it # when an IK solver other than GeneralIK is loaded (e.g. nlopt_ik). # self.ClearIkSolver(robot.GetActiveManipulator()) env.LoadProblem(problem, robot.GetName()) args = ['RunCBiRRT'] args += ['timelimit', str(timelimit)] # By default, CBiRRT interprets the DOF resolutions as an # L-infinity norm; this flag turns on the L-2 norm instead. args += ['bdofresl2norm', '1'] args += ['steplength', '0.05999'] if self._is_baked: args += ['bbakedcheckers', '1'] if extra_args is not None: args += extra_args if smoothingitrs is not None: if smoothingitrs < 0: raise ValueError( 'Invalid number of smoothing iterations. ' 'Value must be non-negative; got {:d}.'.format( smoothingitrs)) args += ['smoothingitrs', str(smoothingitrs)] if allowlimadj is not None: args += ['allowlimadj', str(int(allowlimadj))] if psample is not None: if not (0 <= psample <= 1): raise ValueError( 'Invalid value for "psample". Value must be ' 'in the range [0, 1]; got {:f}.'.format(psample)) args += ['psample', str(psample)] if jointstarts is not None: for start_config in jointstarts: if len(start_config) != robot.GetActiveDOF(): raise ValueError( 'Incorrect number of DOFs in start configuration;' ' expected {:d}, got {:d}'.format( robot.GetActiveDOF(), len(start_config))) args += (['jointstarts'] + self.serialize_dof_values(start_config)) if jointgoals is not None: for goal_config in jointgoals: if len(goal_config) != robot.GetActiveDOF(): raise ValueError( 'Incorrect number of DOFs in goal configuration;' ' expected {:d}, got {:d}'.format( robot.GetActiveDOF(), len(goal_config))) args += ['jointgoals'] + self.serialize_dof_values(goal_config) if len(jointgoals) > 1: is_endpoint_deterministic = False if tsr_chains is not None: for tsr_chain in tsr_chains: args += ['TSRChain', SerializeTSRChain(tsr_chain)] if tsr_chain.sample_goal: is_endpoint_deterministic = False if tsr_chain.constrain: is_constrained = True # FIXME: Why can't we write to anything other than cmovetraj.txt or # /tmp/cmovetraj.txt with CBiRRT? traj_path = 'cmovetraj.txt' args += ['filename', traj_path] args_str = ' '.join(args) # Bypass the context manager since CBiRRT does its own baking in C++. collision_checker = self.robot_checker_factory(robot) options = collision_checker.collision_options if tsr_chains is not None: mimicbodies = [ env.GetKinBody(chain.mimicbodyname) for chain in tsr_chains if chain.mimicbodyname is not 'NULL' ] mimicbody_savers = [ mimicbody.CreateKinBodyStateSaver( KinBody.SaveParameters.LinkTransformation) for mimicbody in mimicbodies ] else: mimicbody_savers = [] with CollisionOptionsStateSaver(env.GetCollisionChecker(), options), \ robot.CreateRobotStateSaver(Robot.SaveParameters.ActiveDOF | Robot.SaveParameters.LinkTransformation), \ contextlib.nested(*mimicbody_savers), save_dof_limits(robot): response = problem.SendCommand(args_str, True) if not response.strip().startswith('1'): raise PlanningError('Unknown error: ' + response, deterministic=False) # Construct the output trajectory. with open(traj_path, 'rb') as traj_file: traj_xml = traj_file.read() traj = openravepy.RaveCreateTrajectory(env, '') traj.deserialize(traj_xml) # Tag the trajectory as non-determistic since CBiRRT is a randomized # planner. Additionally tag the goal as non-deterministic if CBiRRT # chose from a set of more than one goal configuration. SetTrajectoryTags( traj, { Tags.CONSTRAINED: is_constrained, Tags.DETERMINISTIC_TRAJECTORY: False, Tags.DETERMINISTIC_ENDPOINT: is_endpoint_deterministic, }, append=True) # Strip extraneous groups from the output trajectory. # TODO: Where are these groups coming from!? cspec = robot.GetActiveConfigurationSpecification('linear') openravepy.planningutils.ConvertTrajectorySpecification(traj, cspec) 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 plan_transit_motion(robot, q_start, q_goal, pregrasp_start=True, pregrasp_goal=True): """ Plan a transit motion (non-grasping motion) for robot moving from q_start to q_goal. @type robot: openravepy.Robot @param robot: The robot to plan this motion for @type q_start: numpy.ndarray @param q_start: Start configuration of the robot @type q_goal: numpy.ndarray @param q_goal: Goal configuration of the robot @type pregrasp_start: bool @param pregrasp_start: Indicates if pregrasp motion at the beginning of the trajectory should be planned @type pregrasp_goal: bool @param pregrasp_goal: Indicates if pregrasp motion at the end of the trajectory should be planned @rtype: oepnravepy.Trajectory @return: An OpenRAVE trajectory for the robot moving from q_start to q_goal """ # MoveHandStraight parameters minsteps = 1 maxsteps = 10 steplength = 0.005 execute = False env = robot.GetEnv() manip = robot.GetActiveManipulator() basemanip = orpy.interfaces.BaseManipulation(robot) q_original = robot.GetActiveDOFValues() traj_pregrasp_start = None traj_pregrasp_goal = None with robot: # Plan pregrasp motion at the start configuration if pregrasp_start: robot.SetActiveDOFValues(q_start) eematrix = manip.GetTransform() direction = -eematrix[0:3, 2] try: traj_pregrasp_start = basemanip.MoveHandStraight( direction, minsteps=minsteps, maxsteps=maxsteps, steplength=steplength, starteematrix=eematrix, execute=execute, outputtrajobj=True) except: _log.info( "Caught an exception in MoveHandStraight (pregrasp_start)." ) return None if traj_pregrasp_start is None: _log.info("MoveHandStraight failed (pregrasp_start).") new_q_start = q_start pregrasp_start = False else: new_q_start = traj_pregrasp_start.GetWaypoint( traj_pregrasp_start.GetNumWaypoints() - 1, traj_pregrasp_start.GetConfigurationSpecification(). GetGroupFromName("joint_values")) robot.SetActiveDOFValues(new_q_start) if robot.CheckSelfCollision(): new_q_start = q_start pregrasp_start = False else: orpy.planningutils.RetimeActiveDOFTrajectory( traj_pregrasp_start, robot, hastimestamps=False, maxvelmult=0.9, maxaccelmult=0.81, plannername="parabolictrajectoryretimer") else: new_q_start = q_start # Plan pregrasp motion at the goal configuration if pregrasp_goal: robot.SetActiveDOFValues(q_goal) eematrix = manip.GetTransform() direction = -eematrix[0:3, 2] try: traj_pregrasp_goal = basemanip.MoveHandStraight( direction, minsteps=minsteps, maxsteps=maxsteps, steplength=steplength, starteematrix=eematrix, execute=execute, outputtrajobj=True) except: _log.info( "Caught an exception in MoveHandStraight (pregrasp_goal).") return None if traj_pregrasp_goal is None: _log.info("MoveHandStraight failed (pregrasp_goal).") new_q_goal = q_goal pregrasp_goal = False else: new_q_goal = traj_pregrasp_goal.GetWaypoint( traj_pregrasp_goal.GetNumWaypoints() - 1, traj_pregrasp_goal.GetConfigurationSpecification(). GetGroupFromName("joint_values")) robot.SetActiveDOFValues(new_q_goal) if robot.CheckSelfCollision(): new_q_goal = q_goal pregrasp_goal = False else: traj_pregrasp_goal_rev = orpy.planningutils.ReverseTrajectory( traj_pregrasp_goal) orpy.planningutils.RetimeActiveDOFTrajectory( traj_pregrasp_goal_rev, robot, hastimestamps=False, maxvelmult=0.9, maxaccelmult=0.81, plannername="parabolictrajectoryretimer") traj_pregrasp_goal = traj_pregrasp_goal_rev else: new_q_goal = q_goal _log.info("new_q_start = {0}".format(new_q_start.__repr__())) _log.info("new_q_goal = {0}".format(new_q_goal.__repr__())) # Plan a motion connecting new_q_start and new_q_goal try: # Using MoveActiveJoints sometimes has problems with jittering. As a fix, we plan using this birrt + parabolicsmoother planner = orpy.RaveCreatePlanner(env, 'birrt') params = orpy.Planner.PlannerParameters() params.SetRobotActiveJoints( robot ) # robot's active DOFs must be correctly set before calling this function params.SetInitialConfig(new_q_start) params.SetGoalConfig(new_q_goal) params.SetMaxIterations(10000) extraparams = '<_postprocessing planner="parabolicsmoother2"><_nmaxiterations>100</_nmaxiterations></_postprocessing>' params.SetExtraParameters(extraparams) planner.InitPlan(robot, params) traj_connect = orpy.RaveCreateTrajectory(env, '') res = planner.PlanPath(traj_connect) if not (res == orpy.PlannerStatus.HasSolution): _log.info("Planner failed.") return None except Exception as e: _log.info( "Caught an exception ({0}) in MoveActiveJoints.".format(e)) return None if traj_connect is None: _log.info("MoveActiveJoints failed.") return None trajs_list = [ traj for traj in [traj_pregrasp_start, traj_connect, traj_pregrasp_goal] if traj is not None ] final_traj = combine_openrave_trajectories(trajs_list) return final_traj
def plan_constant_velocity_twist(robot, twist, velocity, num_waypoints=10): """ Plan the cartesian trajectory to apply the given twist to the current robot configuration at constant velocity Parameters ---------- robot: orpy.Robot The OpenRAVE robot twist: array_like The twist to be applied to the end-effector velocity: float Robot velocity in the cartesian workspace (units are meters) num_waypoints: int Number of waypoints to be used by the trajectory Returns ------- traj: orpy.Trajectory Planned trajectory. If plan fails, this function returns `None`. """ manip = robot.GetActiveManipulator() indices = manip.GetArmIndices() DOF = robot.GetActiveDOF() velocity_limits = robot.GetDOFVelocityLimits()[indices] spec = orpy.ConfigurationSpecification() suffix = robot.GetName() + ' ' + ' '.join(map(str, indices)) values_offset = spec.AddGroup('joint_values ' + suffix, DOF, 'linear') velocities_offset = spec.AddGroup('joint_velocities ' + suffix, DOF, 'next') deltatime_offset = spec.AddGroup('deltatime', 1, '') traj = orpy.RaveCreateTrajectory(robot.GetEnv(), '') traj.Init(spec) # Add the current robot joint values waypoint = np.zeros(deltatime_offset + 1) waypoint[values_offset:values_offset + DOF] = robot.GetActiveDOFValues() traj.Insert(0, waypoint) # Compute and populate the traj waypoints distance = np.linalg.norm(twist) duration = distance / velocity dt = duration / float(num_waypoints) delta = np.array(twist) / float(num_waypoints) with robot: for i in xrange(num_waypoints): q = robot.GetActiveDOFValues() J = ru.kinematics.compute_jacobian(robot) Jpinv = np.linalg.pinv(J) delta_q = np.dot(Jpinv, delta) q += delta_q qdot = delta_q / dt if not ru.kinematics.check_joint_limits(robot, q): break if np.any(np.abs(qdot) > velocity_limits): break robot.SetActiveDOFValues(q) # Populate the waypoint waypoint = np.zeros(deltatime_offset + 1) waypoint[values_offset:values_offset + DOF] = q waypoint[velocities_offset:velocities_offset + DOF] = qdot * np.ones(DOF) waypoint[deltatime_offset] = dt traj.Insert(traj.GetNumWaypoints(), waypoint) if traj.GetNumWaypoints() < (num_waypoints - 1): return None else: return traj
def PlanToBasePose(self, robot, goal_pose, timelimit=60.0, return_first=False, **kw_args): """ Plan to a base pose using SBPL @param robot @param goal_pose desired base pose @param timelimit timeout in seconds @param return_first return the first path found (if true, the planner will run until a path is found, ignoring the time limit) """ params = openravepy.Planner.PlannerParameters() from openravepy import DOFAffine robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis) params.SetRobotActiveJoints(robot) config_spec = openravepy.RaveGetAffineConfigurationSpecification( DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, robot) #params.SetConfigurationSpecification(self.env, config_spec) # This breaks goal_config = openravepy.RaveGetAffineDOFValuesFromTransform( goal_pose, DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis) params.SetGoalConfig(goal_config) # Setup default extra parameters extra_params = self.planner_params limits = robot.GetAffineTranslationLimits() extents = [limits[0][0], limits[1][0], limits[0][1], limits[1][1]] extra_params["extents"] = extents extra_params["timelimit"] = timelimit if return_first: extra_params["return_first"] = 1 else: extra_params["return_first"] = 0 extra_params["initial_eps"] = 1.0 for key, value in kw_args.iteritems(): extra_params[key] = value params.SetExtraParameters(str(extra_params)) traj = openravepy.RaveCreateTrajectory(self.env, '') try: self.planner.InitPlan(robot, params) status = self.planner.PlanPath(traj, releasegil=True) except Exception as e: raise PlanningError('Planning failed with error: {0:s}'.format(e)) from openravepy import PlannerStatus if status not in [ PlannerStatus.HasSolution, PlannerStatus.InterruptedWithSolution ]: raise PlanningError('Planner returned with status {0:s}.'.format( str(status))) return traj
def PlanToEndEffectorOffset(self, robot, direction, distance, max_distance=None, timelimit=5.0, **kw_args): """ Plan to a desired end-effector offset with move-hand-straight constraint. movement less than distance will return failure. The motion will not move further than max_distance. @param robot @param direction unit vector in the direction of motion @param distance minimum distance in meters @param max_distance maximum distance in meters @param timelimit timeout in seconds @return traj """ env = robot.GetEnv() if distance < 0: raise ValueError('Distance must be non-negative.') elif numpy.linalg.norm(direction) == 0: raise ValueError('Direction must be non-zero') elif max_distance is not None and max_distance < distance: raise ValueError('Max distance is less than minimum distance.') elif max_distance is not None and not numpy.isfinite(max_distance): raise ValueError('Max distance must be finite.') # Normalize the direction vector. direction = numpy.array(direction, dtype='float') direction /= numpy.linalg.norm(direction) with robot: manip = robot.GetActiveManipulator() start_pose = manip.GetEndEffectorTransform() traj = openravepy.RaveCreateTrajectory(env, '') spec = openravepy.IkParameterization.\ GetConfigurationSpecificationFromType( openravepy.IkParameterizationType.Transform6D, 'linear') traj.Init(spec) traj.Insert(traj.GetNumWaypoints(), openravepy.poseFromMatrix(start_pose)) min_pose = numpy.copy(start_pose) min_pose[0:3, 3] += distance * direction traj.Insert(traj.GetNumWaypoints(), openravepy.poseFromMatrix(min_pose)) if max_distance is not None: max_pose = numpy.copy(start_pose) max_pose[0:3, 3] += max_distance * direction traj.Insert(traj.GetNumWaypoints(), openravepy.poseFromMatrix(max_pose)) with robot.CreateRobotStateSaver( Robot.SaveParameters.LinkTransformation): openravepy.planningutils.RetimeAffineTrajectory( traj, maxvelocities=0.1 * numpy.ones(7), maxaccelerations=0.1 * numpy.ones(7)) return self.PlanWorkspacePath(robot, traj, timelimit, min_waypoint_index=1)
def executePath(robot, path, resolution, handles): manip = robot.SetActiveManipulator('arm_torso') print 'path: ' + str(len(path)) dis_poses = discretizePath(path, resolution) print 'dis_poses: ' + str(len(dis_poses)) poses = [] base_poses = [] all_poses = [] all_poses.append(dis_poses) jointnames = [ 'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ] robot.SetActiveDOFs( [robot.GetJoint(name).GetDOFIndex() for name in jointnames], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis) cspec = robot.GetActiveConfigurationSpecification('linear') traj = openravepy.RaveCreateTrajectory(env, '') cspec.AddGroup('joint_velocities', dof=8, interpolation='quadratic') cspec.AddGroup('affine_velocities', dof=4, interpolation='next') cspec.AddDeltaTimeGroup() traj.Init(cspec) #Creating the first point of the trajectory (the current joint values of the robot) arm_curr = robot.GetDOFValues(manip.GetArmIndices()) base_curr = np.zeros(3) base_vel = np.zeros(4) arm_vel_cuur = np.zeros(8) dt = 0. value = [] value.extend(arm_curr) value.extend(base_curr) value.extend(arm_vel_cuur) value.extend(base_vel) value.extend([dt]) traj.Insert(0, value) curr_time = round(time.time() * 1000) for i in range(len(dis_poses) - 1): pose, base_pose, arm_vel, finalgoal = executeVelPath(robot, dis_poses[i + 1], handles, unitTime=1.0) # now creating other waypoints of the trajectory value = [] value.extend(finalgoal[:8]) value.extend(finalgoal[-3:]) value.extend(arm_vel) value.extend(np.zeros(3)) time_now = round(time.time() * 1000) dt = time_now - curr_time value.extend([dt / 5000.]) value.extend([dt / 5000.]) traj.Insert(i + 1, value) poses.append(pose) base_poses.append(base_pose) save_trajectory( traj, '/home/abhi/Desktop/traj2/whole_body_zigzag_timed_traj.xml') all_poses.append(poses) all_poses.append(base_poses) #print 'size of base points: '+str(len(base_goal)) #print 'size of arm points: '+str(len(arm_goal)) # #Creating base Traj # doft = openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis # cspec = openravepy.RaveGetAffineConfigurationSpecification(doft, robot) # base_traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), 'GenericTrajectory') # base_traj.Init(cspec) # for i in range(len(base_goal)): # base_traj.Insert(i, base_goal[i]) # save_trajectory(base_traj,'/home/abhi/Desktop/traj2/base_untimed_traj.xml') # print 'base_traj saved' # #Creating Arm Traj # jointnames=['torso_lift_joint','shoulder_pan_joint','shoulder_lift_joint','upperarm_roll_joint','elbow_flex_joint','forearm_roll_joint','wrist_flex_joint','wrist_roll_joint'] # robot.SetActiveDOFs([robot.GetJoint(name).GetDOFIndex() for name in jointnames],) # acspec = robot.GetActiveConfigurationSpecification('linear') # arm_traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), '') # arm_traj.Init(acspec) # for i in range(len(arm_goal)): # arm_traj.Insert(i, arm_goal[i]) # save_trajectory(arm_traj,'/home/abhi/Desktop/traj2/arm_untimed_traj.xml') # print 'arm_traj saved' return all_poses
def run(self, offset=0.01): """Run the demo. For each object, the robot first move to a pose that is directly on top of it. Afterward, it moves down. Next, it grabs the object to be transported. Finally, it moves to another configuration. Note: planning is done using rrt, not constrained rrt. The later is far too inefficient to be of any use. An even more critical issue with constrained rrt is that its path is too jerky, failing most smoothing attempts. Args: offset: (float, optional) Approach distance. """ fail = False # Start planning/control loop: # Every pick cycle follows the same procedure: # 1. APPROACH: visit a configuration that is directly on top of the object to pick # 2. REACH: move down to make contact w the object # 3. APPROACH: visit the same configuration as 1. # 4. TRANSPORT: visit the goal configuration. for obj_dict in self._scenario['objects']: t0 = self.get_time() # Basic setup manip_name = obj_dict["object_attach_to"] manip = self.get_robot().SetActiveManipulator(manip_name) Tstart = np.array(obj_dict['T_start']) # object's transform Tgoal = np.array(obj_dict['T_goal']) T_ee_start = np.dot( Tstart, self.get_object( obj_dict['name']).get_T_object_link()) self.verify_transform(manip, T_ee_start) T_ee_top = np.array(T_ee_start) try: T_ee_top[:3, 3] -= obj_dict['offset'] * T_ee_top[:3, 2] except BaseException: T_ee_top[:3, 3] -= offset * T_ee_top[:3, 2] q_nominal = np.r_[-0.3, 0.9, 0.9, 0, 0, 0] t1 = self.get_time() # 1. APPROACH logger.info("Plan path to APPROACH") traj0 = plan_to_manip_transform( self._robot, T_ee_top, q_nominal, max_ppiters=200, max_iters=100) t1a = self.get_time() if self.mode == "PRACTICE": self.check_continue() fail = not self.execute_trajectory(traj0) self.get_robot().WaitForController(0) # 2. REACH: Move a "short" trajectory to reach the object logger.info("Plan path to REACH") t2 = self.get_time() traj0b = plan_to_manip_transform( self._robot, T_ee_start, q_nominal, max_ppiters=200, max_iters=100) t2a = self.get_time() if self.mode == "PRACTICE": self.check_continue() fail = not self.execute_trajectory(traj0b) self.get_robot().WaitForController(0) with self._env: self._robot.Grab(self.get_env().GetKinBody(obj_dict['name'])) logger.info("Grabbing the object. Continue moving in 1 sec.") # time.sleep(1) # 3+4: APPROACH+TRANSPORT: Plan two trajectories, one # trajectory to reach the REACH position, another # trajectory to reach the GOAL position. Merge them, then # execute. t3 = self.get_time() logger.info("Plan path to GOAL") # 1st trajectory traj0c = plan_to_manip_transform( self._robot, T_ee_top, q_nominal, max_ppiters=1, max_iters=100) traj0c_waypoints, traj0c_ss = self.extract_waypoints(traj0c) T_ee_goal = np.dot( Tgoal, self.get_object( obj_dict['name']).get_T_object_link()) self.verify_transform(manip, T_ee_goal) q_nominal = np.r_[0.3, 0.9, 0.9, 0, 0, 0] # 2nd trajectory self._robot.SetActiveDOFValues(traj0c_waypoints[-1]) traj1_transport = plan_to_manip_transform( self._robot, T_ee_goal, q_nominal, max_ppiters=1, max_iters=100) self._robot.SetActiveDOFValues(traj0c_waypoints[0]) traj1_transport_waypoints, traj1_transport_ss = self.extract_waypoints( traj1_transport) # concatenate the two trajectories traj2_waypoints = np.vstack( (traj0c_waypoints, traj1_transport_waypoints[1:])) # retime traj2_ss = np.hstack( (traj0c_ss, traj0c_ss[-1] + traj1_transport_ss[1:])) traj2_ss[:] = traj2_ss / traj2_ss[-1] traj2_rave = orpy.RaveCreateTrajectory(self._env, "") spec = self._robot.GetActiveConfigurationSpecification() traj2_rave.Init(spec) for p in traj2_waypoints: traj2_rave.Insert(traj2_rave.GetNumWaypoints(), p) t3a = self.get_time() planner = orpy.RaveCreatePlanner(self._env, "ParabolicSmoother") params = orpy.Planner.PlannerParameters() params.SetRobotActiveJoints(self._robot) params.SetMaxIterations(100) params.SetPostProcessing('', '') success = planner.InitPlan(self._robot, params) status = planner.PlanPath(traj2_rave) if not success or not status: logger.fatal("[Plan Transport Path] Init status: {1:}, Plan status: {0:}. " "Use traj2_rave directly.".format(status, success)) t3b = self.get_time() # Retime the transport trajectory and execute it logger.debug( "Original traj nb waypoints: {:d}".format( traj1_transport.GetNumWaypoints())) logger.debug("Retime using toppra.") t4 = self.get_time() contact = self.get_object(obj_dict['name']).get_contact() contact_constraint = create_object_transporation_constraint( contact, self.get_object(obj_dict['name'])) contact_constraint.set_discretization_type(1) traj2_retimed = toppra.retime_active_joints_kinematics( traj2_rave, self._robot, additional_constraints=[contact_constraint], solver_wrapper=SOLVER, vmult=0.999, amult=0.999) if traj2_retimed is None: logger.error( "Transport trajectory retime fails! Try again without contact constraints.") traj2_retimed = toppra.retime_active_joints_kinematics( traj2_rave, self.get_robot(), additional_constraints=[]) t4a = self.get_time() if self.mode == "PRACTICE": self.check_continue() fail = not self.execute_trajectory(traj2_retimed) self.get_robot().WaitForController(0) # release the object logger.info("RELEASE object") self.check_continue() # release object now self._robot.Release(self.get_env().GetKinBody(obj_dict['name'])) t4b = self.get_time() logger.info("Time report" "\n - setup :{0:f} secs" "\n - APPROACH plan :{1:f} secs" "\n - APPROACH duration :{6:f} secs" "\n - REACH plan :{2:f} secs" "\n - REACH duration :{7:f} secs" "\n - MOVE plan :{3:f} secs" "\n - MOVE shortcut :{4:f} secs" "\n - MOVE retime :{5:f} secs" "\n - MOVE duration :{8:f} secs" "\n - TOTAL duration :{9:f} secs".format( t1 - t0, t1a - t1, t2a - t2, t3a - t3, t3b - t3a, t4a - t4, traj0.GetDuration(), traj0b.GetDuration(), traj2_retimed.GetDuration(), t4b - t0)) # remove objects from environment T_cur = self.get_env().GetKinBody(obj_dict['name']).GetTransform() T_cur[2, 3] -= 3e-3 # simulate 5mm drop self.get_env().GetKinBody(obj_dict['name']).SetTransform(T_cur) # Move the robot back to HOME trajHOME = plan_to_joint_configuration(self._robot, self._qHOME) fail = not self.execute_trajectory(trajHOME) return not fail
atexit.register(openravepy.RaveDestroy) e = openravepy.Environment() atexit.register(e.Destroy) e.SetViewer('qtcoin') r = e.ReadRobotXMLFile('robots/mico-modified.robot.xml') e.Add(r) c = openravepy.RaveCreateController(e, 'roscontroller openrave / 1') r.SetController(c, range(8), 0) #r.SetActiveDOFs(range(6)) time.sleep(1) # make a sweet trajectory raw_input('Press [Enter] to send trajectory ...') t = openravepy.RaveCreateTrajectory(e, '') t.Init(r.GetActiveConfigurationSpecification()) print(t.GetConfigurationSpecification()) t.Insert(0, r.GetActiveDOFValues()) if r.GetActiveDOFValues()[5] < 0.5: t.Insert(1, [0.0, -1.57, 1.57, 0.0, 0.0, 1.0, 0.0, 0.0]) else: t.Insert(1, [0.0, -1.57, 1.57, 0.0, 0.0, 0.0, 0.0, 0.0]) openravepy.planningutils.RetimeActiveDOFTrajectory(t, r) r.GetController().SetPath(t) raw_input('Press [Enter] to quit ...')