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 ...')
