OpenRAVE Documentation

Source code for openravepy.examples.dualarmdemo_schunk

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Copyright (C) 2009-2011 Achint Aggarwal
#
# 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.
#
#Created: 5 January 2010 
#Modified: 20 March 2010
"""Dual arm manipulation example.

.. examplepre-block:: dualarmdemo_schunk

.. examplepost-block:: dualarmdemo_schunk
"""
from __future__ import with_statement # for python 2.5
__author__ = 'Achint Aggarwal'

import time
import openravepy
if not __openravepy_build_doc__:
    from openravepy import *
    from numpy import *

from openravepy.misc import MultiManipIKSolver

[docs]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)
[docs] def WaitForController(self): while not self.robot.GetController().IsDone(): time.sleep(0.001)
[docs] def Serialize(self, T): return 'goal %s'%(' '.join(str(f) for f in T))
[docs] 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
[docs] 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
[docs] 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')
[docs] 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]])
[docs] 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]))
[docs] 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')
[docs] 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))
[docs]def main(env,options): "Main example code." env.Load(options.scene) schunk = Schunkplanner(env) time.sleep(1) while True: # initialize with env: jointvalues=schunk.robot.GetDOFValues() schunk.robot.SetActiveManipulator(schunk.rightArm) obj=env.GetKinBody('Object1') while True: Toriginal=obj.GetTransform() with obj: T=array(Toriginal) T[0:3,3] += 0.4*(random.rand(3)-0.5) # yz only obj.SetTransform(T) if not env.CheckCollision(obj): break obj.SetTransform(T) try: schunk.graspAndMoveObject(jointvalues,obj) schunk.WaitForController() print "Path Planning complete...." except planning_error: pass
from optparse import OptionParser from openravepy.misc import OpenRAVEGlobalArguments @openravepy.with_destroy
[docs]def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description="Schunk Manipulation planning example\nFor a dual arm robot with Schunk LWA3 arms, plan trajectories for grasping an object and manipulating it on a shelf.") OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene', action="store",type='string',dest='scene',default='data/dualarmmanipulation.env.xml', help='Scene file to load') (options, leftargs) = parser.parse_args(args=args) OpenRAVEGlobalArguments.parseAndCreateThreadedUser(options,main,defaultviewer=True)
if __name__ == "__main__": run()

Questions/Feedback

Having problems with OpenRAVE?