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
