def plan(self, qgoal, max_iters=40, max_ppiters=40): traj = orpy.RaveCreateTrajectory(self.env, '') params = orpy.Planner.PlannerParameters() # Configuration specification robot_name = self.robot.GetName() torso_idx = self.torso_joint.GetDOFIndex() spec = orpy.ConfigurationSpecification() spec.AddGroup('joint_values {0} {1}'.format(robot_name, torso_idx), 1, '') left_spec = self.left_manip.GetArmConfigurationSpecification() right_spec = self.right_manip.GetArmConfigurationSpecification() spec += left_spec + right_spec params.SetConfigurationSpecification(self.env, spec) params.SetGoalConfig(qgoal) params.SetMaxIterations(max_iters) params.SetPostProcessing( 'ParabolicSmoother', '<_nmaxiterations>{}</_nmaxiterations>'.format(max_ppiters)) planner = orpy.RaveCreatePlanner(self.env, 'BiRRT') success = planner.InitPlan(None, params) status = orpy.PlannerStatus.Failed if success: status = planner.PlanPath(traj) if status != orpy.PlannerStatus.HasSolution: traj = None return traj
def FromRaveTraj(robot, traj): N = traj.GetNumWaypoints() if N < 2: print("RAVE trajectory has less than 2 waypoints") return None timespec = openravepy.ConfigurationSpecification() timespec.AddGroup('deltatime', 1, 'linear') posspec = robot.GetActiveConfigurationSpecification() velspec = posspec.ConvertToVelocitySpecification() chunkslist = [] vel = traj.GetWaypoint(0, velspec) ndof = len(vel) for i in range(N - 1): pos = traj.GetWaypoint(i, posspec) deltatime = traj.GetWaypoint(i + 1, timespec)[0] if deltatime < 1e-5: continue nextvel = traj.GetWaypoint(i + 1, velspec) polynomialslist = [] for j in range(ndof): x = pos[j] v = vel[j] a = (nextvel[j] - vel[j]) / deltatime polynomialslist.append(Trajectory.Polynomial([x, v, a / 2])) chunkslist.append(Trajectory.Chunk(deltatime, polynomialslist)) vel = nextvel return Trajectory.PiecewisePolynomialTrajectory(chunkslist)
def array_to_traj(robot, a, dt=1): spec = openravepy.ConfigurationSpecification() name = "joint_values %s %s" % (robot.GetName(), ' '.join( map(str, robot.GetActiveDOFIndices()))) spec.AddGroup(name, robot.GetActiveDOF(), interpolation="linear") spec.AddDeltaTimeGroup() traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), '') traj.Init(spec) for i, joints in enumerate(a): pt = np.zeros(spec.GetDOF()) spec.InsertJointValues(pt, joints, robot, robot.GetActiveDOFIndices(), 0) spec.InsertDeltaTime(pt, 0 if i == 0 else dt) traj.Insert(i, pt) return traj
def fix_trajectory(traj): """Remove duplicate waypoints that are introduced during smoothing. """ cspec = openravepy.ConfigurationSpecification() cspec.AddDeltaTimeGroup() iwaypoint = 1 num_removed = 0 while iwaypoint < traj.GetNumWaypoints(): waypoint = traj.GetWaypoint(iwaypoint, cspec) delta_time = cspec.ExtractDeltaTime(waypoint) if delta_time == 0.0: traj.Remove(iwaypoint, iwaypoint + 1) num_removed += 1 else: iwaypoint += 1 return num_removed
def ToRaveTraj(robot, spec, traj): nchunks = len(traj.chunkslist) if nchunks < 1: print("TOPP trajectory has less than 1 chunk") return None timespec = openravepy.ConfigurationSpecification() timespec.AddGroup('deltatime', 1, 'linear') posspec = robot.GetActiveConfigurationSpecification() velspec = posspec.ConvertToVelocitySpecification() ravetraj = openravepy.RaveCreateTrajectory(robot.GetEnv(), '') ravetraj.Init(spec) ravetraj.Insert(0, array([0]), timespec) ravetraj.Insert(0, traj.Eval(0), posspec, True) ravetraj.Insert(0, traj.Evald(0), velspec, True) for i in range(nchunks): c = traj.chunkslist[i] ravetraj.Insert(i + 1, array([c.duration]), timespec) ravetraj.Insert(i + 1, c.Eval(c.duration), posspec, True) ravetraj.Insert(i + 1, c.Evald(c.duration), velspec, True) return ravetraj
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 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)
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 PlanParallel(self,basegoals,armgoals): if not self.parallelplanning: print 'Error: PlanParallel is called even though planner was initialized not parallel.' raise Exception('Error: PlanParallel is called even though planner was initialized not parallel.') initial_base_poses = {} initial_arm_configs = {} grabbed = {} for yname in self.youbots: initial_base_poses[yname] = self.youbots[yname].GetTransform() initial_arm_configs[yname] = self.youbots[yname].GetDOFValues() grabbed[yname] = self.youbots[yname].GetGrabbed() try: for yname in self.youbots: self.youbots[yname].ReleaseAllGrabbed() self.youbots[yname].SetTransform(self.awaypose) raw_input('Hit enter to bring basej robots') for yname in self.youbots: self.youbots_base_joints[yname].SetDOFValues([0.,0.,0.],[0,1,2]) # reset the base joints self.youbots_base_joints[yname].SetDOFValues(initial_arm_configs[yname], np.append(self.youbots_base_joints[yname].GetManipulators()[0].GetArmIndices(), self.youbots_base_joints[yname].GetManipulators()[0].GetGripperIndices())) self.youbots_base_joints[yname].SetTransform(np.eye(4)) # FIXME different height? probably not, but check. #if yname == 'drc2': # remove=np.eye(4) # remove[0,3] = 5.5 # self.youbots_base_joints[yname].SetTransform(remove) # FIXME different height? probably not, but check. SetTransformAsDOF(self.youbots_base_joints[yname],initial_base_poses[yname]) if grabbed[yname] is not None: for o in grabbed[yname]: self.youbots_base_joints[yname].Grab(o) raw_input('Hit enter to plan.') configspec = orpy.ConfigurationSpecification() initialconfig = np.array([]) goal = np.array([]) for yname in self.youbots: self.youbots_base_joints[yname].SetActiveDOFs([self.youbots_base_joints[yname].GetJoint('basex').GetDOFIndex(), self.youbots_base_joints[yname].GetJoint('basey').GetDOFIndex(), self.youbots_base_joints[yname].GetJoint('baserotation').GetDOFIndex(), self.youbots_base_joints[yname].GetJoint('j0').GetDOFIndex(), self.youbots_base_joints[yname].GetJoint('j1').GetDOFIndex(), self.youbots_base_joints[yname].GetJoint('j2').GetDOFIndex(), self.youbots_base_joints[yname].GetJoint('j3').GetDOFIndex(), self.youbots_base_joints[yname].GetJoint('j4').GetDOFIndex()]) configspec = configspec + self.youbots_base_joints[yname].GetActiveConfigurationSpecification() goal = np.append(goal,basegoals[yname]) goal = np.append(goal,armgoals[yname]) initialconfig = np.append(initialconfig, self.youbots_base_joints[yname].GetActiveDOFValues()) params = orpy.Planner.PlannerParameters() params.SetExtraParameters("<_fsteplength>0.15</_fsteplength>") params = orpy.Planner.PlannerParameters(params) params.SetExtraParameters("<_nmaxiterations>10000</_nmaxiterations>") # FIXME check if this takes effect params = orpy.Planner.PlannerParameters(params) params.SetConfigurationSpecification(self.env, configspec) params.SetInitialConfig(initialconfig) params.SetGoalConfig(goal) #IPython.embed() self.planner.InitPlan(None,params) traj = orpy.RaveCreateTrajectory(self.env,"") status = self.planner.PlanPath(traj) for yname in self.youbots: self.youbots_base_joints[yname].ReleaseAllGrabbed() self.youbots_base_joints[yname].SetTransform(self.awaypose) self.youbots_base_joints[yname].SetDOFValues([0.]*self.youbots_base_joints[yname].GetDOF()) finally: for yname in self.youbots: self.youbots[yname].SetTransform(initial_base_poses[yname]) self.youbots[yname].SetDOFValues(initial_arm_configs[yname]) if grabbed[yname] is not None: for o in grabbed[yname]: self.youbots[yname].Grab(o) IPython.embed() # TODO check status? # TODO convert traj to base and arm trajs? return status,base_trajs,arm_trajs