def deserialize(self, turbine, directory):
""" Method deserializes data in OpenRave format.
Args:
turbine: (@ref Turbine) is the turbine object.
directory: (str) is the relative path to the folder where to load.
Examples:
>>> path.deserialize(turbine,'my_folder')
"""
ind = str()
for i in range(turbine.robot.GetDOF()):
ind += str(i) + ' '
cs = ConfigurationSpecification()
_ = cs.AddGroup('joint_values ' + turbine.robot.GetName() + ' ' + ind,
len(turbine.robot.GetActiveDOFIndices()), 'cubic')
cs.AddDerivativeGroups(1, False)
cs.AddDerivativeGroups(2, False)
_ = cs.AddDeltaTimeGroup()
directory = realpath(directory)
TRAJ = []
onlyfiles = [
f for f in listdir(directory) if isfile(join(directory, f))
]
onlyfiles.sort(key=int)
for afile in onlyfiles:
traj = RaveCreateTrajectory(turbine.env, '')
traj.Init(cs)
xml = ET.parse(open(join(directory, afile)))
traj.deserialize(ET.tostring(xml.getroot()))
TRAJ.append(traj)
self.data = TRAJ
return
def get_ompl_rrtconnect_traj(env, robot, active_dof, init_dof, end_dof):
# assert body in env.GetRobot()
dof_inds = robot.GetActiveDOFIndices()
robot.SetActiveDOFs(active_dof)
robot.SetActiveDOFValues(init_dof)
params = Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig(end_dof) # set goal to all ones
# forces parabolic planning with 40 iterations
planner = RaveCreatePlanner(env, 'OMPL_RRTConnect')
planner.InitPlan(robot, params)
traj = RaveCreateTrajectory(env, '')
planner.PlanPath(traj)
traj_list = []
for i in range(traj.GetNumWaypoints()):
# get the waypoint values, this holds velocites, time stamps, etc
data = traj.GetWaypoint(i)
# extract the robot joint values only
dofvalues = traj.GetConfigurationSpecification().ExtractJointValues(
data, robot, robot.GetActiveDOFIndices())
# raveLogInfo('waypint %d is %s'%(i,np.round(dofvalues, 3)))
traj_list.append(np.round(dofvalues, 3))
robot.SetActiveDOFs(dof_inds)
return traj_list
def test_get_joint(self):
""" This test creates a path in openrave format and test the class method get_joint to return a known joint.
"""
path = blade_coverage.Path(self.rays)
ind = str()
for i in range(self.robot.GetDOF()):
ind += str(i) + ' '
cs = ConfigurationSpecification()
_ = cs.AddGroup('joint_values ' + self.robot.GetName() + ' ' + ind,
len(self.robot.GetActiveDOFIndices()), 'cubic')
cs.AddDerivativeGroups(1, False)
cs.AddDerivativeGroups(2, False)
_ = cs.AddDeltaTimeGroup()
traj = RaveCreateTrajectory(self.turbine.env, '')
traj.Init(cs)
for i in range(len(self.joints[:1])):
waypoint = list(self.joints[i])
waypoint.extend(list(self.joints[i]))
waypoint.extend(list(self.joints[i]))
waypoint.extend([0])
traj.Insert(traj.GetNumWaypoints(), waypoint)
path.data = [traj]
joint = path.get_joint(self.robot, 0, 0)
self.assertTrue(linalg.norm(joint - self.joints[0]) <= 1e-5,
msg='get_joint failed')
def get_rrt_traj(env, robot, active_dof, init_dof, end_dof):
# assert body in env.GetRobot()
active_dofs = robot.GetActiveDOFIndices()
robot.SetActiveDOFs(active_dof)
robot.SetActiveDOFValues(init_dof)
params = Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig(end_dof) # set goal to all ones
# # forces parabolic planning with 40 iterations
# import ipdb; ipdb.set_trace()
params.SetExtraParameters("""<_postprocessing planner="parabolicsmoother">
<_nmaxiterations>20</_nmaxiterations>
</_postprocessing>""")
planner = RaveCreatePlanner(env, 'birrt')
planner.InitPlan(robot, params)
traj = RaveCreateTrajectory(env, '')
result = planner.PlanPath(traj)
if result == False:
robot.SetActiveDOFs(active_dofs)
return None
traj_list = []
for i in range(traj.GetNumWaypoints()):
# get the waypoint values, this holds velocites, time stamps, etc
data = traj.GetWaypoint(i)
# extract the robot joint values only
dofvalues = traj.GetConfigurationSpecification().ExtractJointValues(
data, robot, robot.GetActiveDOFIndices())
# raveLogInfo('waypint %d is %s'%(i,np.round(dofvalues, 3)))
traj_list.append(np.round(dofvalues, 3))
robot.SetActiveDOFs(active_dofs)
return np.array(traj_list)
class SmoothTrajectoryTest(object):
def setUp(self):
from openravepy import RaveCreateTrajectory
cspec = self.robot.GetActiveConfigurationSpecification('linear')
self.feasible_path = RaveCreateTrajectory(self.env, '')
self.feasible_path.Init(cspec)
self.feasible_path.Insert(0, self.waypoint1)
self.feasible_path.Insert(1, self.waypoint2)
self.feasible_path.Insert(2, self.waypoint3)
def test_SmoothTrajectory_DoesNotModifyBoundaryPoints(self):
from numpy.testing import assert_allclose
# Setup/Test
traj = self.planner.RetimeTrajectory(self.robot, self.feasible_path)
# Assert
cspec = self.robot.GetActiveConfigurationSpecification('linear')
self.assertGreaterEqual(traj.GetNumWaypoints(), 2)
first_waypoint = traj.GetWaypoint(0, cspec)
last_waypoint = traj.GetWaypoint(traj.GetNumWaypoints() - 1, cspec)
assert_allclose(first_waypoint, self.waypoint1)
assert_allclose(last_waypoint, self.waypoint3)
class ShortcutPathTest(object):
def setUp(self):
from openravepy import RaveCreateTrajectory
self.input_path = RaveCreateTrajectory(self.env, '')
self.input_path.Init(
self.robot.GetActiveConfigurationSpecification('linear'))
self.input_path.Insert(0, self.waypoint1)
self.input_path.Insert(1, self.waypoint2)
self.input_path.Insert(2, self.waypoint3)
def test_ShortcutPath_ShortcutExists_ReducesLength(self):
from numpy.testing import assert_allclose
# Setup/Test
smoothed_path = self.planner.ShortcutPath(self.robot, self.input_path)
# Assert
self.assertEquals(smoothed_path.GetConfigurationSpecification(),
self.input_path.GetConfigurationSpecification())
self.assertGreaterEqual(smoothed_path.GetNumWaypoints(), 2)
n = smoothed_path.GetNumWaypoints()
assert_allclose(smoothed_path.GetWaypoint(0), self.waypoint1)
assert_allclose(smoothed_path.GetWaypoint(n - 1), self.waypoint3)
self.assertLess(self.ComputeArcLength(smoothed_path),
0.5 * self.ComputeArcLength(self.input_path))
def setUp(self):
from openravepy import RaveCreateTrajectory
self.input_path = RaveCreateTrajectory(self.env, '')
self.input_path.Init(
self.robot.GetActiveConfigurationSpecification('linear'))
self.input_path.Insert(0, self.waypoint1)
self.input_path.Insert(1, self.waypoint2)
self.input_path.Insert(2, self.waypoint3)
def __init__(self, template_traj, delay=False):
from openravepy import RaveCreateTrajectory
MockPlanner.__init__(self, delay=delay)
with self.env:
# Clone the template trajectory into the planning environment.
self.traj = RaveCreateTrajectory(self.env,
template_traj.GetXMLId())
self.traj.Clone(template_traj, 0)
def Success_impl(robot):
import numpy
from openravepy import RaveCreateTrajectory
cspec = robot.GetActiveConfigurationSpecification()
traj = RaveCreateTrajectory(self.env, 'GenericTrajectory')
traj.Init(cspec)
traj.Insert(0, numpy.zeros(cspec.GetDOF()))
traj.Insert(1, numpy.ones(cspec.GetDOF()))
return traj
def get_feasible_path(robot, path):
"""
Check a path for feasibility (collision) and return a truncated path
containing the path only up to just before the first collision.
@param robot: the OpenRAVE robot
@param path: the path to check for feasiblity
@return tuple with the (possibly) truncated path, and a boolean indicating
whether or not path was truncated
"""
env = robot.GetEnv()
path_cspec = path.GetConfigurationSpecification()
dof_indices, _ = path_cspec.ExtractUsedIndices(robot)
# Create a ConfigurationSpecification containing only positions.
cspec = path.GetConfigurationSpecification()
with robot.CreateRobotStateSaver(Robot.SaveParameters.LinkTransformation):
# Check for collision. Compute a unit timing to simplify this.
if IsTimedTrajectory(path):
unit_path = path
else:
unit_path = ComputeUnitTiming(robot, path)
t_collision = None
t_prev = 0.0
for t, dofvals in GetCollisionCheckPts(robot, unit_path):
robot.SetDOFValues(dofvals, dof_indices)
if env.CheckCollision(robot) or robot.CheckSelfCollision():
t_collision = t_prev
break
t_prev = t
# Build a path (with no timing) that stops before collision.
output_path = RaveCreateTrajectory(env, '')
if t_collision is not None:
end_idx = unit_path.GetFirstWaypointIndexAfterTime(t_collision)
if end_idx > 0:
waypoints = unit_path.GetWaypoints(0, end_idx, cspec)
else:
waypoints = list()
waypoints = np.concatenate((waypoints,
unit_path.Sample(t_collision, cspec)))
output_path.Init(cspec)
output_path.Insert(0, waypoints)
else:
output_path.Clone(path, 0)
return output_path, t_collision is not None
def setUp(self):
from openravepy import planningutils, Planner, RaveCreateTrajectory
cspec = self.robot.GetActiveConfigurationSpecification('linear')
self.feasible_path = RaveCreateTrajectory(self.env, '')
self.feasible_path.Init(cspec)
self.feasible_path.Insert(0, self.waypoint1)
self.feasible_path.Insert(1, self.waypoint2)
self.feasible_path.Insert(2, self.waypoint3)
self.dt = 0.01
self.tolerance = 0.1 # 10% error
def setUp(self):
from openravepy import Environment, RaveCreateTrajectory
self.join_timeout = 5.0
self.env = Environment()
self.env.Load('data/lab1.env.xml')
self.robot = self.env.GetRobot('BarrettWAM')
# Create a valid trajectory used to test planner successes.
cspec = self.robot.GetActiveConfigurationSpecification()
self.traj = RaveCreateTrajectory(self.env, 'GenericTrajectory')
self.traj.Init(cspec)
self.traj.Insert(0, numpy.zeros(cspec.GetDOF()))
self.traj.Insert(1, numpy.ones(cspec.GetDOF()))
def PlanPath(self, planner_name):
params = Planner.PlannerParameters()
params.SetRobotActiveJoints(self.robot_)
init = self.env_.RobotGetInitialPosition()
goal = self.env_.RobotGetGoalPosition()
params.SetInitialConfig(init)
params.SetGoalConfig(goal)
planner = RaveCreatePlanner(self.env_.env, planner_name)
if planner is None:
print "###############################################################"
print "PLANNER", planner_name, "not implemented"
print "###############################################################"
sys.exit(0)
#######################################################################
planner.InitPlan(self.robot_, params)
rave_traj = RaveCreateTrajectory(self.env_.env, '')
t1 = time.time()
result = planner.PlanPath(rave_traj)
t2 = time.time()
if result != PlannerStatus.HasSolution:
print "Could not find geometrical path"
print "Planner:", planner_name
print "Status :", result
return None
print "Planner", planner_name, " success | time:", t2 - t1
return rave_traj
class SuccessPlanner(MockPlanner):
def __init__(self, template_traj, delay=False):
from openravepy import RaveCreateTrajectory
MockPlanner.__init__(self, delay=delay)
with self.env:
# Clone the template trajectory into the planning environment.
self.traj = RaveCreateTrajectory(self.env,
template_traj.GetXMLId())
self.traj.Clone(template_traj, 0)
@ClonedPlanningMethod
def PlanTest(self, robot):
def Success_impl(robot):
import numpy
from openravepy import RaveCreateTrajectory
cspec = robot.GetActiveConfigurationSpecification()
traj = RaveCreateTrajectory(self.env, 'GenericTrajectory')
traj.Init(cspec)
traj.Insert(0, numpy.zeros(cspec.GetDOF()))
traj.Insert(1, numpy.ones(cspec.GetDOF()))
return traj
return self._PlanGeneric(Success_impl, robot)
def test_rrt_planner(self):
# Adopting examples from openrave
domain, problem, params = load_environment(
'../domains/baxter_domain/baxter.domain',
'../domains/baxter_domain/baxter_probs/grasp_1234_1.prob')
env = Environment() # create openrave environment
objLst = [i[1] for i in params.items() if not i[1].is_symbol()]
view = OpenRAVEViewer.create_viewer(env)
view.draw(objLst, 0, 0.7)
can_body = view.name_to_rave_body["can0"]
baxter_body = view.name_to_rave_body["baxter"]
can = can_body.env_body
robot = baxter_body.env_body
dof = robot.GetActiveDOFValues()
inds = baxter_body._geom.dof_map['rArmPose']
r_init = params['robot_init_pose']
r_end = params['robot_end_pose']
dof[inds] = r_init.rArmPose.flatten()
robot.SetActiveDOFValues(dof)
robot.SetActiveDOFs(inds) # set joints the first 4 dofs
plan_params = Planner.PlannerParameters()
plan_params.SetRobotActiveJoints(robot)
plan_params.SetGoalConfig(
[0.7, -0.204, 0.862, 1.217, 2.731, 0.665,
2.598]) # set goal to all ones
# forces parabolic planning with 40 iterations
traj = RaveCreateTrajectory(env, '')
# Using openrave built in planner
trajectory = {}
plan_params.SetExtraParameters(
""" <_postprocessing planner="parabolicsmoother">
<_nmaxiterations>17</_nmaxiterations>
</_postprocessing>""")
trajectory["BiRRT"] = planing(env, robot, plan_params, traj,
'BiRRT') # 3.5s
# trajectory["BasicRRT"] = planing(env, robot, plan_params, traj, 'BasicRRT') # 0.05s can't run it by its own
# trajectory["ExplorationRRT"] = planing(env, robot, plan_params, traj, 'ExplorationRRT') # 0.03s
# plan_params.SetExtraParameters('<range>0.2</range>')
# Using OMPL planner
# trajectory["OMPL_RRTConnect"] = planing(env, robot, plan_params, traj, 'OMPL_RRTConnect') # 1.5s
# trajectory["OMPL_RRT"] = planing(env, robot, plan_params, traj, 'OMPL_RRT') # 10s
# trajectory["OMPL_RRTstar"] = planing(env, robot, plan_params, traj, 'OMPL_RRTstar') # 10s
# trajectory["OMPL_TRRT"] = planing(env, robot, plan_params, traj, 'OMPL_TRRT') # 10s
# trajectory["OMPL_pRRT"] = planing(env, robot, plan_params, traj, 'OMPL_pRRT') # Having issue, freeze
# trajectory["OMPL_LazyRRT"] = planing(env, robot, plan_params, traj, 'OMPL_LazyRRT') # 1.5s - 10s unsatble
# ompl_traj = trajectory["OMPL_RRTConnect"]
or_traj = trajectory["BiRRT"]
result = process_traj(or_traj, 20)
self.assertTrue(result.shape[1] == 20)
def create_trajectory(turbine, joints, joints_vel, joints_acc, times):
""" This method creates the trajectory specification in OpenRave format and insert the waypoints:
joints - vels - accs - times
DOF - DOF - DOF - DOF - 1
Args:
turbine: (@ref Turbine) turbine object
joints: (float[n<SUB>i</SUB>][nDOF]) joints to create the trajectory
joints_vel: (float[n<SUB>i</SUB>][nDOF]) joints_vel to create the trajectory
joints_acc: (float[n<SUB>i</SUB>][nDOF]) joints_acc to create the trajectory
times: (float[n<SUB>i</SUB>]) deltatimes
Returns:
trajectory: OpenRave object.
"""
robot = turbine.robot
ind = str()
for i in range(robot.GetDOF()):
ind += str(i) + ' '
cs = ConfigurationSpecification()
_ = cs.AddGroup('joint_values ' + robot.GetName() + ' ' + ind,
len(robot.GetActiveDOFIndices()), 'cubic')
cs.AddDerivativeGroups(1, False)
cs.AddDerivativeGroups(2, False)
_ = cs.AddDeltaTimeGroup()
traj = RaveCreateTrajectory(turbine.env, '')
traj.Init(cs)
for i in range(len(joints)):
waypoint = list(joints[i])
waypoint.extend(list(joints_vel[i]))
waypoint.extend(list(joints_acc[i]))
waypoint.extend([times[i]])
traj.Insert(traj.GetNumWaypoints(), waypoint)
return traj
def test_serialize(self):
""" This test verifies if the class can serialize a path in openrave format.
"""
ind = str()
for i in range(self.robot.GetDOF()):
ind += str(i) + ' '
cs = ConfigurationSpecification()
_ = cs.AddGroup('joint_values ' + self.robot.GetName() + ' ' + ind,
len(self.robot.GetActiveDOFIndices()), 'cubic')
traj = RaveCreateTrajectory(self.turbine.env, '')
traj.Init(cs)
for i in range(len(self.joints)):
waypoint = list(self.joints[i])
traj.Insert(traj.GetNumWaypoints(), waypoint)
path = blade_coverage.Path(self.rays)
path.data = [traj]
try:
path.serialize('test_serialize')
shutil.rmtree('test_serialize')
except:
self.assertTrue(False, msg='Data can not be serialized')
def _get_trajectory(self, robot, q_goal, dof_indices):
"""
Generates a hand trajectory that attempts to move the specified
dof_indices to the configuration indicated in q_goal. The trajectory
will move the hand until q_goal is reached or an invalid configuration
(usually collision) is detected.
@param robot: OpenRAVE robot
@param q_goal: goal configuration (dof values)
@param dof_indices: indices of the dofs specified in q_goal
@return hand trajectory
"""
def collision_callback(report, is_physics):
"""
Callback for collision detection. Add the links that are in
collision to the colliding_links set in the enclosing frame.
@param report: collision report
@param is_physics: whether collision is from physics
@return ignore collision action
"""
colliding_links.update([report.plink1, report.plink2])
return CollisionAction.Ignore
env = robot.GetEnv()
collision_checker = env.GetCollisionChecker()
dof_indices = np.array(dof_indices, dtype='int')
report = CollisionReport()
handle = env.RegisterCollisionCallback(collision_callback)
with \
robot.CreateRobotStateSaver(
Robot.SaveParameters.ActiveDOF
| Robot.SaveParameters.LinkTransformation), \
CollisionOptionsStateSaver(
collision_checker,
CollisionOptions.ActiveDOFs):
robot.SetActiveDOFs(dof_indices)
q_prev = robot.GetActiveDOFValues()
# Create the output trajectory.
cspec = robot.GetActiveConfigurationSpecification('linear')
cspec.AddDeltaTimeGroup()
traj = RaveCreateTrajectory(env, '')
traj.Init(cspec)
# Velocity that each joint will be moving at while active.
qd = np.sign(q_goal - q_prev) * robot.GetActiveDOFMaxVel()
# Duration required for each joint to reach the goal.
durations = (q_goal - q_prev) / qd
durations[qd == 0.] = 0.
t_prev = 0.
events = np.concatenate((
[0.],
durations,
np.arange(0., durations.max(), 0.01),
))
mask = np.array([True] * len(q_goal), dtype='bool')
for t_curr in np.unique(events):
robot.SetActiveDOFs(dof_indices[mask])
# Disable joints that have reached the goal.
mask = np.logical_and(mask, durations >= t_curr)
# Advance the active DOFs.
q_curr = q_prev.copy()
q_curr[mask] += (t_curr - t_prev) * qd[mask]
robot.SetDOFValues(q_curr, dof_indices)
# Check for collision. This only operates on the active DOFs
# because the ActiveDOFs flag is set. We use a collision
# callback to build a set of all links in collision.
colliding_links = set()
env.CheckCollision(robot, report=report)
robot.CheckSelfCollision(report=report)
# Check which DOF(s) are involved in the collision.
mask = np.logical_and(mask, [
not any(
robot.DoesAffect(dof_index, link.GetIndex())
for link in colliding_links)
for dof_index in dof_indices
])
# Revert the DOFs that are in collision.
# TODO: This doesn't seem to take the links out of collision.
q_curr = q_prev.copy()
q_curr[mask] += (t_curr - t_prev) * qd[mask]
# Add a waypoint to the output trajectory.
waypoint = np.empty(cspec.GetDOF())
cspec.InsertDeltaTime(waypoint, t_curr - t_prev)
cspec.InsertJointValues(waypoint, q_curr, robot, dof_indices,
0)
traj.Insert(traj.GetNumWaypoints(), waypoint)
t_prev = t_curr
q_prev = q_curr
# Terminate if all joints are inactive.
if not mask.any():
break
del handle
return traj
class RetimeTrajectoryTest(object):
def setUp(self):
from openravepy import planningutils, Planner, RaveCreateTrajectory
cspec = self.robot.GetActiveConfigurationSpecification('linear')
self.feasible_path = RaveCreateTrajectory(self.env, '')
self.feasible_path.Init(cspec)
self.feasible_path.Insert(0, self.waypoint1)
self.feasible_path.Insert(1, self.waypoint2)
self.feasible_path.Insert(2, self.waypoint3)
self.dt = 0.01
self.tolerance = 0.1 # 10% error
def test_RetimeTrajectory(self):
import numpy
from numpy.testing import assert_allclose
from openravepy import planningutils, Planner
# Setup/Test
traj = self.planner.RetimeTrajectory(self.robot, self.feasible_path)
# Assert
position_cspec = self.feasible_path.GetConfigurationSpecification()
velocity_cspec = position_cspec.ConvertToDerivativeSpecification(1)
zero_dof_values = numpy.zeros(position_cspec.GetDOF())
# Verify that the trajectory passes through the original waypoints.
waypoints = [self.waypoint1, self.waypoint2, self.waypoint3]
waypoint_indices = [None] * len(waypoints)
for iwaypoint in xrange(traj.GetNumWaypoints()):
joint_values = traj.GetWaypoint(iwaypoint, position_cspec)
# Compare the waypoint against every input waypoint.
for icandidate, candidate_waypoint in enumerate(waypoints):
if numpy.allclose(joint_values, candidate_waypoint):
self.assertIsNone(
waypoint_indices[icandidate],
'Input waypoint {} appears twice in the output'
' trajectory (indices: {} and {})'.format(
icandidate, waypoint_indices[icandidate],
iwaypoint))
waypoint_indices[icandidate] = iwaypoint
self.assertEquals(waypoint_indices[0], 0)
self.assertEquals(waypoint_indices[-1], traj.GetNumWaypoints() - 1)
for iwaypoint in waypoint_indices:
self.assertIsNotNone(iwaypoint)
# Verify that the velocity at the waypoint is zero.
joint_velocities = traj.GetWaypoint(iwaypoint, velocity_cspec)
assert_allclose(joint_velocities, zero_dof_values)
# Verify the trajectory between waypoints.
for t in numpy.arange(self.dt, traj.GetDuration(), self.dt):
iafter = traj.GetFirstWaypointIndexAfterTime(t)
ibefore = iafter - 1
joint_values = traj.Sample(t, position_cspec)
joint_values_before = traj.GetWaypoint(ibefore, position_cspec)
joint_values_after = traj.GetWaypoint(iafter, position_cspec)
distance_full = numpy.linalg.norm(joint_values_after -
joint_values_before)
distance_before = numpy.linalg.norm(joint_values -
joint_values_before)
distance_after = numpy.linalg.norm(joint_values -
joint_values_after)
deviation = distance_before + distance_after - distance_full
self.assertLess(deviation, self.tolerance * distance_full)
# Check joint limits and dynamic feasibility.
params = Planner.PlannerParameters()
params.SetRobotActiveJoints(self.robot)
planningutils.VerifyTrajectory(params, traj, self.dt)
def FollowVectorField(self, robot, fn_vectorfield, fn_terminate,
integration_time_interval=10.0, timelimit=5.0,
sampling_func=util.SampleTimeGenerator,
norm_order=2, **kw_args):
"""
Follow a joint space vectorfield to termination.
@param robot
@param fn_vectorfield a vectorfield of joint velocities
@param fn_terminate custom termination condition
@param integration_time_interval The time interval to integrate
over.
@param timelimit time limit before giving up
@param sampling_func sample generator to compute validity checks
Note: Function will terminate as soon as invalid configuration is
encountered. No more samples will be requested from the
sampling_func after this occurs.
@param norm_order order of norm to use for collision checking
@param kw_args keyword arguments to be passed to fn_vectorfield
@return traj
"""
from .exceptions import (
CollisionPlanningError,
SelfCollisionPlanningError,
)
from openravepy import CollisionReport, RaveCreateTrajectory
from ..util import GetLinearCollisionCheckPts
import time
import scipy.integrate
CheckLimitsAction = openravepy.KinBody.CheckLimitsAction
# This is a workaround to emulate 'nonlocal' in Python 2.
nonlocals = {
'exception': None,
't_cache': None,
# Must be negative so we check the start of the trajectory.
't_check': -1.,
}
env = robot.GetEnv()
active_indices = robot.GetActiveDOFIndices()
# Create a new trajectory matching the current
# robot's joint configuration specification
cspec = robot.GetActiveConfigurationSpecification('linear')
cspec.AddDeltaTimeGroup()
cspec.ResetGroupOffsets()
path = RaveCreateTrajectory(env, '')
path.Init(cspec)
time_start = time.time()
def fn_wrapper(t, q):
"""
The integrator will try to solve this equation
at each time step.
Note: t is the integration time and is non-monotonic.
"""
# Set the joint values, without checking the joint limits
robot.SetActiveDOFValues(q, CheckLimitsAction.Nothing)
return fn_vectorfield()
def fn_status_callback(t, q):
"""
Check joint-limits and collisions for a specific joint
configuration. This is called multiple times at DOF
resolution in order to check along the entire length of the
trajectory.
Note: This is called by fn_callback, which is currently
called after each integration time step, which means we are
doing more checks than required.
"""
if time.time() - time_start >= timelimit:
raise TimeLimitError()
# Check joint position limits.
# We do this before setting the joint angles.
util.CheckJointLimits(robot, q)
robot.SetActiveDOFValues(q)
# Check collision (throws an exception on collision)
robot_checker.VerifyCollisionFree()
# Check the termination condition.
status = fn_terminate()
if Status.DoesCache(status):
nonlocals['t_cache'] = t
if Status.DoesTerminate(status):
raise TerminationError()
def fn_callback(t, q):
"""
This is called at every successful integration step.
"""
try:
# Add the waypoint to the trajectory.
waypoint = numpy.zeros(cspec.GetDOF())
cspec.InsertDeltaTime(waypoint, t - path.GetDuration())
cspec.InsertJointValues(waypoint, q, robot, active_indices, 0)
path.Insert(path.GetNumWaypoints(), waypoint)
# Run constraint checks at DOF resolution.
if path.GetNumWaypoints() == 1:
checks = [(t, q)]
else:
# This returns collision checks for the entire trajectory,
# including the part that has already been checked. These
# duplicate checks will be filtered out below.
checks = GetLinearCollisionCheckPts(robot, path,
norm_order=norm_order,
sampling_func=sampling_func)
for t_check, q_check in checks:
# TODO: It would be more efficient to only generate checks
# for the new part of the trajectory.
if t_check <= nonlocals['t_check']:
continue
# Record the time of this check so we continue checking
# from where we left off. We do this first, just in case
# fn_status_callback raises an exception.
nonlocals['t_check'] = t_check
fn_status_callback(t_check, q_check)
return 0 # Keep going.
except PlanningError as e:
nonlocals['exception'] = e
return -1 # Stop.
with self.robot_checker_factory(robot) as robot_checker, \
robot.CreateRobotStateSaver(Robot.SaveParameters.LinkTransformation):
# Integrate the vector field to get a configuration space path.
#
# TODO: Tune the integrator parameters.
#
# Integrator: 'dopri5'
# DOPRI (Dormand & Prince 1980) is an explicit method for solving ODEs.
# It is a member of the Runge-Kutta family of solvers.
integrator = scipy.integrate.ode(f=fn_wrapper)
integrator.set_integrator(name='dopri5',
first_step=0.1,
atol=1e-3,
rtol=1e-3)
# Set function to be called at every successful integration step.
integrator.set_solout(fn_callback)
integrator.set_initial_value(y=robot.GetActiveDOFValues(), t=0.)
integrator.integrate(t=integration_time_interval)
t_cache = nonlocals['t_cache']
exception = nonlocals['exception']
if t_cache is None:
raise exception or PlanningError(
'An unknown error has occurred.', deterministic=True)
elif exception:
logger.warning('Terminated early: %s', str(exception))
# Remove any parts of the trajectory that are not cached. This also
# strips the (potentially infeasible) timing information.
output_cspec = robot.GetActiveConfigurationSpecification('linear')
output_path = RaveCreateTrajectory(env, '')
output_path.Init(output_cspec)
# Add all waypoints before the last integration step. GetWaypoints does
# not include the upper bound, so this is safe.
cached_index = path.GetFirstWaypointIndexAfterTime(t_cache)
output_path.Insert(0, path.GetWaypoints(0, cached_index), cspec)
# Add a segment for the feasible part of the last integration step.
output_path.Insert(output_path.GetNumWaypoints(),
path.Sample(t_cache),
cspec)
util.SetTrajectoryTags(output_path, {
Tags.SMOOTH: True,
Tags.CONSTRAINED: True,
Tags.DETERMINISTIC_TRAJECTORY: True,
Tags.DETERMINISTIC_ENDPOINT: True,
}, append=True)
return output_path
def MoveUntilTouch(manipulator, direction, distance, max_distance=None,
max_force=5.0, max_torque=None, ignore_collisions=None,
velocity_limit_scale=0.25, **kw_args):
"""Execute a straight move-until-touch action.
This action stops when a sufficient force is is felt or the manipulator
moves the maximum distance. The motion is considered successful if the
end-effector moves at least distance. In simulation, a move-until-touch
action proceeds until the end-effector collids with the environment.
@param direction unit vector for the direction of motion in the world frame
@param distance minimum distance in meters
@param max_distance maximum distance in meters
@param max_force maximum force in Newtons
@param max_torque maximum torque in Newton-Meters
@param ignore_collisions collisions with these objects are ignored when
planning the path, e.g. the object you think you will touch
@param velocity_limit_scale A multiplier to use to scale velocity limits
when executing MoveUntilTouch ( < 1 in most cases).
@param **kw_args planner parameters
@return felt_force flag indicating whether we felt a force.
"""
from contextlib import nested
from openravepy import CollisionReport, KinBody, Robot, RaveCreateTrajectory
from prpy.planning.exceptions import CollisionPlanningError
delta_t = 0.01
robot = manipulator.GetRobot()
env = robot.GetEnv()
dof_indices = manipulator.GetArmIndices()
direction = numpy.array(direction, dtype='float')
# Default argument values.
if max_distance is None:
max_distance = 1.
warnings.warn(
'MoveUntilTouch now requires the "max_distance" argument.'
' This will be an error in the future.',
DeprecationWarning)
if max_torque is None:
max_torque = numpy.array([100.0, 100.0, 100.0])
if ignore_collisions is None:
ignore_collisions = []
with env:
# Compute the expected force direction in the hand frame.
hand_pose = manipulator.GetEndEffectorTransform()
force_direction = numpy.dot(hand_pose[0:3, 0:3].T, -direction)
# Disable the KinBodies listed in ignore_collisions. We backup the
# "enabled" state of all KinBodies so we can restore them later.
body_savers = [
body.CreateKinBodyStateSaver() for body in ignore_collisions]
robot_saver = robot.CreateRobotStateSaver(
Robot.SaveParameters.ActiveDOF
| Robot.SaveParameters.ActiveManipulator
| Robot.SaveParameters.LinkTransformation)
with robot_saver, nested(*body_savers) as f:
manipulator.SetActive()
robot_cspec = robot.GetActiveConfigurationSpecification()
for body in ignore_collisions:
body.Enable(False)
path = robot.PlanToEndEffectorOffset(direction=direction,
distance=distance, max_distance=max_distance, **kw_args)
# Execute on the real robot by tagging the trajectory with options that
# tell the controller to stop on force/torque input.
if not manipulator.simulated:
raise NotImplementedError('MoveUntilTouch not yet implemented under ros_control.')
# Forward-simulate the motion until it hits an object.
else:
traj = robot.PostProcessPath(path)
is_collision = False
traj_cspec = traj.GetConfigurationSpecification()
new_traj = RaveCreateTrajectory(env, '')
new_traj.Init(traj_cspec)
robot_saver = robot.CreateRobotStateSaver(
Robot.SaveParameters.LinkTransformation)
with env, robot_saver:
for t in numpy.arange(0, traj.GetDuration(), delta_t):
waypoint = traj.Sample(t)
dof_values = robot_cspec.ExtractJointValues(
waypoint, robot, dof_indices, 0)
manipulator.SetDOFValues(dof_values)
# Terminate if we detect collision with the environment.
report = CollisionReport()
if env.CheckCollision(robot, report=report):
logger.info('Terminated from collision: %s',
str(CollisionPlanningError.FromReport(report)))
is_collision = True
break
elif robot.CheckSelfCollision(report=report):
logger.info('Terminated from self-collision: %s',
str(CollisionPlanningError.FromReport(report)))
is_collision = True
break
# Build the output trajectory that stops in contact.
if new_traj.GetNumWaypoints() == 0:
traj_cspec.InsertDeltaTime(waypoint, 0.)
else:
traj_cspec.InsertDeltaTime(waypoint, delta_t)
new_traj.Insert(new_traj.GetNumWaypoints(), waypoint)
if new_traj.GetNumWaypoints() > 0:
robot.ExecuteTrajectory(new_traj)
return is_collision
def _Plan(self,
robot,
goal=None,
tsrchains=None,
timelimit=None,
ompl_args=None,
formatted_extra_params=None,
timeout=None,
**kw_args):
extraParams = ''
# Handle the 'timelimit' parameter.
if timeout is not None:
warnings.warn('"timeout" has been replaced by "timelimit".',
DeprecationWarning)
timelimit = timeout
if timelimit is None:
timelimit = self.default_timelimit
if timelimit <= 0.:
raise ValueError('"timelimit" must be positive.')
extraParams += '<time_limit>{:f}</time_limit>'.format(timelimit)
env = robot.GetEnv()
planner_name = 'OMPL_{:s}'.format(self.algorithm)
planner = RaveCreatePlanner(env, planner_name)
if planner is None:
raise UnsupportedPlanningError(
'Unable to create "{:s}" planner. Is or_ompl installed?'.
format(planner_name))
# Handle other parameters that get passed directly to OMPL.
if ompl_args is None:
ompl_args = dict()
merged_ompl_args = deepcopy(self.default_ompl_args)
merged_ompl_args.update(ompl_args)
for key, value in merged_ompl_args.iteritems():
extraParams += '<{k:s}>{v:s}</{k:s}>'.format(k=str(key),
v=str(value))
# Serialize TSRs into the space-delimited format used by CBiRRT.
if tsrchains is not None:
for chain in tsrchains:
if chain.sample_start:
raise UnsupportedPlanningError(
'OMPL does not support start TSRs.')
# Goal TSRs are parsed using the space delimited CBiRRT format.
if chain.sample_goal:
extraParams += '<{k:s}>{v:s}</{k:s}>'.format(
k='tsr_chain', v=SerializeTSRChain(chain))
# Constraint TSRs are parsed by pr_constraint_or_ompl, which
# uses a JSON format. This differs from the space delimited
# format used by CBiRRT.
if chain.constrain:
if self.supports_constraints:
extraParams += '<{k:s}>{v:s}</{k:s}>'.format(
k='tsr_chain_constraint', v=chain.to_json())
else:
raise UnsupportedPlanningError(
'The "{:s}" OMPL planner does not support TSR'
' constraints.'.format(self.algorithm))
# Enable baked collision checking. This is handled natively.
if self._is_baked and merged_ompl_args.get('do_baked', True):
extraParams += '<do_baked>1</do_baked>'
# Serialize formatted values last, in case of any overrides.
if formatted_extra_params is not None:
extraParams += formatted_extra_params
# Setup the planning query.
params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
if goal is not None:
params.SetGoalConfig(goal)
params.SetExtraParameters(extraParams)
planner.InitPlan(robot, params)
# Bypass the context manager since or_ompl does its own baking.
env = robot.GetEnv()
robot_checker = self.robot_checker_factory(robot)
options = robot_checker.collision_options
with CollisionOptionsStateSaver(env.GetCollisionChecker(), options), \
robot.CreateRobotStateSaver(Robot.SaveParameters.LinkTransformation):
traj = RaveCreateTrajectory(env, 'GenericTrajectory')
status = planner.PlanPath(traj, releasegil=True)
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError('Planner returned with status {:s}.'.format(
str(status)),
deterministic=False)
# Tag the trajectory as non-determistic since most OMPL planners are
# randomized. Additionally tag the goal as non-deterministic if OMPL
# chose from a set of more than one goal configuration.
SetTrajectoryTags(traj, {
Tags.DETERMINISTIC_TRAJECTORY: False,
Tags.DETERMINISTIC_ENDPOINT: tsrchains is None,
},
append=True)
return traj
def new_traj():
return RaveCreateTrajectory(get_env(), '')
def _deserialize_internal(env, data, data_type):
from numpy import array, ndarray
from openravepy import (Environment, KinBody, Robot, Trajectory,
RaveCreateTrajectory)
from prpy.tsr import TSR, TSRChain
from .exceptions import UnsupportedTypeDeserializationException
if data_type == dict.__name__:
return {
deserialize(env, k): deserialize(env, v)
for k, v in data.iteritems()
if k != TYPE_KEY
}
elif data_type == ndarray.__name__:
return array(data['data'])
elif data_type in [ KinBody.__name__, Robot.__name__ ]:
body = env.GetKinBody(data['name'])
if body is None:
raise ValueError('There is no body with name "{:s}".'.format(
data['name']))
return body
elif data_type == KinBody.Link.__name__:
body = env.GetKinBody(data['parent_name'])
if body is None:
raise ValueError('There is no body with name "{:s}".'.format(
data['parent_name']))
link = body.GetLink(data['name'])
if link is None:
raise ValueError('Body "{:s}" has no link named "{:s}".'.format(
data['parent_name'], data['name']))
return link
elif data_type == KinBody.Joint.__name__:
body = env.GetKinBody(data['parent_name'])
if body is None:
raise ValueError('There is no body with name "{:s}".'.format(
data['parent_name']))
joint = body.GetJoint(data['name'])
if joint is None:
raise ValueError('Body "{:s}" has no joint named "{:s}".'.format(
data['parent_name'], data['name']))
return joint
elif data_type == Robot.Manipulator.__name__:
body = env.GetKinBody(data['parent_name'])
if body is None:
raise ValueError('There is no robot with name "{:s}".'.format(
data['parent_name']))
elif not body.IsRobot():
raise ValueError('Body "{:s}" is not a robot.'.format(
data['parent_name']))
manip = body.GetJoint(data['name'])
if manip is None:
raise ValueError('Robot "{:s}" has no manipulator named "{:s}".'.format(
data['parent_name'], data['name']))
return manip
elif data_type == Trajectory.__name__:
traj = RaveCreateTrajectory(env, '')
traj.deserialize(data['data'])
return traj
elif data_type == TSR.__name__:
return TSR.from_dict(data['data'])
elif data_type == TSRChain.__name__:
return TSRChain.from_dict(data['data'])
else:
raise UnsupportedTypeDeserializationException(data_type)
def FollowVectorField(self,
robot,
fn_vectorfield,
fn_terminate,
integration_time_interval=10.0,
timelimit=5.0,
**kw_args):
"""
Follow a joint space vectorfield to termination.
@param robot
@param fn_vectorfield a vectorfield of joint velocities
@param fn_terminate custom termination condition
@param integration_time_interval The time interval to integrate
over.
@param timelimit time limit before giving up
@param kw_args keyword arguments to be passed to fn_vectorfield
@return traj
"""
from .exceptions import (
CollisionPlanningError,
SelfCollisionPlanningError,
)
from openravepy import CollisionReport, RaveCreateTrajectory
from ..util import GetCollisionCheckPts
import time
import scipy.integrate
CheckLimitsAction = openravepy.KinBody.CheckLimitsAction
# This is a workaround to emulate 'nonlocal' in Python 2.
nonlocals = {
'exception': None,
't_cache': None,
't_check': 0.,
}
env = robot.GetEnv()
active_indices = robot.GetActiveDOFIndices()
# Create a new trajectory matching the current
# robot's joint configuration specification
cspec = robot.GetActiveConfigurationSpecification('linear')
cspec.AddDeltaTimeGroup()
cspec.ResetGroupOffsets()
path = RaveCreateTrajectory(env, '')
path.Init(cspec)
time_start = time.time()
def fn_wrapper(t, q):
"""
The integrator will try to solve this equation
at each time step.
Note: t is the integration time and is non-monotonic.
"""
# Set the joint values, without checking the joint limits
robot.SetActiveDOFValues(q, CheckLimitsAction.Nothing)
return fn_vectorfield()
def fn_status_callback(t, q):
"""
Check joint-limits and collisions for a specific joint
configuration. This is called multiple times at DOF
resolution in order to check along the entire length of the
trajectory.
Note: This is called by fn_callback, which is currently
called after each integration time step, which means we are
doing more checks than required.
"""
if time.time() - time_start >= timelimit:
raise TimeLimitError()
# Check joint position limits.
# We do this before setting the joint angles.
util.CheckJointLimits(robot, q)
robot.SetActiveDOFValues(q)
# Check collision.
report = CollisionReport()
if env.CheckCollision(robot, report=report):
raise CollisionPlanningError.FromReport(report)
elif robot.CheckSelfCollision(report=report):
raise SelfCollisionPlanningError.FromReport(report)
# Check the termination condition.
status = fn_terminate()
if Status.DoesCache(status):
nonlocals['t_cache'] = t
if Status.DoesTerminate(status):
raise TerminationError()
def fn_callback(t, q):
"""
This is called at every successful integration step.
"""
try:
# Add the waypoint to the trajectory.
waypoint = numpy.zeros(cspec.GetDOF())
cspec.InsertDeltaTime(waypoint, t - path.GetDuration())
cspec.InsertJointValues(waypoint, q, robot, active_indices, 0)
path.Insert(path.GetNumWaypoints(), waypoint)
# Run constraint checks at DOF resolution.
if path.GetNumWaypoints() == 1:
checks = [(t, q)]
else:
# TODO: This should start at t_check. Unfortunately, a bug
# in GetCollisionCheckPts causes this to enter an infinite
# loop.
checks = GetCollisionCheckPts(robot,
path,
include_start=False)
# start_time=nonlocals['t_check'])
for t_check, q_check in checks:
fn_status_callback(t_check, q_check)
# Record the time of this check so we continue checking at
# DOF resolution the next time the integrator takes a step.
nonlocals['t_check'] = t_check
return 0 # Keep going.
except PlanningError as e:
nonlocals['exception'] = e
return -1 # Stop.
# Integrate the vector field to get a configuration space path.
#
# TODO: Tune the integrator parameters.
#
# Integrator: 'dopri5'
# DOPRI (Dormand & Prince 1980) is an explicit method for solving ODEs.
# It is a member of the Runge-Kutta family of solvers.
integrator = scipy.integrate.ode(f=fn_wrapper)
integrator.set_integrator(name='dopri5',
first_step=0.1,
atol=1e-3,
rtol=1e-3)
# Set function to be called at every successful integration step.
integrator.set_solout(fn_callback)
# Initial conditions
integrator.set_initial_value(y=robot.GetActiveDOFValues(), t=0.)
integrator.integrate(t=integration_time_interval)
t_cache = nonlocals['t_cache']
exception = nonlocals['exception']
if t_cache is None:
raise exception or PlanningError('An unknown error has occurred.')
elif exception:
logger.warning('Terminated early: %s', str(exception))
# Remove any parts of the trajectory that are not cached. This also
# strips the (potentially infeasible) timing information.
output_cspec = robot.GetActiveConfigurationSpecification('linear')
output_path = RaveCreateTrajectory(env, '')
output_path.Init(output_cspec)
# Add all waypoints before the last integration step. GetWaypoints does
# not include the upper bound, so this is safe.
cached_index = path.GetFirstWaypointIndexAfterTime(t_cache)
output_path.Insert(0, path.GetWaypoints(0, cached_index), cspec)
# Add a segment for the feasible part of the last integration step.
output_path.Insert(output_path.GetNumWaypoints(), path.Sample(t_cache),
cspec)
# Flag this trajectory as constrained.
util.SetTrajectoryTags(output_path, {
Tags.CONSTRAINED: 'true',
Tags.SMOOTH: 'true'
},
append=True)
return output_path