def __init__(self, env, robotname):
"""
Create a CBiRRT Problem Instance
:param env: rave.Environment
:param robotname: str
:return: CBiRRT
"""
self.env = env
self.problem = rave.RaveCreateProblem(env, 'CBiRRT')
env.LoadProblem(self.problem, robotname)
# Build a mapping for manipulators to manipulator indices
self.manip_indices = {}
for index, manip in enumerate(
env.GetRobot(robotname).GetManipulators()):
self.manip_indices[manip] = index
self.manip_indices[manip.GetName()] = index
def setupEnv(self, env):
self.env = env
try:
self.problem = openravepy.RaveCreateProblem(self.env, 'SBPL')
except openravepy.openrave_exception:
raise UnsupportedPlanningError('Unable to create SBPL module.')
def createProblem(robot):
env = robot.GetEnv()
problem = _rave.RaveCreateProblem(env, 'CBiRRT')
env.LoadProblem(problem, robot.GetName())
return problem
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
import openhubo
import openravepy as rave
import time
if __name__=='__main__':
(env,options)=openhubo.setup('qtcoin')
[robot,ind,__,__,__]=openhubo.load_scene(env,options)
#Read in trajectory from matlab source
traj=trajectory.read_text_traj('trajectories/ingress_test.traj.txt',robot,0.01,1.0)
config=traj.GetConfigurationSpecification()
Getvals=trajectory.makeJointValueExtractor(robot,traj,config)
Gettrans=trajectory.makeTransformExtractor(robot,traj,config)
probs_cbirrt = rave.RaveCreateProblem(env,'CBiRRT')
env.LoadProblem(probs_cbirrt,'rlhuboplus')
i=0
t0=time.time()
for i in range(traj.GetNumWaypoints()):
robot.SetTransformWithDOFValues(Gettrans(i),Getvals(i))
#print openhubo.find_com(robot)
datastring=probs_cbirrt.SendCommand('CheckSupport supportlinks 2 rightFoot leftFoot exact 1 ')
datalist=datastring.split(' ')
check=float(datalist[0])
mass=float(datalist[1])
com=[float(x) for x in datalist[2:4]]
pointdata=[float(x) for x in datalist[5:-1]]
#print Gettrans(i)
time.sleep(.01)
#print robot.GetJoint('LAP').GetValues()
def __init__(self):
super(CBiRRTPlanner, self).__init__()
self.problem = openravepy.RaveCreateProblem(self.env, 'CBiRRT')
if self.problem is None:
raise UnsupportedPlanningError('Unable to create CBiRRT module.')