OpenRAVE Documentation

Source code for openravepy.interfaces.BaseManipulation

# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from __future__ import with_statement # for python 2.5
__author__ = 'Rosen Diankov'
__copyright__ = 'Copyright (C) 2009-2011 Rosen Diankov <rosen.diankov@gmail.com>'
__license__ = 'Apache License, Version 2.0'
# python 2.5 raises 'import *' not allowed with 'from .'
from ..openravepy_int import RaveCreateModule, RaveCreateTrajectory, matrixSerialization, IkParameterization
from ..openravepy_ext import planning_error
    
import numpy
from copy import copy as shallowcopy

[docs]class BaseManipulation: """Interface wrapper for :ref:`module-basemanipulation` """ def __init__(self,robot,plannername=None,maxvelmult=None): env = robot.GetEnv() self.prob = RaveCreateModule(env,'BaseManipulation') self.robot = robot self.args = self.robot.GetName() if plannername is not None: self.args += u' planner ' + plannername if maxvelmult is not None: self.args += u' maxvelmult %.15e '%maxvelmult env.Add(self.prob,True,self.args) def __del__(self): self.prob.GetEnv().Remove(self.prob)
[docs] def clone(self,envother): return self.Clone(envother)
[docs] def Clone(self,envother): """Clones the interface into another environment """ clone = shallowcopy(self) clone.prob = RaveCreateModule(envother,'BaseManipulation') clone.robot = envother.GetRobot(self.robot.GetName()) envother.Add(clone.prob,True,clone.args) return clone
[docs] def SetRobot(self,robot): """See :ref:`module-basemanipulation-setrobot` """ success = self.prob.SendCommand(u'setrobot '+robot.GetName()) if success is not None: self.robot = robot return True return False
[docs] def TrajFromData(self,data,resettrans=False,resettiming=False): """See :ref:`module-basemanipulation-traj` """ return self.prob.SendCommand('traj stream ' + data + ' %d %d '%(resettrans,resettiming))
[docs] def VerifyTrajectory(self,data,resettrans=False,resettiming=False,samplingstep=None): """See :ref:`module-basemanipulation-verifytrajectory` """ cmd = 'VerifyTrajectory stream ' + data + ' resettrans %d resettiming %d '%(resettrans,resettiming) if samplingstep is not None: cmd += 'samplingstep %.15e '%samplingstep print cmd return self.prob.SendCommand(cmd)
[docs] def MoveHandStraight(self,direction,minsteps=None,maxsteps=None,stepsize=None,ignorefirstcollision=None,starteematrix=None,greedysearch=None,execute=None,outputtraj=None,maxdeviationangle=None,steplength=None,planner=None,outputtrajobj=None): """See :ref:`module-basemanipulation-movehandstraight` """ cmd = 'MoveHandStraight direction %.15e %.15e %.15e '%(direction[0],direction[1],direction[2]) if minsteps is not None: cmd += 'minsteps %d '%minsteps if maxsteps is not None: cmd += 'maxsteps %d '%maxsteps if stepsize is not None: cmd += 'steplength %.15e '%stepsize if steplength is not None: cmd += 'steplength %.15e '%steplength if planner is not None: cmd += 'planner %s '%planner if execute is not None: cmd += 'execute %d '%execute if starteematrix is not None: cmd += 'starteematrix ' + matrixSerialization(starteematrix) + ' ' if greedysearch is not None: cmd += 'greedysearch %d '%greedysearch if (outputtraj is not None and outputtraj) or (outputtrajobj is not None and outputtrajobj): cmd += 'outputtraj ' if ignorefirstcollision is not None: cmd += 'ignorefirstcollision %.15e '%ignorefirstcollision if maxdeviationangle is not None: cmd += 'maxdeviationangle %.15e '%maxdeviationangle res = self.prob.SendCommand(cmd) if res is None: raise planning_error('MoveHandStraight') if outputtrajobj is not None and outputtrajobj: return RaveCreateTrajectory(self.prob.GetEnv(),'').deserialize(res) return res
[docs] def MoveManipulator(self,goal=None,maxiter=None,execute=None,outputtraj=None,maxtries=None,goals=None,steplength=None,outputtrajobj=None,jitter=None,releasegil=False): """See :ref:`module-basemanipulation-movemanipulator` """ if goal is not None: assert(len(goal) == len(self.robot.GetActiveManipulator().GetArmIndices())) return self._MoveJoints('MoveManipulator',goal=goal,steplength=steplength,maxiter=maxiter,maxtries=maxtries,execute=execute,outputtraj=outputtraj,goals=goals,outputtrajobj=outputtrajobj,jitter=jitter,releasegil=releasegil)
[docs] def MoveActiveJoints(self,goal=None,steplength=None,maxiter=None,maxtries=None,execute=None,outputtraj=None,goals=None,outputtrajobj=None,jitter=None,releasegil=False,postprocessingplanner=None,postprocessingparameters=None): """See :ref:`module-basemanipulation-moveactivejoints` """ if goal is not None: assert(len(goal) == self.robot.GetActiveDOF() and len(goal) > 0) return self._MoveJoints('MoveActiveJoints',goal=goal,steplength=steplength,maxiter=maxiter,maxtries=maxtries,execute=execute,outputtraj=outputtraj,goals=goals,outputtrajobj=outputtrajobj,jitter=jitter,releasegil=releasegil,postprocessingplanner=postprocessingplanner,postprocessingparameters=postprocessingparameters)
def _MoveJoints(self,cmd,goal=None,steplength=None,maxiter=None,maxtries=None,execute=None,outputtraj=None,goals=None,outputtrajobj=None,jitter=None,releasegil=False,postprocessingplanner=None,postprocessingparameters=None): """See :ref:`module-basemanipulation-moveactivejoints` """ cmd += ' ' if goal is not None: cmd += 'goal ' + ' '.join('%.15e'%f for f in goal) + ' ' if goals is not None: cmd += 'goals %d '%len(goals) for g in goals: for f in g: cmd += '%.15e '%f if steplength is not None: cmd += 'steplength %.15e '%steplength if execute is not None: cmd += 'execute %d '%execute if (outputtraj is not None and outputtraj) or (outputtrajobj is not None and outputtrajobj): cmd += 'outputtraj ' if maxiter is not None: cmd += 'maxiter %d '%maxiter if maxtries is not None: cmd += 'maxtries %d '%maxtries if jitter is not None: cmd += 'jitter %f '%jitter if postprocessingplanner is not None: cmd += 'postprocessingplanner %s\n'%postprocessingplanner if postprocessingparameters is not None: cmd += 'postprocessingparameters %s\n'%postprocessingparameters res = self.prob.SendCommand(cmd,releasegil=releasegil) if res is None: raise planning_error('MoveActiveJoints') if outputtrajobj is not None and outputtrajobj: return RaveCreateTrajectory(self.prob.GetEnv(),'').deserialize(res) return res
[docs] def MoveToHandPosition(self,matrices=None,affinedofs=None,maxiter=None,maxtries=None,translation=None,rotation=None,seedik=None,constraintfreedoms=None,constraintmatrix=None,constrainterrorthresh=None,execute=None,outputtraj=None,steplength=None,goalsamples=None,ikparam=None,ikparams=None,jitter=None,minimumgoalpaths=None,outputtrajobj=None,postprocessing=None,jittergoal=None, constrainttaskmatrix=None, constrainttaskpose=None,goalsampleprob=None,goalmaxsamples=None,goalmaxtries=None): """See :ref:`module-basemanipulation-movetohandposition` postprocessing is two parameters: (plannername,parmaeters) """ cmd = 'MoveToHandPosition ' if matrices is not None: cmd += 'matrices %d '%len(matrices) for m in matrices: cmd += matrixSerialization(m) + ' ' if maxiter is not None: cmd += 'maxiter %d '%maxiter if maxtries is not None: cmd += 'maxtries %d '%maxtries if translation is not None: cmd += 'translation %.15e %.15e %.15e '%(translation[0],translation[1],translation[2]) if rotation is not None: cmd += 'rotation %.15e %.15e %.15e %.15e '%(rotation[0],rotation[1],rotation[2],rotation[3]) if seedik is not None: cmd += 'seedik %d '%seedik if goalsamples is not None: cmd += 'goalsamples %d '%goalsamples if postprocessing is not None: cmd += 'postprocessingplanner %s\n postprocessingparameters %s\n '%(postprocessing[0],postprocessing[1]) if constraintfreedoms is not None: cmd += 'constraintfreedoms %s '%(' '.join(str(constraintfreedoms[i]) for i in range(6))) if constraintmatrix is not None: cmd += 'constraintmatrix %s '%matrixSerialization(constraintmatrix) if constrainttaskmatrix is not None: cmd += 'constrainttaskmatrix %s '%matrixSerialization(constrainttaskmatrix) if constrainterrorthresh is not None: cmd += 'constrainterrorthresh %s '%constrainterrorthresh if jitter is not None: cmd += 'jitter %.15e '%jitter if steplength is not None: cmd += 'steplength %.15e '%steplength if jittergoal is not None: cmd += 'jittergoal %.15e '%jittergoal if ikparam is not None: cmd += 'ikparam ' + str(ikparam) + ' ' if ikparams is not None: cmd += 'ikparams %d '%len(ikparams) for ikp in ikparams: cmd += str(ikp) + ' ' if execute is not None: cmd += 'execute %d '%execute if (outputtraj is not None and outputtraj) or (outputtrajobj is not None and outputtrajobj): cmd += 'outputtraj ' if minimumgoalpaths is not None: cmd += 'minimumgoalpaths %d '%minimumgoalpaths if goalsampleprob is not None: cmd += 'goalsampleprob %.15e '%goalsampleprob if goalmaxtries is not None: cmd += 'goalmaxtries %d '%goalmaxtries res = self.prob.SendCommand(cmd) if res is None: raise planning_error('MoveToHandPosition') if outputtrajobj is not None and outputtrajobj: return RaveCreateTrajectory(self.prob.GetEnv(),'').deserialize(res) return res
[docs] def MoveUnsyncJoints(self,jointvalues,jointinds,maxtries=None,planner=None,maxdivision=None,execute=None,outputtraj=None,outputtrajobj=None): """See :ref:`module-basemanipulation-moveunsyncjoints` """ assert(len(jointinds)==len(jointvalues) and len(jointinds)>0) cmd = 'MoveUnsyncJoints handjoints %d %s %s '%(len(jointinds),' '.join('%.15e'%f for f in jointvalues), ' '.join(str(f) for f in jointinds)) if planner is not None: cmd += 'planner %s '%planner if execute is not None: cmd += 'execute %d '%execute if (outputtraj is not None and outputtraj) or (outputtrajobj is not None and outputtrajobj): cmd += 'outputtraj ' if maxtries is not None: cmd += 'maxtries %d '%maxtries if maxdivision is not None: cmd += 'maxdivision %d '%maxdivision res = self.prob.SendCommand(cmd) if res is None: raise planning_error('MoveUnsyncJoints') if outputtrajobj is not None and outputtrajobj: return RaveCreateTrajectory(self.prob.GetEnv(),'').deserialize(res) return res
[docs] def JitterActive(self,maxiter=None,jitter=None,execute=None,outputtraj=None,outputfinal=None,outputtrajobj=None): """See :ref:`module-basemanipulation-jitteractive` """ cmd = 'JitterActive ' if maxiter is not None: cmd += 'maxiter %d '%maxiter if jitter is not None: cmd += 'jitter %.15e '%jitter if execute is not None: cmd += 'execute %d '%execute if (outputtraj is not None and outputtraj) or (outputtrajobj is not None and outputtrajobj): cmd += 'outputtraj ' if outputfinal: cmd += 'outputfinal' res = self.prob.SendCommand(cmd) if res is None: raise planning_error('JitterActive') resvalues = res.split() if outputfinal: final = numpy.array([numpy.float64(resvalues[i]) for i in range(self.robot.GetActiveDOF())]) resvalues=resvalues[len(final):] else: final=None if (outputtraj is not None and outputtraj) or (outputtrajobj is not None and outputtrajobj): traj = ' '.join(resvalues) else: traj = None if traj is not None and outputtrajobj is not None and outputtrajobj: traj = RaveCreateTrajectory(self.prob.GetEnv(),'').deserialize(traj) return final,traj
[docs] def FindIKWithFilters(self,ikparam,cone=None,solveall=None,filteroptions=None): """See :ref:`module-basemanipulation-findikwithfilters` """ cmd = 'FindIKWithFilters ikparam %s '%str(ikparam) if cone is not None: cmd += 'cone %s '%(' '.join('%.15e'%f for f in cone)) if solveall is not None and solveall: cmd += 'solveall ' if filteroptions is not None: cmd += 'filteroptions %d '%filteroptions res = self.prob.SendCommand(cmd) if res is None: raise planning_error('FindIKWithFilters') resvalues = res.split() num = int(resvalues[0]) dim = (len(resvalues)-1)/num solutions = numpy.reshape([numpy.float64(s) for s in resvalues[1:]],(num,dim)) return solutions

Questions/Feedback

Having problems with OpenRAVE?