def __init__(self, env):
self.env = env
self.robot = self.env.GetRobots()[0]
self.probsmanip = RaveCreateModule(self.env, 'dualmanipulation')
args = self.robot.GetName()
#args += ' planner birrt'
self.env.Add(self.probsmanip, True, args)
self.leftArm = self.robot.GetManipulator('leftarm')
self.rightArm = self.robot.GetManipulator('rightarm')
self.dualsolver = MultiManipIKSolver([self.leftArm, self.rightArm])
for manip in [self.leftArm, self.rightArm]:
self.robot.SetActiveManipulator(manip)
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
self.robot, iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
self.robot.SetActiveManipulator(self.leftArm)
def __init__(self,env):
self.env = env
self.robot = self.env.GetRobots()[0]
self.probsmanip = RaveCreateModule(self.env,'dualmanipulation')
args = self.robot.GetName()
#args += ' planner birrt'
self.env.Add(self.probsmanip,True,args)
self.leftArm=self.robot.GetManipulator('leftarm')
self.rightArm=self.robot.GetManipulator('rightarm')
self.dualsolver = MultiManipIKSolver([self.leftArm,self.rightArm])
for manip in [self.leftArm,self.rightArm]:
self.robot.SetActiveManipulator(manip)
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(self.robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
self.robot.SetActiveManipulator(self.leftArm)
class Schunkplanner:
probsmanip = None
def __init__(self, env):
self.env = env
self.robot = self.env.GetRobots()[0]
self.probsmanip = RaveCreateModule(self.env, 'dualmanipulation')
args = self.robot.GetName()
#args += ' planner birrt'
self.env.Add(self.probsmanip, True, args)
self.leftArm = self.robot.GetManipulator('leftarm')
self.rightArm = self.robot.GetManipulator('rightarm')
self.dualsolver = MultiManipIKSolver([self.leftArm, self.rightArm])
for manip in [self.leftArm, self.rightArm]:
self.robot.SetActiveManipulator(manip)
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
self.robot, iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
self.robot.SetActiveManipulator(self.leftArm)
def WaitForController(self):
while not self.robot.GetController().IsDone():
time.sleep(0.001)
def Serialize(self, T):
return 'goal %s' % (' '.join(str(f) for f in T))
def MoveArmsToJointPosition(self, T):
"""Moves the two arms to the given joint position T"""
success = self.probsmanip.SendCommand('movealljoints ' +
self.Serialize(T))
return success is not None
def MoveObjectToPosition(self, T):
"""Constrained movement of the arms to new position T while holding the object"""
success = self.probsmanip.SendCommand('movealljoints ' +
self.Serialize(T) +
' constrainterrorthresh 1')
return success is not None
def planDualPath(self, obj):
"""this plans the trajectory for both the manipulators"""
Tbody = obj.GetTransform()
ab = obj.ComputeAABB().extents()
halfwidth = ab[1] #this is y
TRightGrasp = dot(
Tbody,
array([[0, 0, -1, 0], [1, 0, 0, (halfwidth + .1)], [0, -1, 0, 0],
[0, 0, 0, 1]]))
TLeftGrasp = dot(
Tbody,
array([[0, 0, -1, 0], [-1, 0, 0, -(halfwidth + .1)], [0, 1, 0, 0],
[0, 0, 0, 1]])
) #to determine the grasp for the eef given the transform of the object
solutions = self.dualsolver.findMultiIKSolution(
Tgrasps=[TLeftGrasp, TRightGrasp],
filteroptions=IkFilterOptions.CheckEnvCollisions)
if solutions is None or len(solutions) == 0:
raise planning_error('failed to find solution')
if not self.MoveArmsToJointPosition(r_[solutions[0], solutions[1]]):
print('failed to move to position next to object')
def moveObject(self, obj, delta):
"""this plans the trajectory for both the manipulators manipulating the object 'name' """
Tbody = obj.GetTransform()
ab = obj.ComputeAABB().extents()
halfwidth = ab[1] #this is y
Tbody[0:3, 3] += delta
TRightGrasp = dot(
Tbody,
array([[0, 0, -1, 0], [1, 0, 0, (halfwidth + .04)], [0, -1, 0, 0],
[0, 0, 0, 1]])) #.04 is just half the thickness of the EEF
TLeftGrasp = dot(
Tbody,
array([[0, 0, -1, 0], [-1, 0, 0, -(halfwidth + .04)], [0, 1, 0, 0],
[0, 0, 0, 1]])
) #to determine the grasp for the eef given the transform of the object
solutions = self.dualsolver.findMultiIKSolution(
Tgrasps=[TLeftGrasp, TRightGrasp],
filteroptions=IkFilterOptions.CheckEnvCollisions)
if solutions is None or len(solutions) == 0:
raise planning_error('failed to find solution')
self.MoveObjectToPosition(r_[solutions[0], solutions[1]])
def graspObject(self):
ThandR = self.robot.GetManipulators()[0].GetEndEffectorTransform()
ThandL = self.robot.GetManipulators()[1].GetEndEffectorTransform()
self.probsmanip.SendCommand('movebothhandsstraight direction1 %lf ' %
(ThandR[0, 3] - ThandL[0, 3]) + '%lf ' %
(ThandR[1, 3] - ThandL[1, 3]) + '%lf' %
(ThandR[2, 3] - ThandL[2, 3]) +
' direction0 %lf ' %
(ThandL[0, 3] - ThandR[0, 3]) + '%lf ' %
(ThandL[1, 3] - ThandR[1, 3]) + '%lf' %
(ThandL[2, 3] - ThandR[2, 3]))
def releaseObject(self):
ThandR = self.robot.GetManipulators()[0].GetEndEffectorTransform()
ThandL = self.robot.GetManipulators()[1].GetEndEffectorTransform()
self.probsmanip.SendCommand('movebothhandsstraight direction1 %lf ' %
(ThandL[0, 3] - ThandR[0, 3]) + '%lf ' %
(ThandL[1, 3] - ThandR[1, 3]) + '%lf' %
(ThandL[2, 3] - ThandR[2, 3]) +
' direction0 %lf ' %
(ThandR[0, 3] - ThandL[0, 3]) + '%lf ' %
(ThandR[1, 3] - ThandL[1, 3]) + '%lf' %
(ThandR[2, 3] - ThandL[2, 3]) +
' maxsteps 100')
def graspAndMoveObject(self, jointvalues, obj):
print('Moving to Grasping position for object: %s' % (obj))
self.planDualPath(obj)
self.WaitForController()
print('Grasping body %s' % (obj))
self.graspObject()
self.WaitForController()
print('Grabbing body %s' % (obj))
with self.env:
self.robot.Grab(obj, self.rightArm.GetEndEffector())
print('Moving body %s out of the shelf' % (obj))
self.moveObject(obj, delta=array([.2, 0.0, 0.0]))
self.WaitForController()
print('Moving body %s to final position' % (obj))
#change delta to give a new position
self.moveObject(obj, delta=array([-.20, -0.0, 0]))
self.WaitForController()
with self.env:
print('Releasing body %s' % (obj))
self.robot.ReleaseAllGrabbed()
self.releaseObject()
self.WaitForController()
print('Returning to Starting position')
self.MoveArmsToJointPosition(jointvalues)
self.WaitForController()
print('Body %s successfully manipulated' % (obj))
class Schunkplanner:
probsmanip = None
def __init__(self,env):
self.env = env
self.robot = self.env.GetRobots()[0]
self.probsmanip = RaveCreateModule(self.env,'dualmanipulation')
args = self.robot.GetName()
#args += ' planner birrt'
self.env.Add(self.probsmanip,True,args)
self.leftArm=self.robot.GetManipulator('leftarm')
self.rightArm=self.robot.GetManipulator('rightarm')
self.dualsolver = MultiManipIKSolver([self.leftArm,self.rightArm])
for manip in [self.leftArm,self.rightArm]:
self.robot.SetActiveManipulator(manip)
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(self.robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
self.robot.SetActiveManipulator(self.leftArm)
def WaitForController(self):
while not self.robot.GetController().IsDone():
time.sleep(0.001)
def Serialize(self, T):
return 'goal %s'%(' '.join(str(f) for f in T))
def MoveArmsToJointPosition(self, T):
"""Moves the two arms to the given joint position T"""
success = self.probsmanip.SendCommand('movealljoints '+self.Serialize(T))
return success is not None
def MoveObjectToPosition(self, T):
"""Constrained movement of the arms to new position T while holding the object"""
success = self.probsmanip.SendCommand('movealljoints '+self.Serialize(T)+' constrainterrorthresh 1')
return success is not None
def planDualPath(self,obj):
"""this plans the trajectory for both the manipulators"""
Tbody=obj.GetTransform()
ab = obj.ComputeAABB().extents()
halfwidth= ab[1] #this is y
TRightGrasp= dot(Tbody,array([[0, 0, -1, 0],[1, 0, 0, (halfwidth+.1)],[0, -1, 0, 0],[0, 0, 0, 1]]))
TLeftGrasp= dot(Tbody,array([[0, 0, -1, 0],[-1, 0, 0,-(halfwidth+.1)],[0, 1, 0, 0],[0, 0, 0, 1]])) #to determine the grasp for the eef given the transform of the object
solutions = self.dualsolver.findMultiIKSolution(Tgrasps=[TLeftGrasp,TRightGrasp],filteroptions=IkFilterOptions.CheckEnvCollisions)
if solutions is None or len(solutions) == 0:
raise planning_error('failed to find solution')
if not self.MoveArmsToJointPosition(r_[solutions[0],solutions[1]]):
print('failed to move to position next to object')
def moveObject(self,obj,delta):
"""this plans the trajectory for both the manipulators manipulating the object 'name' """
Tbody=obj.GetTransform()
ab = obj.ComputeAABB().extents()
halfwidth= ab[1] #this is y
Tbody[0:3,3]+=delta
TRightGrasp= dot(Tbody,array([[0, 0, -1, 0],[1, 0, 0, (halfwidth+.04)],[0, -1, 0, 0 ],[0, 0, 0, 1]])) #.04 is just half the thickness of the EEF
TLeftGrasp= dot(Tbody,array([[0, 0, -1, 0],[-1, 0, 0, -(halfwidth+.04)],[0, 1, 0, 0],[0, 0, 0, 1]])) #to determine the grasp for the eef given the transform of the object
solutions = self.dualsolver.findMultiIKSolution(Tgrasps=[TLeftGrasp,TRightGrasp],filteroptions=IkFilterOptions.CheckEnvCollisions)
if solutions is None or len(solutions) == 0:
raise planning_error('failed to find solution')
self.MoveObjectToPosition(r_[solutions[0],solutions[1]])
def graspObject(self):
ThandR=self.robot.GetManipulators()[0].GetEndEffectorTransform()
ThandL=self.robot.GetManipulators()[1].GetEndEffectorTransform()
self.probsmanip.SendCommand('movebothhandsstraight direction1 %lf ' %(ThandR[0,3]-ThandL[0,3]) +'%lf '%(ThandR[1,3]-ThandL[1,3]) +'%lf'%(ThandR[2,3]-ThandL[2,3]) +' direction0 %lf ' %(ThandL[0,3]-ThandR[0,3]) +'%lf '%(ThandL[1,3]-ThandR[1,3]) +'%lf'%(ThandL[2,3]-ThandR[2,3]))
def releaseObject(self):
ThandR=self.robot.GetManipulators()[0].GetEndEffectorTransform()
ThandL=self.robot.GetManipulators()[1].GetEndEffectorTransform()
self.probsmanip.SendCommand('movebothhandsstraight direction1 %lf ' %(ThandL[0,3]-ThandR[0,3]) +'%lf '%(ThandL[1,3]-ThandR[1,3]) +'%lf'%(ThandL[2,3]-ThandR[2,3]) +' direction0 %lf ' %(ThandR[0,3]-ThandL[0,3]) +'%lf '%(ThandR[1,3]-ThandL[1,3]) +'%lf'%(ThandR[2,3]-ThandL[2,3]) +' maxsteps 100')
def graspAndMoveObject(self,jointvalues,obj):
print('Moving to Grasping position for object: %s'%(obj))
self.planDualPath(obj)
self.WaitForController()
print('Grasping body %s'%(obj))
self.graspObject()
self.WaitForController()
print('Grabbing body %s'%(obj))
with self.env:
self.robot.Grab(obj,self.rightArm.GetEndEffector())
print('Moving body %s out of the shelf'%(obj))
self.moveObject(obj, delta=array([.2,0.0,0.0]))
self.WaitForController()
print('Moving body %s to final position'%(obj))
#change delta to give a new position
self.moveObject(obj,delta=array([-.20,-0.0,0]))
self.WaitForController()
with self.env:
print('Releasing body %s'%(obj))
self.robot.ReleaseAllGrabbed()
self.releaseObject()
self.WaitForController()
print('Returning to Starting position')
self.MoveArmsToJointPosition(jointvalues)
self.WaitForController()
print('Body %s successfully manipulated'%(obj))
