def load_scene(env,robotfile=None,scenefile=None,stop=None,physics=True,ghost=False,options=None):
""" Load files and configure the simulation environment based on arguments and the options structure.
The returned tuple contains:
:robot: handle to the created robot
:ctrl: either trajectorycontroller or idealcontroller depending on physics
:ind: name-to-joint-index converter
:ref: handle to visualization "ghost" robot
:recorder: video recorder python class for quick video dumps
"""
if not (type(robotfile) is list or type(robotfile) is str):
_rave.raveLogWarn("Assuming 2nd argument is options structure...")
options=robotfile
else:
(options,__)=get_options()
options.robotfile=robotfile
options.scenefile=scenefile
options.stop=stop
if physics:
options._physics='ode'
options.physicsfile='physics.xml'
options.ghost=ghost
return load_scene_from_options(env,options)
def load_scene(env,
robotfile=None,
scenefile=None,
stop=None,
physics=True,
ghost=False,
options=None):
""" Load files and configure the simulation environment based on arguments and the options structure.
The returned tuple contains:
:robot: handle to the created robot
:ctrl: either trajectorycontroller or idealcontroller depending on physics
:ind: name-to-joint-index converter
:ref: handle to visualization "ghost" robot
:recorder: video recorder python class for quick video dumps
"""
if not (type(robotfile) is list or type(robotfile) is str):
_rave.raveLogWarn("Assuming 2nd argument is options structure...")
options = robotfile
else:
(options, __) = get_options()
options.robotfile = robotfile
options.scenefile = scenefile
options.stop = stop
if physics:
options._physics = 'ode'
options.physicsfile = 'physics.xml'
options.ghost = ghost
return load_scene_from_options(env, options)
def PickerThread(env,pickers,iktype):
"""this function runs in a separate thread monitoring each of the pickers
"""
Tpickers = [None]*len(pickers)
while True:
if env.GetViewer() is None:
break
with env:
for ipicker,(picker,manip) in enumerate(pickers):
T=picker.GetTransform()
if Tpickers[ipicker] is not None and sum(abs(Tpickers[ipicker]-T).flatten()) <= 1e-10:
continue
data = None
if iktype == IkParameterizationType.Direction3D:
data = T[0:3,2]
elif iktype == IkParameterizationType.Lookat3D:
data = T[0:3,3]
elif iktype == IkParameterizationType.Ray4D:
data = Ray(T[0:3,3],T[0:3,2])
elif iktype == IkParameterizationType.Rotation3D:
data = T[0:3,0:3]
elif iktype == IkParameterizationType.Transform6D:
data = T
elif iktype == IkParameterizationType.Translation3D:
data = T[0:3,3]
elif iktype == IkParameterizationType.TranslationDirection5D:
ikparam = Ray(T[0:3,3],T[0:3,2])
elif iktype == IkParameterizationType.TranslationLocalGlobal6D:
ikparam = [T[0:3,3],zeros(3)]
else:
raveLogWarn('iktype %s unsupported'%str(iktype))
continue
sol = manip.FindIKSolution(IkParameterization(data,iktype),IkFilterOptions.CheckEnvCollisions)
if sol is not None:
robot.SetDOFValues(sol,manip.GetArmIndices())
# have to update all other pickers since two manipulators can be connected to the same joints (torso)
for ipicker2, (picker2,manip2) in enumerate(pickers):
Tpickers[ipicker2] = manip2.GetTransform()
picker2.SetTransform(manip2.GetTransform())
# changing color produces bugs with qtcoin
# for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
# color = zeros(3)
# color[igeom] = 0.4 if sol is None else 1.0
# geom.SetDiffuseColor(color)
Tpickers[ipicker] = T
# update rate of 0.05s
time.sleep(0.05)
def main(env,options):
"Main example code."
if options.iktype is not None:
# cannot use .names due to python 2.5 (or is it boost version?)
for value,type in IkParameterization.Type.values.iteritems():
if type.name.lower() == options.iktype.lower():
iktype = type
break
else:
iktype = IkParameterizationType.Transform6D
env.Load(options.scene)
with env:
# load the Transform6D IK models
ikmodels = []
for robot in env.GetRobots():
for manip in robot.GetManipulators():
print manip
try:
robot.SetActiveManipulator(manip)
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=iktype)
if not ikmodel.load():
ikmodel.autogenerate()
if not ikmodel.has():
continue
ikmodels.append(ikmodel)
except Exception, e:
print 'failed manip %s'%manip, e
if len(ikmodels) == 0:
raveLogWarn('no manipulators found that can be loaded with iktype %s'%str(iktype))
# create the pickers
pickers = []
for ikmodel in ikmodels:
raveLogInfo('creating picker for %s'%str(ikmodel.manip))
picker = RaveCreateKinBody(env,'')
picker.InitFromBoxes(array([ [0.05,0,0,0.05,0.01,0.01], [0,0.05,0,0.01,0.05,0.01], [0,0,0.05,0.01,0.01,0.05] ]),True)
for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
color = zeros(3)
color[igeom] = 1
geom.SetDiffuseColor(color)
picker.SetName(ikmodel.manip.GetName())
env.Add(picker,True)
# have to disable since not part of collision
picker.Enable(False)
picker.SetTransform(ikmodel.manip.GetTransform())
pickers.append([picker,ikmodel.manip])
def PickerThread(env,pickers,iktype):
"""this function runs in a separate thread monitoring each of the pickers
"""
Tpickers = [None]*len(pickers)
while True:
if env.GetViewer() is None:
break
with env:
for ipicker,(picker,manip) in enumerate(pickers):
T=picker.GetTransform()
if Tpickers[ipicker] is not None and sum(abs(Tpickers[ipicker]-T).flatten()) <= 1e-10:
continue
data = None
if iktype == IkParameterizationType.Direction3D:
data = T[0:3,2]
elif iktype == IkParameterizationType.Lookat3D:
data = T[0:3,3]
elif iktype == IkParameterizationType.Ray4D:
data = Ray(T[0:3,3],T[0:3,2])
elif iktype == IkParameterizationType.Rotation3D:
data = T[0:3,0:3]
elif iktype == IkParameterizationType.Transform6D:
data = T
elif iktype == IkParameterizationType.Translation3D:
data = T[0:3,3]
elif iktype == IkParameterizationType.TranslationDirection5D:
ikparam = Ray(T[0:3,3],T[0:3,2])
elif iktype == IkParameterizationType.TranslationLocalGlobal6D:
ikparam = [T[0:3,3],zeros(3)]
else:
raveLogWarn('iktype %s unsupported'%str(iktype))
continue
sol = manip.FindIKSolution(IkParameterization(data,iktype),IkFilterOptions.CheckEnvCollisions)
if sol is not None:
robot.SetDOFValues(sol,manip.GetArmIndices())
# have to update all other pickers since two manipulators can be connected to the same joints (torso)
for ipicker2, (picker2,manip2) in enumerate(pickers):
Tpickers[ipicker2] = manip2.GetTransform()
picker2.SetTransform(manip2.GetTransform())
# changing color produces bugs with qtcoin
# for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
# color = zeros(3)
# color[igeom] = 0.4 if sol is None else 1.0
# geom.SetDiffuseColor(color)
Tpickers[ipicker] = T
# update rate of 0.05s
time.sleep(0.05)
def get_setcache_path(family, roadmap, read_only):
# get family id
family_id = family.SendCommand('GetFamilyId')
# get roadmap id
roadmap_id = prpy_lemur.roadmaps.get_roadmap_id(roadmap)
setcache_filename = 'prpy_lemur/lemur_selfcached/family-{}-roadmap-{}.txt'.format(family_id,roadmap_id)
setcache_path = openravepy.RaveFindDatabaseFile(setcache_filename, read_only) # bRead
if setcache_path == '':
if read_only:
openravepy.raveLogWarn('Self cache database file not found:')
openravepy.raveLogWarn('{}'.format(setcache_filename))
else:
raise RuntimeError('cant save to openrave database!')
return setcache_path
def DoGeneralIK(self, execute=None, gettime=None, norot=None, returnclosest=None, robottm=None, maniptm=None,
movecog=None, supportlinks=None, polyscale=None, polytrans=None, support=None, gravity=None,
printcommand=False):
"""
Get an IK solution with GeneralIK. In addition to these parameters, you can specify which DOF GeneralIK will
use when planning by changing the active DOF
:param bool execute: If True, GeneralIK will set the robot's configuration to the IK solution it found
(optional, defaults to False)
:param bool gettime: If True, this function will return a 2-tuple of the result and the time in seconds that was
spent in the IK solver (optional, defaults to False)
:param bool norot: If True, GeneralIK will ignore the rotation components of the goal transforms (optional,
defaults to False)
:param bool returnclosest: If True, on failure GeneralIK will return the closest configuration it was able to
achieve. If this is True there is no feedback about whether the solver succeeded. (optional,
defaults to False)
:param numpy.array robottm: Transform matrix representing the desired position and orientation of the base link
relative to the world frame (optional)
:param list[(str|rave.Manipulator, numpy.array)] maniptm: List of tuples specifying a manipulator, by name
or Manipulator object, and its desired transform relative to the world frame. Any manipulator that
does not appear in this list but is in the active DOF list may be moved to avoid obstacles or keep
the robot balanced.
:param list[float] movecog: The desired center of gravity of the robot as an [x, y, z] list. Support polygon
stability will not be performed if this parameter is not provided (optional)
:param list[string|rave.KinBody.Link] supportlinks: Links to use to calculate the robot's support polygon,
specified either as the link name or link object. Setting this parameter enables balance checking
using the robot's support polygon. (optional)
:param list[float] polyscale: [x, y, z] values that scale the support polygon. Z is ignored and may be ommitted.
(optional, defaults to [1, 1, 1])
:param list[float] polytrans: [x, y, z] values that translate the support polygon. Z is ignored and may be
ommitted. (optional, defaults to [0, 0, 0])
:param list[(str|rave.Manipulator, float)] support: Manipulators to use when calculating support using the
GIWC (Gravito-Inertial Wrench Cone). Each should be specified as a 2-tuple of the Manipulator object
or manipulator's name and the coefficient of friction to use for that manipulator. Specifying this
parameter enables GIWC support checking, and it should not be specified with the supportlinks
parameter.
:param list[float] gravity: The [x, y, z] gravity vector to use when calculating support using GIWC (optional,
defaults to [0, 0, -9.8])
:param bool printcommand: If this True, prints the resulting command string immediately before calling
GeneralIK, meant for debuggging (optional, defaults to False)
:return list[float]|None|(list[float]|None, float): If gettime is False, the solution of the IK solver as a list
of joint angles, in the order specified by the active DOF list, or None if no solution was found.
If gettime is True, a 2-tuple of the solution or None and the time spent in the planner, measured
in seconds.
"""
cmd = ["DoGeneralIK"]
""":type : list[int | float | str]"""
if execute is not None:
cmd.append("exec")
if gettime is not None:
cmd.append("gettime")
if norot is not None:
cmd.append("norot")
if returnclosest is not None:
cmd.append("returnclosest")
if robottm is not None:
cmd.append("robottm")
cmd.append(SerializeTransform(robottm))
if maniptm is not None:
cmd.append("nummanips")
cmd.append(len(maniptm))
for manip, tm in maniptm:
cmd.append("maniptm")
cmd.append(self.manip_indices[manip])
cmd.append(SerializeTransform(tm))
if movecog is not None:
cmd.append("movecog")
# Enumerating manually to ensure you can pass in any indexable object and get a movecog vector of 3 items
cmd.append(movecog[0])
cmd.append(movecog[1])
cmd.append(movecog[2])
if supportlinks is not None and support is not None:
rave.raveLogWarn("Supportlinks and support should be mutually exclusive")
if supportlinks is not None:
cmd.append("supportlinks")
cmd.append(len(supportlinks))
cmd.extend(rave_object_name(l) for l in supportlinks)
if polyscale is not None:
if supportlinks is None:
rave.raveLogWarn("Polyscale has no effect when supportlinks is not provided")
cmd.append("polyscale")
cmd.extend(polyscale)
if len(polyscale) == 2:
cmd.append(0)
elif len(polyscale) != 3:
raise ValueError("Polyscale must have either 2 or 3 values")
if polytrans is not None:
if supportlinks is None:
rave.raveLogWarn("Polytrans has no effect when supportlinks is not provided")
cmd.append("polytrans")
cmd.extend(polytrans)
if len(polytrans) == 2:
cmd.append(0)
elif len(polytrans) != 3:
raise ValueError("Polytrans must have either 2 or 3 values")
if support is not None:
for manip, mu in support:
cmd.append("support {} {}".format(rave_object_name(manip), mu))
if gravity is not None:
cmd.append("gravity")
# Enumerating manually to ensure you can pass in any indexable object and get a gravity vector of 3 items
cmd.append(gravity[0])
cmd.append(gravity[1])
cmd.append(gravity[2])
cmd_str = " ".join(str(item) for item in cmd)
if printcommand:
print(cmd_str)
result_str = self.problem.SendCommand(cmd_str)
result = [float(x) for x in result_str.split()]
if gettime:
# Convert the time from ms to seconds to match what is expected in Python
time = result.pop() / 1000.0
if not result:
return time, None
else:
return time, result
else:
if not result:
return None
else:
return result
def RunElasticStrips(self,
manips=None,
trajectory=None,
gettime=None,
contact_manips=None,
desiredmanippose=None,
checkcollisionlink=None,
selfcollisionlinkpair=None,
obstacles=None,
posturecontrol=None,
supportlinks=None,
polyscale=None,
polytrans=None,
support=None,
gravity=None,
printcommand=False):
"""
Get an IK solution with GeneralIK. In addition to these parameters, you can specify which DOF GeneralIK will
use when planning by changing the active DOF
:param bool execute: If True, GeneralIK will set the robot's configuration to the IK solution it found
(optional, defaults to False)
:param bool gettime: If True, this function will return a 2-tuple of the result and the time in seconds that was
spent in the IK solver (optional, defaults to False)
:param bool norot: If True, GeneralIK will ignore the rotation components of the goal transforms (optional,
defaults to False)
:param bool returnclosest: If True, on failure GeneralIK will return the closest configuration it was able to
achieve. If this is True there is no feedback about whether the solver succeeded. (optional,
defaults to False)
:param numpy.array robottm: Transform matrix representing the desired position and orientation of the base link
relative to the world frame (optional)
:param list[(str|rave.Manipulator, numpy.array)] maniptm: List of tuples specifying a manipulator, by name
or Manipulator object, and its desired transform relative to the world frame. Any manipulator that
does not appear in this list but is in the active DOF list may be moved to avoid obstacles or keep
the robot balanced.
:param list[float] movecog: The desired center of gravity of the robot as an [x, y, z] list. Support polygon
stability will not be performed if this parameter is not provided (optional)
:param list[str] obstacles: The KinBody name that are taken as obstacle in the GeneralIK solver
:param list[string|rave.KinBody.Link] supportlinks: Links to use to calculate the robot's support polygon,
specified either as the link name or link object. Setting this parameter enables balance checking
using the robot's support polygon. (optional)
:param list[float] polyscale: [x, y, z] values that scale the support polygon. Z is ignored and may be ommitted.
(optional, defaults to [1, 1, 1])
:param list[float] polytrans: [x, y, z] values that translate the support polygon. Z is ignored and may be
ommitted. (optional, defaults to [0, 0, 0])
:param list[(str|rave.Manipulator, float)] support: Manipulators to use when calculating support using the
GIWC (Gravito-Inertial Wrench Cone). Each should be specified as a 2-tuple of the Manipulator object
or manipulator's name and the coefficient of friction to use for that manipulator. Specifying this
parameter enables GIWC support checking, and it should not be specified with the supportlinks
parameter.
:param list[float] gravity: The [x, y, z] gravity vector to use when calculating support using GIWC (optional,
defaults to [0, 0, -9.8])
:param bool printcommand: If this True, prints the resulting command string immediately before calling
GeneralIK, meant for debuggging (optional, defaults to False)
:return list[float]|None|(list[float]|None, float): If gettime is False, the solution of the IK solver as a list
of joint angles, in the order specified by the active DOF list, or None if no solution was found.
If gettime is True, a 2-tuple of the solution or None and the time spent in the planner, measured
in seconds.
"""
cmd = ["RunElasticStrips"]
""":type : list[int | float | str]"""
cmd.append(self.robotname)
if manips is not None:
cmd.append("nummanips")
cmd.append(len(manips))
cmd.append("manips")
for m in manips:
cmd.append(self.manip_indices[m])
if trajectory is not None:
cmd.append("trajectory")
cmd.append(len(trajectory))
for waypoint in trajectory:
for dim in waypoint:
cmd.append(
dim) # dim includes the index and joint angle value
if gettime is not None:
cmd.append("gettime")
if contact_manips is not None:
cmd.append("contact_manips")
cmd.append(len(contact_manips))
for i, manips in enumerate(contact_manips):
cmd.append(i)
cmd.append(len(manips))
for manip in manips:
cmd.append(manip[0])
cmd.append(manip[1])
if desiredmanippose is not None:
cmd.append("desiredmanippose")
cmd.append(len(desiredmanippose))
for waypoints in desiredmanippose:
cmd.append(waypoints[0][0]) # index
cmd.append(len(waypoints))
for wp in waypoints:
cmd.append(wp[1])
cmd.append(wp[2])
cmd.append(SerializeTransform(wp[3]))
if checkcollisionlink is not None:
cmd.append("checkcollision")
cmd.append(len(checkcollisionlink))
cmd.extend(rave_object_name(l) for l in checkcollisionlink)
if selfcollisionlinkpair is not None:
cmd.append("checkselfcollision")
cmd.append(len(selfcollisionlinkpair))
for l in selfcollisionlinkpair:
cmd.extend([l[0], l[1]])
if obstacles is not None:
cmd.append("obstacles")
cmd.append(len(obstacles))
for obs in obstacles:
cmd.append(rave_object_name(obs[0]))
cmd.append(obs[1][0])
cmd.append(obs[1][1])
cmd.append(obs[1][2])
# cmd.extend(rave_object_name(l) for l in obstacles)
if posturecontrol is not None:
cmd.append("posturecontrol")
cmd.append(len(posturecontrol))
for link, tm in posturecontrol:
cmd.append(link)
cmd.append(SerializeTransform(tm))
if supportlinks is not None and support is not None:
rave.raveLogWarn(
"Supportlinks and support should be mutually exclusive")
if supportlinks is not None:
cmd.append("supportlinks")
cmd.append(len(supportlinks))
cmd.extend(rave_object_name(l) for l in supportlinks)
if polyscale is not None:
if supportlinks is None:
rave.raveLogWarn(
"Polyscale has no effect when supportlinks is not provided"
)
cmd.append("polyscale")
cmd.extend(polyscale)
if len(polyscale) == 2:
cmd.append(0)
elif len(polyscale) != 3:
raise ValueError("Polyscale must have either 2 or 3 values")
if polytrans is not None:
if supportlinks is None:
rave.raveLogWarn(
"Polytrans has no effect when supportlinks is not provided"
)
cmd.append("polytrans")
cmd.extend(polytrans)
if len(polytrans) == 2:
cmd.append(0)
elif len(polytrans) != 3:
raise ValueError("Polytrans must have either 2 or 3 values")
if support is not None:
for manip, mu in support:
cmd.append("support {} {}".format(rave_object_name(manip), mu))
if gravity is not None:
cmd.append("gravity")
# Enumerating manually to ensure you can pass in any indexable object and get a gravity vector of 3 items
cmd.append(gravity[0])
cmd.append(gravity[1])
cmd.append(gravity[2])
cmd_str = " ".join(str(item) for item in cmd)
if printcommand:
print(cmd_str)
result_str = self.module.SendCommand(cmd_str, True)
result = [float(x) for x in result_str.split()]
if gettime:
# Convert the time from ms to seconds to match what is expected in Python
time = result.pop() / 1000.0
if not result:
return time, None
else:
return time, result
else:
if not result:
return None
else:
return result
def main(env,options):
"Main example code."
if options.iktype is not None:
# cannot use .names due to python 2.5 (or is it boost version?)
for value,type in IkParameterization.Type.values.items():
if type.name.lower() == options.iktype.lower():
iktype = type
break
else:
iktype = IkParameterizationType.Transform6D
env.Load(options.scene)
with env:
# load the Transform6D IK models
ikmodels = []
for robot in env.GetRobots():
for manip in robot.GetManipulators():
print(manip)
try:
robot.SetActiveManipulator(manip)
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=iktype)
if not ikmodel.load():
ikmodel.autogenerate()
if not ikmodel.has():
continue
ikmodels.append(ikmodel)
except Exception as e:
print('failed manip %s %r'%(manip, e))
if len(ikmodels) == 0:
raveLogWarn('no manipulators found that can be loaded with iktype %s'%str(iktype))
# create the pickers
pickers = []
for ikmodel in ikmodels:
raveLogInfo('creating picker for %s'%str(ikmodel.manip))
picker = RaveCreateKinBody(env,'')
picker.InitFromBoxes(array([ [0.05,0,0,0.05,0.01,0.01], [0,0.05,0,0.01,0.05,0.01], [0,0,0.05,0.01,0.01,0.05] ]),True)
for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
color = zeros(3)
color[igeom] = 1
geom.SetDiffuseColor(color)
picker.SetName(ikmodel.manip.GetName())
env.Add(picker,True)
# have to disable since not part of collision
picker.Enable(False)
picker.SetTransform(ikmodel.manip.GetTransform())
pickers.append([picker,ikmodel.manip])
raveLogInfo('pickers loaded, try moving them')
def PickerThread(env,pickers,iktype):
"""this function runs in a separate thread monitoring each of the pickers
"""
Tpickers = [None]*len(pickers)
while True:
if env.GetViewer() is None:
break
with env:
for ipicker,(picker,manip) in enumerate(pickers):
T=picker.GetTransform()
if Tpickers[ipicker] is not None and sum(abs(Tpickers[ipicker]-T).flatten()) <= 1e-10:
continue
data = None
if iktype == IkParameterizationType.Direction3D:
data = T[0:3,2]
elif iktype == IkParameterizationType.Lookat3D:
data = T[0:3,3]
elif iktype == IkParameterizationType.Ray4D:
data = Ray(T[0:3,3],T[0:3,2])
elif iktype == IkParameterizationType.Rotation3D:
data = T[0:3,0:3]
elif iktype == IkParameterizationType.Transform6D:
data = T
elif iktype == IkParameterizationType.Translation3D:
data = T[0:3,3]
elif iktype == IkParameterizationType.TranslationDirection5D:
ikparam = Ray(T[0:3,3],T[0:3,2])
elif iktype == IkParameterizationType.TranslationLocalGlobal6D:
ikparam = [T[0:3,3],zeros(3)]
else:
raveLogWarn('iktype %s unsupported'%str(iktype))
continue
sol = manip.FindIKSolution(IkParameterization(data,iktype),IkFilterOptions.CheckEnvCollisions)
if sol is not None:
robot.SetDOFValues(sol,manip.GetArmIndices())
# have to update all other pickers since two manipulators can be connected to the same joints (torso)
for ipicker2, (picker2,manip2) in enumerate(pickers):
Tpickers[ipicker2] = manip2.GetTransform()
picker2.SetTransform(manip2.GetTransform())
# changing color produces bugs with qtcoin
# for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
# color = zeros(3)
# color[igeom] = 0.4 if sol is None else 1.0
# geom.SetDiffuseColor(color)
Tpickers[ipicker] = T
# update rate of 0.05s
time.sleep(0.05)
# create the thread
t = threading.Thread(target=PickerThread,args=[env,pickers,iktype])
t.start()
while True:
t.join(1)
def RunCBiRRT(self,
jointgoals=None,
jointstarts=None,
ikguess=None,
Tattachedik_0=None,
smoothingitrs=None,
filename="/tmp/cmovetraj.txt",
timelimit=None,
TSRChains=None,
supportlinks=None,
polyscale=None,
polytrans=None,
support=None,
gravity=None,
heldobject=None,
psample=None,
bikfastsinglesolution=None,
planinnewthread=None,
allowlimadj=None,
smoothtrajonly=None,
outputtrajobj=False,
exportfulltraj=None,
printcommand=False):
"""
Run the CBiRRT planner.
This command has many parameters which are fully documented below, but here is an overview:
The common use case, planning a path with TSR constraints, usually involves all or most of these parameters:
-- TSRChains, which describe the end effector constraints,
-- psample, which is required in order to sample from goal TSRs
-- outputtrajobj, which returs the planned trajectory
-- supportlinks or support, which do stability checking
-- timelimit, which limits the time taken by CBiRRT
These parameters change certian aspects of CBiRRT's behavior:
-- jointgoals, which sets the goal pose or poses (unclear whether that is instead of or in addition to sampling)
-- jointstarts, which sets the start pose or poses (default behavior is to use the current pose)
-- ikguess, which proivdes an initial guess for the iterative IK solver when creating the initial goal pose.
Setting ikguess can sometimes help if CBiRRT always returns "Unable to find a goal IK solution".
-- smoothingitrs, which controls how much smoothing will be performed. Increasing this value will usually
reduce cases where the robot makes unnecessarily complex motions.
-- bikfastsinglesolution, which is described below
Some sets of parameters are related or mutually exclusive:
-- polyscale and polytrans only have effect if you specify supportlinks
-- gravity only has effect if you specify support
-- supportlinks and support are mutually exclusive (they use different support modes)
-- planinnewthread, smoothtrajonly, and outputtrajobj are all mutually exclusive
-- smoothtrajonly causes every parameter except smoothingitrs and parameters which apply constraints on the path
to be ignored
:param list[float]|list[list[float]] jointgoals: Desired joint value vector at end of planning or list of
desired joint value vectors (optional)
:param list[float]|list[list[float]] jointstarts: Joint value vector at start of planning or list of desired
joint value vectors (optional, defaults to current
configuration)
:param list[float] ikguess: Joint value vector that the itertive IK solver will use as an initial guess when
sampling goals (optional)
:param dict[int, np.array|np.matrix] Tattachedik_0: Transformation matrix between attachedik frame and 0 frame
for each manipulator (optional, defaults to identity)
:param int smoothingitrs: Number of iterations of smoothing to apply to the final trajectory (optional,
defaults to 300)
:param str filename: Filename of generated trajectory file (optional, defaults to "/tmp/cmovetraj.txt")
:param float timelimit: Time limit in seconds before planning gives up (optional, defaults to 25 seconds)
:param list[TSRChain] TSRChains: A list of TSR chains to constrain the path, start, and/or goal (optional)
:param list[string|rave.KinBody.Link] supportlinks: Links to use to calculate the robot's support polygon,
specified either as the link name or link object. Setting this parameter enables balance checking
using the robot's support polygon. (optional)
:param list[float] polyscale: [x, y, z] values that scale the support polygon. Z is ignored and may be ommitted.
(optional, defaults to [1, 1, 1])
:param list[float] polytrans: [x, y, z] values that translate the support polygon. Z is ignored and may be
ommitted. (optional, defaults to [0, 0, 0])
:param list[(str|rave.Manipulator, float, int)] support: Manipulators to use when calculating support using the
GIWC (Gravito-Inertial Wrench Cone). Each should be specified as a 3-tuple of the Manipulator object
or manipulator's name, the coefficient of friction to use for that manipulator, and a bitmask
indicating whether the manipulator is supporting the robot at the start, path, and/or goal. The
CBiRRT.START, CBiRRT.PATH, and CBiRRT.GOAL variables can be used with boolean or to construct this
bitmask. Note that path constraints also apply at the start and goal, so if the goal support is a
superset of the path support it has no effect. Specifying this parameter enables GIWC support
checking, and it should not be specified with the supportlinks parameter.
:param list[float] gravity: The [x, y, z] gravity vector to use when calculating support using GIWC (optional,
defaults to [0, 0, -9.8])
:param str|rave.KinBody heldobject: A KinBody that robot is holding, specified as the KinBody object or name.
This body will be considered when calculating whether the robot is balanced. Only one is supported.
(optional)
:param float psample: Probability of sampling from the TSR chains rather than growing the RRT trees on each
iteration. Defaults to 0, which is usually not desired behavior. This module will output a warning
if psample is not specified and TSRs are provided. To silence this warning, specifically set psample
to 0. (optional, defaults to 0)
:param boolean bikfastsinglesolution: Controls the use of attached IK solvers (such as ikfast) when sampling
start and goal configurations. If this is true, CBiRRT will get a single IK solution with collision
checking enabled in the IK solver. If fasle, it will get multiple IK solutoins with collision
checking disabled, and perform collision checking on them later. (optional, defaults to True)
:param boolean planinnewthread: Whether to run the planner in a new thread. If this is true then RunCBiRRT
returns quickly and GetPlannerState and StopPlanner must be used to interact with the planner.
(optional, defaults to False)
:param boolean allowlimadj: Allow CBiRRT to temporarily adjust joint limits up or down so that the start
configuration is always within joint limits (optional, defaults to False)
:param str smoothtrajonly: If this parameter is provided, CBiRRT just performs trajectory smoothing on the
trajectory file. The value of this argument is the filename of the trajectory to smooth (optional)
:param bool outputtrajobj: If outputtrajobj is True then this method returns a 2-tuple of CBiRRT's output and
the resulting trajectory. This is incompatible with the planinnewthread parameter. (optional,
defaults to False)
:param bool exportfulltraj: If this is True, the trajectory saved to file (and optionally returned) is for the
full robot, in internal DOF order rather than just the active DOF (optional, defaults to False)
:param bool printcommand: If this True, prints the resulting command string immediately before calling CBiRRT,
meant for debuggging (optional, defaults to False)
:return str|(str, rave.Trajectory): If outputtrajobj is False, returns the output of CBiRRT. Otheriwse
returns a tuple of the output and the decoded trajectory.
"""
cmd = ["RunCBiRRT"]
""":type : list[int | float | str]"""
if jointgoals is not None:
try: # Assume it's a list of lists of goals
for jointgoal in jointgoals:
assert isinstance(
jointgoal, list) # This is for the benefit of my IDE
cmd.append("jointgoals")
cmd.append(len(jointgoal))
cmd.extend(jointgoal)
except (AssertionError,
TypeError): # Fall back to single list of goals
cmd.append("jointgoals")
cmd.append(len(jointgoals))
cmd.extend(jointgoals)
if jointstarts is not None:
try: # Assume it's a list of lists of starts
for jointstart in jointstarts:
assert isinstance(
jointstart, list) # This is for the benefit of my IDE
cmd.append("jointstarts")
cmd.append(len(jointstart))
cmd.extend(jointstart)
except (AssertionError,
TypeError): # Fall back to single list of starts
cmd.append("jointstarts")
cmd.append(len(jointstarts))
cmd.extend(jointstarts)
if ikguess is not None:
cmd.append("ikguess")
cmd.append(len(ikguess))
cmd.extend(ikguess)
if Tattachedik_0 is not None:
for manipindex, T in Tattachedik_0.items():
cmd.append("Tattachedik_0")
cmd.append(manipindex)
cmd.append(SerializeTransform(T))
if smoothingitrs is not None:
cmd.append("smoothingitrs")
cmd.append(smoothingitrs)
if filename is not None:
cmd.append("filename")
cmd.append(filename)
if timelimit is not None:
cmd.append("timelimit")
cmd.append(timelimit)
if TSRChains is not None:
for chain in TSRChains:
# chain.serialize appends the "TSRChain" label as well
cmd.append(chain.serialize())
if supportlinks is not None and support is not None:
rave.raveLogWarn(
"Supportlinks and support should be mutually exclusive")
if supportlinks is not None:
cmd.append("supportlinks")
cmd.append(len(supportlinks))
cmd.extend(rave_object_name(l) for l in supportlinks)
if polyscale is not None:
if supportlinks is None:
rave.raveLogWarn(
"Polyscale has no effect when supportlinks is not provided"
)
cmd.append("polyscale")
cmd.extend(polyscale)
if len(polyscale) == 2:
cmd.append(0)
elif len(polyscale) != 3:
raise ValueError("Polyscale must have either 2 or 3 values")
if polytrans is not None:
if supportlinks is None:
rave.raveLogWarn(
"Polytrans has no effect when supportlinks is not provided"
)
cmd.append("polytrans")
cmd.extend(polytrans)
if len(polytrans) == 2:
cmd.append(0)
elif len(polytrans) != 3:
raise ValueError("Polytrans must have either 2 or 3 values")
if support is not None:
for manip, mu, mode in support:
cmd.append("support {} {} {:b}".format(rave_object_name(manip),
mu, mode))
if gravity is not None:
cmd.append("gravity")
# Enumerating manually to ensure you can pass in any indexable object and get a gravity vector of 3 items
cmd.append(gravity[0])
cmd.append(gravity[1])
cmd.append(gravity[2])
if heldobject is not None:
cmd.append("heldobject")
cmd.append(heldobject)
if psample is not None:
cmd.append("psample")
cmd.append(psample)
if bikfastsinglesolution is not None:
cmd.append("bikfastsinglesolution")
cmd.append(int(bikfastsinglesolution))
if planinnewthread is not None:
cmd.append("planinnewthread")
cmd.append(int(planinnewthread))
if allowlimadj is not None:
cmd.append("allowlimadj")
cmd.append(int(allowlimadj))
if smoothtrajonly is not None:
# This is a confusing parameter name, so add an assert to try to make sure it's being used correctly
if isinstance(smoothtrajonly, bool) or isinstance(
smoothtrajonly, int):
rave.raveLogWarn(
"smoothtrajonly should be a filename, not a boolean")
cmd.append("smoothtrajonly")
cmd.append(smoothtrajonly)
if exportfulltraj is not None:
cmd.append("exportfulltraj")
cmd.append(int(exportfulltraj))
cmd_str = " ".join(str(item) for item in cmd)
if printcommand:
print(cmd_str)
cbirrt_result = self.problem.SendCommand(cmd_str)
if cbirrt_result is None:
raise rave.PlanningError("CBiRRT Unknown Error")
elif cbirrt_result[0] == '0':
raise rave.PlanningError(cbirrt_result.lstrip("0 "))
if outputtrajobj:
trajobj = load_traj_from_file(filename, self.env)
return (cbirrt_result.lstrip("1 "), trajobj)
else:
return cbirrt_result.lstrip("1 ")
def CheckSupport(self,
supportlinks=None,
heldobject=None,
polyscale=None,
polytrans=None,
draw=False,
printcommand=False):
"""
Check robot support according to polygon-based stability. This function does not currently support GIWC
stability checking.
:param list[string|rave.KinBody.Link] supportlinks: Links to use to calculate the robot's support polygon,
specified either as the link name or link object. Setting this parameter enables balance checking
using the robot's support polygon. (optional)
:param str|rave.KinBody heldobject: A KinBody that robot is holding, specified as the KinBody object or name.
This body will be considered when calculating whether the robot is balanced. Only one is supported.
(optional)
:param list[float] polyscale: [x, y, z] values that scale the support polygon. Z is ignored and may be ommitted.
(optional, defaults to [1, 1, 1])
:param list[float] polytrans: [x, y, z] values that translate the support polygon. Z is ignored and may be
ommitted. (optional, defaults to [0, 0, 0])
:param bool draw: If True, the support polygon and robot's center of mass will be drawn in the viewer (optional,
defaults to False)
:param bool printcommand: If this True, prints the resulting command string immediately before calling CBiRRT,
meant for debuggging (optional, defaults to False)
:return bool: True if the robot is supported, false otherwise
"""
cmd = ["CheckSupport"]
""":type : list[int | float | str]"""
if supportlinks is not None:
cmd.append("supportlinks")
cmd.append(len(supportlinks))
cmd.extend(rave_object_name(l) for l in supportlinks)
if polyscale is not None:
if supportlinks is None:
rave.raveLogWarn(
"Polyscale has no effect when supportlinks is not provided"
)
cmd.append("polyscale")
cmd.extend(polyscale)
if len(polyscale) == 2:
cmd.append(0)
elif len(polyscale) != 3:
raise ValueError("Polyscale must have either 2 or 3 values")
if polytrans is not None:
if supportlinks is None:
rave.raveLogWarn(
"Polytrans has no effect when supportlinks is not provided"
)
cmd.append("polytrans")
cmd.extend(polytrans)
if len(polytrans) == 2:
cmd.append(0)
elif len(polytrans) != 3:
raise ValueError("Polytrans must have either 2 or 3 values")
if heldobject is not None:
cmd.append("heldobject")
cmd.append(heldobject)
if draw is not None:
cmd.append("draw")
cmd_str = " ".join(str(item) for item in cmd)
if printcommand:
print(cmd_str)
result = self.problem.SendCommand(cmd_str)
return bool(int(result))
def main(env,options):
"Main example code."
if options.iktype is not None:
# cannot use .names due to python 2.5 (or is it boost version?)
for value,type in IkParameterization.Type.values.items():
if type.name.lower() == options.iktype.lower():
iktype = type
break
else:
iktype = IkParameterizationType.Transform6D
env.Load(options.scene)
with env:
# load the Transform6D IK models
ikmodels = []
for robot in env.GetRobots():
for manip in robot.GetManipulators():
print(manip)
try:
robot.SetActiveManipulator(manip)
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=iktype)
if not ikmodel.load():
ikmodel.autogenerate()
if not ikmodel.has():
continue
ikmodels.append(ikmodel)
except Exception as e:
print('failed manip %s'%manip, e)
if len(ikmodels) == 0:
raveLogWarn('no manipulators found that can be loaded with iktype %s'%str(iktype))
# create the pickers
pickers = []
for ikmodel in ikmodels:
raveLogInfo('creating picker for %s'%str(ikmodel.manip))
picker = RaveCreateKinBody(env,'')
picker.InitFromBoxes(array([ [0.05,0,0,0.05,0.01,0.01], [0,0.05,0,0.01,0.05,0.01], [0,0,0.05,0.01,0.01,0.05] ]),True)
for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
color = zeros(3)
color[igeom] = 1
geom.SetDiffuseColor(color)
picker.SetName(ikmodel.manip.GetName())
env.Add(picker,True)
# have to disable since not part of collision
picker.Enable(False)
picker.SetTransform(ikmodel.manip.GetTransform())
pickers.append([picker,ikmodel.manip])
raveLogInfo('pickers loaded, try moving them')
def PickerThread(env,pickers,iktype):
"""this function runs in a separate thread monitoring each of the pickers
"""
Tpickers = [None]*len(pickers)
while True:
if env.GetViewer() is None:
break
with env:
for ipicker,(picker,manip) in enumerate(pickers):
T=picker.GetTransform()
if Tpickers[ipicker] is not None and sum(abs(Tpickers[ipicker]-T).flatten()) <= 1e-10:
continue
data = None
if iktype == IkParameterizationType.Direction3D:
data = T[0:3,2]
elif iktype == IkParameterizationType.Lookat3D:
data = T[0:3,3]
elif iktype == IkParameterizationType.Ray4D:
data = Ray(T[0:3,3],T[0:3,2])
elif iktype == IkParameterizationType.Rotation3D:
data = T[0:3,0:3]
elif iktype == IkParameterizationType.Transform6D:
data = T
elif iktype == IkParameterizationType.Translation3D:
data = T[0:3,3]
elif iktype == IkParameterizationType.TranslationDirection5D:
ikparam = Ray(T[0:3,3],T[0:3,2])
elif iktype == IkParameterizationType.TranslationLocalGlobal6D:
ikparam = [T[0:3,3],zeros(3)]
else:
raveLogWarn('iktype %s unsupported'%str(iktype))
continue
sol = manip.FindIKSolution(IkParameterization(data,iktype),IkFilterOptions.CheckEnvCollisions)
if sol is not None:
robot.SetDOFValues(sol,manip.GetArmIndices())
# have to update all other pickers since two manipulators can be connected to the same joints (torso)
for ipicker2, (picker2,manip2) in enumerate(pickers):
Tpickers[ipicker2] = manip2.GetTransform()
picker2.SetTransform(manip2.GetTransform())
# changing color produces bugs with qtcoin
# for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
# color = zeros(3)
# color[igeom] = 0.4 if sol is None else 1.0
# geom.SetDiffuseColor(color)
Tpickers[ipicker] = T
# update rate of 0.05s
time.sleep(0.05)
# create the thread
t = threading.Thread(target=PickerThread,args=[env,pickers,iktype])
t.start()
while True:
t.join(1)
def load_scene_from_options(env, options):
"""Load a scene for openhubo based on the options structure (see setup function for detailed options)."""
if hasattr(options, 'physics') and options.physics is False:
#Kludge since we won't be not using ODE for a while...
physics = False
elif hasattr(options, 'physics') and options.physics:
physics = True
elif options._physics == 'ode':
physics = True
else:
physics = False
vidrecorder = _recorder(env, filename=options.recordfile)
vidrecorder.videoparams[0:2] = [1024, 768]
vidrecorder.realtime = False
with env:
if options.stop or physics:
#KLUDGE: need to do this for stability reasons, not sure why, probably can be fixed another way
print "Stopping OpenRAVE simulation to load models and configure..."
env.StopSimulation()
if type(options.scenefile) is list:
for n in options.scenefile:
env.Load(n)
elif type(options.scenefile) is str:
env.Load(options.scenefile)
#This method ensures that the URI is preserved in the robot
if options.robotfile is not None:
robot = env.ReadRobotURI(options.robotfile)
env.Add(robot)
else:
robot = env.GetRobots()[0]
_rave.raveLogWarn("Assuming robot is {} (id=0)...".format(
robot.GetName()))
robot.SetDOFValues(zeros(robot.GetDOF()))
#Legacy command mode
if options.physicsfile == True:
options.physicsfile = 'physics.xml'
with env:
if physics and not check_physics(env):
_rave.raveLogInfo('Loading physics parameters from "{}"'.format(
options.physicsfile))
env.Load(options.physicsfile)
elif not physics:
env.SetPhysicsEngine(
_rave.RaveCreatePhysicsEngine(env, 'GenericPhysicsEngine'))
else:
#TODO: find a more efficient way to avoid double creation?
_rave.raveLogInfo(
"Physics engine already configured, overwriting...")
env.Load(options.physicsfile)
if check_physics(env):
_rave.raveLogInfo('Creating controller for physics simulation')
controller = _rave.RaveCreateController(env,
'trajectorycontroller')
robot.SetController(controller)
controller.SendCommand('set gains 150 0 .9 ')
controller.SendCommand('set radians ')
else:
#Just load ideal controller if physics engine is not present
_rave.raveLogInfo(
'Physics engine not loaded, using idealcontroller...')
controller = _rave.RaveCreateController(env, 'idealcontroller')
robot.SetController(controller)
if options.ghost and options.robotfile:
ghost_robot = load_ghost(env, options.robotfile, prefix="ghost_")
if check_physics(env):
controller.SendCommand("set visrobot " + ghost_robot.GetName())
else:
ghost_robot = None
collisionChecker = _rave.RaveCreateCollisionChecker(env, 'pqp')
if collisionChecker == None:
collisionChecker = _rave.RaveCreateCollisionChecker(env, 'ode')
_rave.raveLogWarn(
'Using ODE collision checker since PQP is not available...')
env.SetCollisionChecker(collisionChecker)
ind = make_name_to_index_converter(robot)
if options.atheight is not None:
align_robot(robot, options.atheight)
if ghost_robot:
align_robot(ghost_robot, options.atheight)
#FIXME: better way to do this?
global TIMESTEP
TIMESTEP = get_timestep(env)
return (robot, controller, ind, ghost_robot, vidrecorder)
def CheckSupport(self, supportlinks=None, heldobject=None, polyscale=None, polytrans=None, draw=False,
printcommand=False):
"""
Check robot support according to polygon-based stability. This function does not currently support GIWC
stability checking.
:param list[string|rave.KinBody.Link] supportlinks: Links to use to calculate the robot's support polygon,
specified either as the link name or link object. Setting this parameter enables balance checking
using the robot's support polygon. (optional)
:param str|rave.KinBody heldobject: A KinBody that robot is holding, specified as the KinBody object or name.
This body will be considered when calculating whether the robot is balanced. Only one is supported.
(optional)
:param list[float] polyscale: [x, y, z] values that scale the support polygon. Z is ignored and may be ommitted.
(optional, defaults to [1, 1, 1])
:param list[float] polytrans: [x, y, z] values that translate the support polygon. Z is ignored and may be
ommitted. (optional, defaults to [0, 0, 0])
:param bool draw: If True, the support polygon and robot's center of mass will be drawn in the viewer (optional,
defaults to False)
:param bool printcommand: If this True, prints the resulting command string immediately before calling CBiRRT,
meant for debuggging (optional, defaults to False)
:return bool: True if the robot is supported, false otherwise
"""
cmd = ["CheckSupport"]
""":type : list[int | float | str]"""
if supportlinks is not None:
cmd.append("supportlinks")
cmd.append(len(supportlinks))
cmd.extend(rave_object_name(l) for l in supportlinks)
if polyscale is not None:
if supportlinks is None:
rave.raveLogWarn("Polyscale has no effect when supportlinks is not provided")
cmd.append("polyscale")
cmd.extend(polyscale)
if len(polyscale) == 2:
cmd.append(0)
elif len(polyscale) != 3:
raise ValueError("Polyscale must have either 2 or 3 values")
if polytrans is not None:
if supportlinks is None:
rave.raveLogWarn("Polytrans has no effect when supportlinks is not provided")
cmd.append("polytrans")
cmd.extend(polytrans)
if len(polytrans) == 2:
cmd.append(0)
elif len(polytrans) != 3:
raise ValueError("Polytrans must have either 2 or 3 values")
if heldobject is not None:
cmd.append("heldobject")
cmd.append(heldobject)
if draw is not None:
cmd.append("draw")
cmd_str = " ".join(str(item) for item in cmd)
if printcommand:
print(cmd_str)
result = self.problem.SendCommand(cmd_str)
return bool(int(result))
def load_scene_from_options(env,options):
"""Load a scene for openhubo based on the options structure (see setup function for detailed options)."""
if hasattr(options,'physics') and options.physics is False:
#Kludge since we won't be not using ODE for a while...
physics=False
elif hasattr(options,'physics') and options.physics:
physics=True
elif options._physics=='ode':
physics=True
else:
physics=False
vidrecorder=_recorder(env,filename=options.recordfile)
vidrecorder.videoparams[0:2]=[1024,768]
vidrecorder.realtime=False
with env:
if options.stop or physics:
#KLUDGE: need to do this for stability reasons, not sure why, probably can be fixed another way
print "Stopping OpenRAVE simulation to load models and configure..."
env.StopSimulation()
if type(options.scenefile) is list:
for n in options.scenefile:
env.Load(n)
elif type(options.scenefile) is str:
env.Load(options.scenefile)
#This method ensures that the URI is preserved in the robot
if options.robotfile is not None:
robot=env.ReadRobotURI(options.robotfile)
env.Add(robot)
else:
robot=env.GetRobots()[0]
_rave.raveLogWarn("Assuming robot is {} (id=0)...".format(robot.GetName()))
robot.SetDOFValues(zeros(robot.GetDOF()))
#Legacy command mode
if options.physicsfile==True:
options.physicsfile='physics.xml'
with env:
if physics and not check_physics(env):
_rave.raveLogInfo('Loading physics parameters from "{}"'.format(options.physicsfile))
env.Load(options.physicsfile)
elif not physics:
env.SetPhysicsEngine(_rave.RaveCreatePhysicsEngine(env,'GenericPhysicsEngine'))
else:
#TODO: find a more efficient way to avoid double creation?
_rave.raveLogInfo("Physics engine already configured, overwriting...")
env.Load(options.physicsfile)
if check_physics(env):
_rave.raveLogInfo('Creating controller for physics simulation')
controller=_rave.RaveCreateController(env,'trajectorycontroller')
robot.SetController(controller)
controller.SendCommand('set gains 150 0 .9 ')
controller.SendCommand('set radians ')
else:
#Just load ideal controller if physics engine is not present
_rave.raveLogInfo('Physics engine not loaded, using idealcontroller...')
controller=_rave.RaveCreateController(env,'idealcontroller')
robot.SetController(controller)
if options.ghost and options.robotfile:
ghost_robot=load_ghost(env,options.robotfile,prefix="ghost_")
if check_physics(env):
controller.SendCommand("set visrobot " + ghost_robot.GetName())
else:
ghost_robot=None
collisionChecker = _rave.RaveCreateCollisionChecker(env,'pqp')
if collisionChecker==None:
collisionChecker = _rave.RaveCreateCollisionChecker(env,'ode')
_rave.raveLogWarn('Using ODE collision checker since PQP is not available...')
env.SetCollisionChecker(collisionChecker)
ind=make_name_to_index_converter(robot)
if options.atheight is not None:
align_robot(robot,options.atheight)
if ghost_robot:
align_robot(ghost_robot,options.atheight)
#FIXME: better way to do this?
global TIMESTEP
TIMESTEP=get_timestep(env)
return (robot,controller,ind,ghost_robot,vidrecorder)
def RunCBiRRT(self, jointgoals=None, jointstarts=None, ikguess=None, Tattachedik_0=None, smoothingitrs=None,
filename=None, timelimit=None, TSRChains=None, supportlinks=None, polyscale=None, polytrans=None,
support=None, gravity=None, heldobject=None, psample=None, bikfastsinglesolution=None,
planinnewthread=None, allowlimadj=None, smoothtrajonly=None, outputtrajobj=False, printcommand=False):
"""
Run the CBiRRT planner.
This command has many parameters which are fully documented below, but here is an overview:
The common use case, planning a path with TSR constraints, usually involves all or most of these parameters:
-- TSRChains, which describe the end effector constraints,
-- psample, which is required in order to sample from goal TSRs
-- outputtrajobj, which returs the planned trajectory
-- supportlinks or support, which do stability checking
-- timelimit, which limits the time taken by CBiRRT
These parameters change certian aspects of CBiRRT's behavior:
-- jointgoals, which sets the goal pose or poses (unclear whether that is instead of or in addition to sampling)
-- jointstarts, which sets the start pose or poses (default behavior is to use the current pose)
-- ikguess, which proivdes an initial guess for the iterative IK solver when creating the initial goal pose.
Setting ikguess can sometimes help if CBiRRT always returns "Unable to find a goal IK solution".
-- smoothingitrs, which controls how much smoothing will be performed. Increasing this value will usually
reduce cases where the robot makes unnecessarily complex motions.
-- bikfastsinglesolution, which is described below
Some sets of parameters are related or mutually exclusive:
-- polyscale and polytrans only have effect if you specify supportlinks
-- gravity only has effect if you specify support
-- supportlinks and support are mutually exclusive (they use different support modes)
-- planinnewthread, smoothtrajonly, and outputtrajobj are all mutually exclusive
-- smoothtrajonly causes every parameter except smoothingitrs and parameters which apply constraints on the path
to be ignored
:param list[float]|list[list[float]] jointgoals: Desired joint value vector at end of planning or list of
desired joint value vectors (optional)
:param list[float]|list[list[float]] jointstarts: Joint value vector at start of planning or list of desired
joint value vectors (optional, defaults to current
configuration)
:param list[float] ikguess: Joint value vector that the itertive IK solver will use as an initial guess when
sampling goals (optional)
:param dict[int, np.array|np.matrix] Tattachedik_0: Transformation matrix between attachedik frame and 0 frame
for each manipulator (optional, defaults to identity)
:param int smoothingitrs: Number of iterations of smoothing to apply to the final trajectory (optional,
defaults to 300)
:param str filename: Filename of generated trajectory file (optional, defaults to "cmovetraj.txt")
:param float timelimit: Time limit in seconds before planning gives up (optional, defaults to 25 seconds)
:param list[TSRChain] TSRChains: A list of TSR chains to constrain the path, start, and/or goal (optional)
:param list[string|rave.KinBody.Link] supportlinks: Links to use to calculate the robot's support polygon,
specified either as the link name or link object. Setting this parameter enables balance checking
using the robot's support polygon. (optional)
:param list[float] polyscale: [x, y, z] values that scale the support polygon. Z is ignored and may be ommitted.
(optional, defaults to [1, 1, 1])
:param list[float] polytrans: [x, y, z] values that translate the support polygon. Z is ignored and may be
ommitted. (optional, defaults to [0, 0, 0])
:param list[(str|rave.Manipulator, float, int)] support: Manipulators to use when calculating support using the
GIWC (Gravito-Inertial Wrench Cone). Each should be specified as a 3-tuple of the Manipulator object
or manipulator's name, the coefficient of friction to use for that manipulator, and a bitmask
indicating whether the manipulator is supporting the robot at the start, path, and/or goal. The
CBiRRT.START, CBiRRT.PATH, and CBiRRT.GOAL variables can be used with boolean or to construct this
bitmask. Note that path constraints also apply at the start and goal, so if the goal support is a
superset of the path support it has no effect. Specifying this parameter enables GIWC support
checking, and it should not be specified with the supportlinks parameter.
:param list[float] gravity: The [x, y, z] gravity vector to use when calculating support using GIWC (optional,
defaults to [0, 0, -9.8])
:param str|rave.KinBody heldobject: A KinBody that robot is holding, specified as the KinBody object or name.
This body will be considered when calculating whether the robot is balanced. Only one is supported.
(optional)
:param float psample: Probability of sampling from the TSR chains rather than growing the RRT trees on each
iteration. Defaults to 0, which is usually not desired behavior. This module will output a warning
if psample is not specified and TSRs are provided. To silence this warning, specifically set psample
to 0. (optional, defaults to 0)
:param boolean bikfastsinglesolution: Controls the use of attached IK solvers (such as ikfast) when sampling
start and goal configurations. If this is true, CBiRRT will get a single IK solution with collision
checking enabled in the IK solver. If fasle, it will get multiple IK solutoins with collision
checking disabled, and perform collision checking on them later. (optional, defaults to True)
:param boolean planinnewthread: Whether to run the planner in a new thread. If this is true then RunCBiRRT
returns quickly and GetPlannerState and StopPlanner must be used to interact with the planner.
(optional, defaults to False)
:param boolean allowlimadj: Allow CBiRRT to temporarily adjust joint limits up or down so that the start
configuration is always within joint limits (optional, defaults to False)
:param str smoothtrajonly: If this parameter is provided, CBiRRT just performs trajectory smoothing on the
trajectory file. The value of this argument is the filename of the trajectory to smooth (optional)
:param bool outputtrajobj: If outputtrajobj is True then this method returns a 2-tuple of CBiRRT's output and
the resulting trajectory. This is incompatible with the planinnewthread parameter. (optional,
defaults to False)
:param bool printcommand: If this True, prints the resulting command string immediately before calling CBiRRT,
meant for debuggging (optional, defaults to False)
:return str|(str, rave.Trajectory): If outputtrajobj is False, returns the output of CBiRRT. Otheriwse
returns a tuple of the output and the decoded trajectory.
"""
cmd = ["RunCBiRRT"]
""":type : list[int | float | str]"""
if jointgoals is not None:
try: # Assume it's a list of lists of goals
for jointgoal in jointgoals:
assert isinstance(jointgoal, list) # This is for the benefit of my IDE
cmd.append("jointgoals")
cmd.append(len(jointgoal))
cmd.extend(jointgoal)
except (AssertionError, TypeError): # Fall back to single list of goals
# "jointgoals" will have already been appended
cmd.append(len(jointgoals))
cmd.extend(jointgoals)
if jointstarts is not None:
try: # Assume it's a list of lists of starts
for jointstart in jointstarts:
assert isinstance(jointstart, list) # This is for the benefit of my IDE
cmd.append("jointstarts")
cmd.append(len(jointstart))
cmd.extend(jointstart)
except (AssertionError, TypeError): # Fall back to single list of starts
# "jointstarts" will have already been appended
cmd.append(len(jointstarts))
cmd.extend(jointstarts)
if ikguess is not None:
cmd.append("ikguess")
cmd.append(len(ikguess))
cmd.extend(ikguess)
if Tattachedik_0 is not None:
for manipindex, T in Tattachedik_0.items():
cmd.append("Tattachedik_0")
cmd.append(manipindex)
cmd.append(SerializeTransform(T))
if smoothingitrs is not None:
cmd.append("smoothingitrs")
cmd.append(smoothingitrs)
if filename is not None:
cmd.append("filename")
cmd.append(filename)
if timelimit is not None:
cmd.append("timelimit")
cmd.append(timelimit)
if TSRChains is not None:
for chain in TSRChains:
# chain.serialize appends the "TSRChain" label as well
cmd.append(chain.serialize())
if supportlinks is not None and support is not None:
rave.raveLogWarn("Supportlinks and support should be mutually exclusive")
if supportlinks is not None:
cmd.append("supportlinks")
cmd.append(len(supportlinks))
cmd.extend(rave_object_name(l) for l in supportlinks)
if polyscale is not None:
if supportlinks is None:
rave.raveLogWarn("Polyscale has no effect when supportlinks is not provided")
cmd.append("polyscale")
cmd.extend(polyscale)
if len(polyscale) == 2:
cmd.append(0)
elif len(polyscale) != 3:
raise ValueError("Polyscale must have either 2 or 3 values")
if polytrans is not None:
if supportlinks is None:
rave.raveLogWarn("Polytrans has no effect when supportlinks is not provided")
cmd.append("polytrans")
cmd.extend(polytrans)
if len(polytrans) == 2:
cmd.append(0)
elif len(polytrans) != 3:
raise ValueError("Polytrans must have either 2 or 3 values")
if support is not None:
for manip, mu, mode in support:
cmd.append("support {} {} {:b}".format(rave_object_name(manip), mu, mode))
if gravity is not None:
cmd.append("gravity")
# Enumerating manually to ensure you can pass in any indexable object and get a gravity vector of 3 items
cmd.append(gravity[0])
cmd.append(gravity[1])
cmd.append(gravity[2])
if heldobject is not None:
cmd.append("heldobject")
cmd.append(heldobject)
if psample is not None:
cmd.append("psample")
cmd.append(psample)
if bikfastsinglesolution is not None:
cmd.append("bikfastsinglesolution")
cmd.append(int(bikfastsinglesolution))
if planinnewthread is not None:
cmd.append("planinnewthread")
cmd.append(int(planinnewthread))
if allowlimadj is not None:
cmd.append("allowlimadj")
cmd.append(int(allowlimadj))
if smoothtrajonly is not None:
# This is a confusing parameter name, so add an assert to try to make sure it's being used correctly
if isinstance(smoothtrajonly, bool) or isinstance(smoothtrajonly, int):
rave.raveLogWarn("smoothtrajonly should be a filename, not a boolean")
cmd.append("smoothtrajonly")
cmd.append(smoothtrajonly)
cmd_str = " ".join(str(item) for item in cmd)
if printcommand:
print(cmd_str)
cbirrt_result = self.problem.SendCommand(cmd_str)
if cbirrt_result is None:
raise rave.PlanningError("CBiRRT Unknown Error")
elif cbirrt_result[0] == '0':
raise rave.PlanningError(cbirrt_result.lstrip("0 "))
if outputtrajobj:
trajobj = load_traj_from_file(filename or "cmovetraj.txt", self.env)
return (cbirrt_result.lstrip("1 "), trajobj)
else:
return cbirrt_result.lstrip("1 ")
def plan_env_matching(or_robot,
contact_points,
contact_regions,
torso_pose_grid,
start,
goal,
torso_path,
structures,
motion_mode,
motion_plan_library,
motion_plan_clusters,
motion_plan_explore_order,
planning_time_limit,
elasticstrips=None,
general_ik_interface=None):
######################################################################################
# Initialization
rave.raveLogInfo('plan_env_matching')
DOFNameActiveIndexDict = or_robot.DOFNameActiveIndexDict
lower_limits = or_robot.lower_limits
higher_limits = or_robot.higher_limits
IKInitDOFValues = or_robot.IKInitDOFValues
StandingPostureDOFValues = or_robot.StandingPostureDOFValues
or_robot.robot.SetDOFValues(StandingPostureDOFValues)
goal_x = goal[0]
goal_y = goal[1]
goal_z = torso_pose_grid.get_cell_height(
torso_pose_grid.position_to_grid_xy((goal_x, goal_y)))
if goal_z < -0.05:
goal_z = 0
goal_theta = goal[2]
start_x = start[0]
start_y = start[1]
start_z = torso_pose_grid.get_cell_height(
torso_pose_grid.position_to_grid_xy((start_x, start_y)))
if start_z < -0.05:
start_z = 0
start_theta = start[2]
goal_dist = math.hypot(goal_x - start_x, goal_y - start_y)
collision_link_list = [
'l_shin', 'l_foot', 'r_shin', 'r_foot', 'l_forearm', 'l_palm',
'r_forearm', 'r_palm'
]
selfcollision_link_pair_list = [('l_foot', 'r_foot'), ('l_shin', 'r_shin'),
('l_palm', 'l_thigh'),
('l_index_prox', 'l_thigh'),
('l_index_med', 'l_thigh'),
('l_index_dist', 'l_thigh'),
('r_palm', 'r_thigh'),
('r_index_prox', 'r_thigh'),
('r_index_med', 'r_thigh'),
('r_index_dist', 'r_thigh')]
i_xpa = DOFNameActiveIndexDict['x_prismatic_joint']
i_ypa = DOFNameActiveIndexDict['y_prismatic_joint']
i_zpa = DOFNameActiveIndexDict['z_prismatic_joint']
i_rra = DOFNameActiveIndexDict['roll_revolute_joint']
i_pra = DOFNameActiveIndexDict['pitch_revolute_joint']
i_yra = DOFNameActiveIndexDict['yaw_revolute_joint']
env = or_robot.env
start_bound_radius = 0.1
goal_bound_radius = 0.1
# DrawRegion(or_robot.env,xyzrpy_to_SE3([0,0,init_z+0.01,0,0,0]),start_bound_radius)
# DrawRegion(or_robot.env,xyzrpy_to_SE3([goal_x,goal_y,goal_z+0.01,0,0,goal_theta]),goal_bound_radius)
######################################################################################
start_time = time.time()
solution_found = False
subsegment_goal = copy.deepcopy(start)
largest_covered_cell_index = 0
subsegment_list = []
# check if every part of the path is covered
while largest_covered_cell_index != len(torso_path) - 1:
if time.time() - start_time > planning_time_limit:
break
subsegment_start = copy.deepcopy(subsegment_goal)
subsegment_start_to_goal_dist = math.hypot(
goal[0] - subsegment_start[0], goal[1] - subsegment_start[1])
found_matching_motion_plan = False
for motion_plan_index in motion_plan_explore_order:
or_robot.robot.SetDOFValues(StandingPostureDOFValues)
plan = motion_plan_library[motion_plan_index]
plan.contact_plan[0].parent = None
plan_shorter_than_torso_path = plan.plan_travel_dist < subsegment_start_to_goal_dist + goal_bound_radius
# if the plan travel distance is shorter than the subsegment length, use all the motion plan
if plan_shorter_than_torso_path:
# the intersection of the motion plan to the closest torso path node
current_travel_dist = 0
cell_position = subsegment_start
subsegment_goal = None
subsegment_torso_path_range = None
for i, cell in enumerate(
torso_path[largest_covered_cell_index:]):
last_cell_position = cell_position
cell_position = cell.get_position()
last_travel_dist = current_travel_dist
current_travel_dist = math.hypot(
cell_position[0] - subsegment_start[0],
cell_position[1] - subsegment_start[1])
if plan.plan_travel_dist < current_travel_dist:
# decide subsegment_goal
if abs(current_travel_dist -
plan.plan_travel_dist) < abs(
last_travel_dist - plan.plan_travel_dist):
subsegment_goal = cell_position
else:
subsegment_goal = last_cell_position
# decide subsegment_torso_path_range
subsegment_torso_path_range = (
largest_covered_cell_index,
i + largest_covered_cell_index)
break
# means the motion plan cover the whole remaining torso path
if subsegment_torso_path_range is None:
subsegment_goal = cell_position
subsegment_torso_path_range = (largest_covered_cell_index,
len(torso_path) - 1)
involved_contact_sequence = copy.deepcopy(
plan.contact_sequence)
# if the plan travel distance is longer than the subsegment length, use part of the motion plan
else:
# find the contacts that will be involved in the the feature matching
current_left_foot_contact = None
current_right_foot_contact = None
involved_contact_sequence = []
orientation_offset = None
final_left_foot_contact = None
final_right_foot_contact = None
for contact in plan.contact_sequence:
if contact.manip == 'l_foot':
current_left_foot_contact = contact
elif contact.manip == 'r_foot':
current_right_foot_contact = contact
if current_left_foot_contact is None or current_right_foot_contact is None:
involved_contact_sequence.append(
copy.deepcopy(contact))
continue
current_mean_foot_position = (
current_left_foot_contact.transform[0:3, 3] +
current_right_foot_contact.transform[0:3, 3]) / 2.0
current_travel_dist = math.hypot(
current_mean_foot_position[0],
current_mean_foot_position[1])
if current_travel_dist <= subsegment_start_to_goal_dist + goal_bound_radius:
involved_contact_sequence.append(
copy.deepcopy(contact))
orientation_offset = math.atan2(
current_mean_foot_position[1],
current_mean_foot_position[0])
final_left_foot_contact = current_left_foot_contact
final_right_foot_contact = current_right_foot_contact
else:
break
# decide subsegment_goal
subsegment_goal = copy.deepcopy(goal)
# decide subsegment indices range
subsegment_torso_path_range = (largest_covered_cell_index,
len(torso_path) - 1)
subsegment_goal_x = subsegment_goal[0]
subsegment_goal_y = subsegment_goal[1]
subsegment_goal_z = torso_pose_grid.get_cell_height(
torso_pose_grid.position_to_grid_xy(
(subsegment_goal_x, subsegment_goal_y)))
if subsegment_goal_z < -0.05:
subsegment_goal_z = 0
subsegment_goal_theta = subsegment_goal[2]
subsegment_start_x = subsegment_start[0]
subsegment_start_y = subsegment_start[1]
subsegment_start_z = torso_pose_grid.get_cell_height(
torso_pose_grid.position_to_grid_xy(
(subsegment_start_x, subsegment_start_y)))
if subsegment_start_z < -0.05:
subsegment_start_z = 0
subsegment_start_theta = subsegment_start[2]
# initialize the rigid plan pose
if plan_shorter_than_torso_path:
init_theta = math.atan2(subsegment_goal_y - subsegment_start_y,
subsegment_goal_x - subsegment_start_x)
else:
init_theta = math.atan2(
subsegment_goal_y - subsegment_start_y, subsegment_goal_x -
subsegment_start_x) - orientation_offset
init_x = subsegment_start_x
init_y = subsegment_start_y
init_z = subsegment_start_z
# the plan are all aligned to the mean position of two feet at the beginning.
q_seq = np.array([[init_x], [init_y], [init_z], [init_theta]
]) # (x,y,z,theta) of the whole sequence
# jacobian alignment and get the matching score
plan_cost = fitting_env_and_get_plan_cost(
or_robot,
involved_contact_sequence,
contact_regions,
q_seq, (subsegment_start_x, subsegment_start_y,
subsegment_start_z, subsegment_start_theta),
(subsegment_goal_x, subsegment_goal_y, subsegment_goal_z,
subsegment_goal_theta),
start_bound_radius,
goal_bound_radius,
dynamic_final_feet_contacts=True)
print('Plan %i, Cost: %5.5f' % (motion_plan_index, plan_cost))
if plan_cost == 9999 or plan_cost == 4999: # length not match
rave.raveLogWarn(
'Contact pose distance too high, reject the motion plan.')
continue
plan_yaw = q_seq[3, 0] * rad_to_deg
plan_transform = xyzrpy_to_SE3(
[q_seq[0, 0], q_seq[1, 0], q_seq[2, 0], 0, 0, plan_yaw])
plan_contact_sequence = copy.deepcopy(involved_contact_sequence)
for i in range(len(involved_contact_sequence)):
plan_contact_sequence[i].transform = np.dot(
plan_transform, involved_contact_sequence[i].transform)
##################################################
# initialize the objects needed for elastic strips
aligned_rave_traj = rave.RaveCreateTrajectory(or_robot.env, '')
traj_serial = open(plan.traj_path, 'r').read()
aligned_rave_traj.deserialize(traj_serial)
if plan_shorter_than_torso_path:
waypoint_contact_manips = copy.deepcopy(
plan.waypoint_contact_manips)
plan_path = copy.deepcopy(plan.contact_plan)
plan_aligned_traj = [None
] * aligned_rave_traj.GetNumWaypoints()
for i in range(aligned_rave_traj.GetNumWaypoints()):
plan_aligned_traj[i] = copy.deepcopy(
aligned_rave_traj.GetWaypoint(i))
else:
final_left_foot_contact_position = final_left_foot_contact.transform[
0:3, 3]
final_right_foot_contact_position = final_right_foot_contact.transform[
0:3, 3]
final_left_foot_contact_reached = False
final_right_foot_contact_reached = False
contact_state_bound_indices = None
for i, node in enumerate(plan.contact_plan):
if np.linalg.norm(
(np.array(node.left_leg[0:3]) -
final_left_foot_contact_position)) < 0.002:
final_left_foot_contact_reached = True
else:
if final_left_foot_contact_reached and final_right_foot_contact_reached:
contact_state_bound_indices = (0, i - 1)
break
if np.linalg.norm(
(np.array(node.right_leg[0:3]) -
final_right_foot_contact_position)) < 0.002:
final_right_foot_contact_reached = True
else:
if final_left_foot_contact_reached and final_right_foot_contact_reached:
contact_state_bound_indices = (0, i - 1)
break
if final_left_foot_contact_reached and final_right_foot_contact_reached and i == len(
plan.contact_plan) - 1:
contact_state_bound_indices = (0, i)
if contact_state_bound_indices is None:
print('empty contact state bound indices')
IPython.embed()
(plan_path, plan_aligned_traj,
waypoint_contact_manips) = extract_partial_motion_plan(
plan.contact_plan, aligned_rave_traj,
plan.waypoint_contact_manips, contact_state_bound_indices)
if len(waypoint_contact_manips) == 0:
rave.raveLogWarn(
'Extracted motion plan does not contain any waypoint, reject the motion plan.'
)
continue
# check if the motion plan orientation matches the start and goal orientation
if (abs(
angle_diff(subsegment_start_theta,
plan_path[0].get_virtual_body_yaw() + plan_yaw))
> 30 or abs(
angle_diff(
subsegment_goal_theta,
plan_path[-1].get_virtual_body_yaw() + plan_yaw)) >
30):
rave.raveLogWarn(
'Path orientation difference too high, reject the motion plan.'
)
continue
##################################################
# delete_contact_visualization(or_robot)
if draw_planning_progress:
show_contact(or_robot, plan_contact_sequence)
# DrawLocation(or_robot.env,np.array([[1,0,0,0],[0,1,0,0],[0,0,1,get_z(0,0,structures)+0.01],[0,0,0,1]]),[255,0,0],start_bound_radius)
# DrawLocation(or_robot.env,np.array([[1,0,0,goal_x],[0,1,0,goal_y],[0,0,1,goal_z+0.01],[0,0,0,1]]),[0,0,255],goal_bound_radius)
# initialize robot configuration
or_robot.robot.SetDOFValues(IKInitDOFValues)
num_waypoints = len(plan_aligned_traj)
plan_traj = [None] * num_waypoints
for i in range(num_waypoints):
q = copy.deepcopy(plan_aligned_traj[i])
affine_x = q[i_xpa]
affine_y = q[i_ypa]
affine_z = q[i_zpa]
affine_roll = q[i_rra] * rad_to_deg
affine_pitch = q[i_pra] * rad_to_deg
affine_yaw = q[i_yra] * rad_to_deg
affine_transform = xyzrpy_to_SE3([
affine_x, affine_y, affine_z, affine_roll, affine_pitch,
affine_yaw
])
new_affine_transform = np.dot(plan_transform, affine_transform)
new_affine_xyzrpy = SE3_to_xyzrpy(new_affine_transform)
q[i_xpa] = min(max(new_affine_xyzrpy[0], lower_limits[i_xpa]),
higher_limits[i_xpa])
q[i_ypa] = min(max(new_affine_xyzrpy[1], lower_limits[i_ypa]),
higher_limits[i_ypa])
q[i_zpa] = min(max(new_affine_xyzrpy[2], lower_limits[i_zpa]),
higher_limits[i_zpa])
q[i_rra] = min(
max(new_affine_xyzrpy[3] * deg_to_rad,
lower_limits[i_rra]), higher_limits[i_rra])
q[i_pra] = min(
max(new_affine_xyzrpy[4] * deg_to_rad,
lower_limits[i_pra]), higher_limits[i_pra])
q[i_yra] = min(
max(new_affine_xyzrpy[5] * deg_to_rad,
lower_limits[i_yra]), higher_limits[i_yra])
plan_traj[i] = q
# Elastic Strips
# trajectory list is going to be a list of lists, each list stores the activeDOF value of the waypoint
trajectory_list = plan_traj
manips_list = set(['l_leg', 'r_leg'])
for contact_manips in waypoint_contact_manips:
for contact_manip in contact_manips:
if contact_manip[0] == 'l_arm':
manips_list.add('l_arm')
elif contact_manip[0] == 'r_arm':
manips_list.add('r_arm')
manips_list = list(manips_list)
support_list = [('l_leg', mu), ('r_leg', mu), ('l_arm', mu),
('r_arm', mu)]
print('before elastic strips.')
or_robot.robot.SetDOFValues(StandingPostureDOFValues)
elasticstrips = ElasticStrips(env, or_robot.robot.GetName())
or_robot.robot.SetDOFValues(StandingPostureDOFValues)
print('reachability elastic strips')
if len(trajectory_list) != len(waypoint_contact_manips):
rave.raveLogError(
'Trajectory waypoint number mismatch. Cannot do Elastic Strips.'
)
IPython.embed()
transition_trajectory_list = []
transition_waypoint_contact_manips = []
waypoint_manip_num = len(waypoint_contact_manips[0])
downsampled_trajectory_list = []
downsampled_waypoint_contact_manips = []
for i in range(len(trajectory_list) + 1):
if i == len(trajectory_list) or waypoint_manip_num != len(
waypoint_contact_manips[i]):
if len(transition_trajectory_list) > 2:
downsampled_transition_trajectory_list = transition_trajectory_list[
0:len(transition_trajectory_list):5]
downsampled_transition_waypoint_contact_manips = transition_waypoint_contact_manips[
0:len(transition_trajectory_list):5]
if (len(transition_trajectory_list) - 1) % 5 != 0:
downsampled_transition_trajectory_list.append(
transition_trajectory_list[-1])
downsampled_transition_waypoint_contact_manips.append(
transition_waypoint_contact_manips[-1])
else:
downsampled_transition_trajectory_list = transition_trajectory_list
downsampled_transition_waypoint_contact_manips = transition_waypoint_contact_manips
downsampled_trajectory_list.extend(
downsampled_transition_trajectory_list)
downsampled_waypoint_contact_manips.extend(
downsampled_transition_waypoint_contact_manips)
if i < len(trajectory_list):
waypoint_manip_num = len(waypoint_contact_manips[i])
transition_trajectory_list = [trajectory_list[i]]
transition_waypoint_contact_manips = [
waypoint_contact_manips[i]
]
else:
transition_trajectory_list.append(trajectory_list[i])
transition_waypoint_contact_manips.append(
waypoint_contact_manips[i])
num_waypoints = len(downsampled_trajectory_list)
trajectory_list = downsampled_trajectory_list
waypoint_contact_manips = downsampled_waypoint_contact_manips
with env:
with or_robot.robot:
output_waypoints_list = elasticstrips.RunElasticStrips(
manips=manips_list,
trajectory=trajectory_list,
contact_manips=waypoint_contact_manips,
printcommand=False)
or_robot.robot.SetDOFValues(StandingPostureDOFValues)
if output_waypoints_list:
end_time = time.time()
modified_rave_traj = rave.RaveCreateTrajectory(
or_robot.env, '')
modified_rave_traj.Init(
aligned_rave_traj.GetConfigurationSpecification())
q_modified = [None] * or_robot.robot.GetActiveDOF()
for w in range(num_waypoints):
for i in range(or_robot.robot.GetActiveDOF()):
q_modified[i] = output_waypoints_list[
w * (or_robot.robot.GetActiveDOF() + 1) + i + 1]
modified_rave_traj.Insert(
modified_rave_traj.GetNumWaypoints(), q_modified)
# extract contact plan path from the trajectory
prev_contact_manips = waypoint_contact_manips[0]
or_robot.robot.SetActiveDOFValues(
modified_rave_traj.GetWaypoint(0))
plan_path[0].left_leg = SE3_to_xyzrpy(
or_robot.robot.GetManipulator('l_leg').GetTransform())
plan_path[0].right_leg = SE3_to_xyzrpy(
or_robot.robot.GetManipulator('r_leg').GetTransform())
if plan_path[0].left_arm[0] != -99.0:
plan_path[0].left_arm = SE3_to_xyzrpy(
or_robot.robot.GetManipulator('l_arm').GetTransform())
if plan_path[0].right_arm[0] != -99.0:
plan_path[0].right_arm = SE3_to_xyzrpy(
or_robot.robot.GetManipulator('r_arm').GetTransform())
plan_path[0].prev_move_manip = None
current_path_index = 1
for w in range(1, modified_rave_traj.GetNumWaypoints()):
or_robot.robot.SetActiveDOFValues(
modified_rave_traj.GetWaypoint(w))
if len(prev_contact_manips) < len(
waypoint_contact_manips[w]):
plan_path[current_path_index].left_leg = copy.copy(
plan_path[current_path_index - 1].left_leg)
plan_path[current_path_index].right_leg = copy.copy(
plan_path[current_path_index - 1].right_leg)
plan_path[current_path_index].left_arm = copy.copy(
plan_path[current_path_index - 1].left_arm)
plan_path[current_path_index].right_arm = copy.copy(
plan_path[current_path_index - 1].right_arm)
if plan_path[current_path_index].prev_move_manip == 0:
plan_path[
current_path_index].left_leg = SE3_to_xyzrpy(
or_robot.robot.GetManipulator(
'l_leg').GetTransform())
elif plan_path[
current_path_index].prev_move_manip == 1:
plan_path[
current_path_index].right_leg = SE3_to_xyzrpy(
or_robot.robot.GetManipulator(
'r_leg').GetTransform())
elif plan_path[
current_path_index].prev_move_manip == 2:
plan_path[
current_path_index].left_arm = SE3_to_xyzrpy(
or_robot.robot.GetManipulator(
'l_arm').GetTransform())
elif plan_path[
current_path_index].prev_move_manip == 3:
plan_path[
current_path_index].right_arm = SE3_to_xyzrpy(
or_robot.robot.GetManipulator(
'r_arm').GetTransform())
while current_path_index < len(plan_path) - 1:
current_path_index = current_path_index + 1
if plan_path[
current_path_index].prev_move_manip == 0 or plan_path[
current_path_index].prev_move_manip == 1:
break
elif plan_path[
current_path_index].prev_move_manip == 2 and plan_path[
current_path_index].left_arm[
0] != -99.0:
break
elif plan_path[
current_path_index].prev_move_manip == 3 and plan_path[
current_path_index].right_arm[
0] != -99.0:
break
prev_contact_manips = waypoint_contact_manips[w]
for i in range(len(plan_path)):
pose_list = [
plan_path[i].left_leg, plan_path[i].right_leg,
plan_path[i].left_arm, plan_path[i].right_arm
]
for j in range(len(pose_list)):
pose_list[j][0:3] = [
round(v, 3) for v in pose_list[j][0:3]
]
pose_list[j][3:6] = [
round(v, 1) for v in pose_list[j][3:6]
]
# check if it satisfies the end point balance criterion
transition_feasible = True
with env:
with or_robot.robot:
for plan_node in plan_path:
if not transition_feasibility(
or_robot, general_ik_interface, plan_node):
transition_feasible = False
break
# check if the iniitial node is feasible
if transition_feasible:
all_support_list = [('l_leg', mu), ('r_leg', mu)]
if plan_path[0].left_arm[0] != -99.0:
all_support_list.append(('l_arm', mu))
if plan_path[0].right_arm[0] != -99.0:
all_support_list.append(('r_arm', mu))
for support_list in itertools.combinations(
all_support_list,
len(all_support_list) - 1):
if node_IK(or_robot, general_ik_interface,
plan_path[0], support_list) is None:
transition_feasible = False
break
# check if the goal node is feasible
if transition_feasible:
all_support_list = [('l_leg', mu), ('r_leg', mu)]
if plan_path[-1].left_arm[0] != -99.0:
all_support_list.append(('l_arm', mu))
if plan_path[-1].right_arm[0] != -99.0:
all_support_list.append(('r_arm', mu))
for support_list in itertools.combinations(
all_support_list,
len(all_support_list) - 1):
if node_IK(or_robot, general_ik_interface,
plan_path[-1],
support_list) is None:
transition_feasible = False
break
if not transition_feasible:
rave.raveLogWarn(
'Transition is not feasible, reject the motion plan.')
continue
found_matching_motion_plan = True
# final_rave_traj = rave.RaveCreateTrajectory(or_robot.env,'')
# final_rave_traj.Init(aligned_rave_traj.GetConfigurationSpecification())
final_rave_traj = None
final_waypoint_contact_manips = None
# with env:
# (feasible_traj,final_waypoint_contact_manips) = step_interpolation(final_rave_traj,plan_path,or_robot)
# break
rave.raveLogInfo('Matching motion plan found.')
delete_contact_visualization(or_robot)
subsegment_list.append(
(plan_path, final_rave_traj, final_waypoint_contact_manips,
subsegment_torso_path_range))
largest_covered_cell_index = subsegment_torso_path_range[1]
break
else:
rave.raveLogWarn('Elastic Strips fail. Not feasible.')
delete_contact_visualization(or_robot)
if not found_matching_motion_plan:
if subsegment_list:
subsegment_list.append(
(None, None, None, subsegment_torso_path_range))
break
delete_contact_visualization(or_robot)
if not subsegment_list or time.time() - start_time > planning_time_limit:
return None
else:
return subsegment_list
