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 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