def main(env,options): "Main example code." while True: env.Reset() env.Load(options.scene) hanoi = HanoiPuzzle(env,env.GetRobots()[0],plannername=options.planner) hanoi.hanoisolve(3,hanoi.srcpeg,hanoi.destpeg,hanoi.peg) if options.

testmode: break

#!/usr/bin/env python # -*- coding: utf-8 -*# Copyright (C) 2009-2011 Rosen Diankov (rosen.diankov@gmail.com) # # 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. """Solves the hanoi problem using simple arm planning. .. examplepre-block:: hanoi Description ----------This example solves the Hanoi Puzzle using the Puma arm. You can easily change the locations of the pegs, disks, or add obstacles in the environment files **data/hanoi_complex.env.xml** and **data/hanoi.env.xml** to make the problem harder. The default planner used is the rBiRRT, you can easily change it to a different planner by changing the arguments to the BaseManipulation problem. .. examplepost-block:: hanoi """ from __future__ import with_statement # for python 2.5 __author__ = 'Rosen Diankov'

import time import openravepy if not __openravepy_build_doc__: from openravepy import * from numpy import * [docs]class HanoiPuzzle: def __init__(self,env,robot,plannername=None): self.env = env self.robot = robot # load the IK solver self.ikmodel = databases.inversekinematics.InverseKinematicsModel(robot=robot,iktype=I kParameterization.Type.Transform6D) if not self.ikmodel.load(): self.ikmodel.autogenerate() # autogenerate if one doesn't exist self.lmodel = databases.linkstatistics.LinkStatisticsModel(self.robot) if not self.lmodel.load(): self.lmodel.autogenerate() self.lmodel.setRobotWeights() self.lmodel.setRobotResolutions(xyzdelta=0.002) # the pegs are really thin print 'robot resolutions: ',robot.GetDOFResolutions() print 'robot weights: ',robot.GetDOFWeights() with self.env: # lock the environment self.basemanip = interfaces.BaseManipulation(self.robot,plannername=plannername) self.taskmanip = interfaces.TaskManipulation(self.robot,plannername=plannername) disknames = ['disk0','disk1','disk2'] self.heights = array([0.021,0.062,0.103])+0.01 disks = [] diskradius = [] for name in disknames: disk = env.GetKinBody(name) ab = disk.ComputeAABB() disk.radius = ab.extents()[1]-0.02 disks.append(disk) self.srcpeg = env.GetKinBody('srcpeg') self.destpeg = env.GetKinBody('destpeg') self.peg = env.GetKinBody('peg') self.srcpeg.disks = disks self.destpeg.disks = [] self.peg.disks = [] [docs] def waitrobot(self): """busy wait for robot completion""" while not self.robot.GetController().IsDone():

time.sleep(0.01) [docs] def MoveToPosition(self, values,indices): """uses a planner to safely move the hand to the preshape and returns the trajectory. move the robot out of the way so it can complete a preshape """ with self.robot: self.robot.SetActiveDOFs(indices) self.basemanip.MoveUnsyncJoints(jointvalues=values,jointinds=indices) self.waitrobot() with self.robot: # move the hand to the preshape self.robot.SetActiveDOFs(indices) self.basemanip.MoveActiveJoints(goal=values) self.waitrobot() [docs] def putblock(self, disk, srcpeg, destpeg, height): with self.env: srcpegbox = srcpeg.ComputeAABB() destpegbox = destpeg.ComputeAABB() # get all the transformations Thand = self.robot.GetActiveManipulator().GetTransform() Tdisk = disk.GetTransform() Tsrcpeg = srcpeg.GetTransform() Tpeg = destpeg.GetTransform() src_upvec = Tsrcpeg[0:3,2:3] dest_upvec = Tpeg[0:3,2:3] Tdiff = dot(linalg.inv(Tdisk), Thand)

# iterate across all possible orientations the destination peg can be in for ang in arange(-pi,pi,0.3): # find the dest position p = Tpeg[0:3,3:4] + height * dest_upvec R = dot(Tpeg[0:3,0:3], array(((cos(ang),sin(ang),0),(sin(ang),cos(ang),0),(0,0,1)))) T = dot(r_[c_[R,p], [[0,0,0,1]]], Tdiff) with self.env: # check the IK of the destination if self.robot.GetActiveManipulator().FindIKSolution(T,True) is None: continue # add two intermediate positions, one right above the source peg # and one right above the destination peg Tnewhand = array(Thand) Tnewhand[0:3,3:4] += src_upvec*(max(srcpegbox.extents())*2.5-0.02)

# check the IK of the destination if self.robot.GetActiveManipulator().FindIKSolution(Tnewhand,True) is None: print('Tnewhand invalid') continue Tnewhand2 = array(T) Tnewhand2[0:3,3:4] += dest_upvec*(max(destpegbox.extents())*2.5-height) # check the IK of the destination if self.robot.GetActiveManipulator().FindIKSolution(Tnewhand2,True) is None: print('Tnewhand2 invalid') continue try: self.basemanip.MoveToHandPosition(matrices=[Tnewhand],maxtries=1) raveLogInfo('move to position above source peg') self.waitrobot() # wait for robot to complete all trajectories

self.basemanip.MoveToHandPosition(matrices=[Tnewhand2],maxtries=1) raveLogInfo('move to position above dest peg') self.waitrobot() # wait for robot to complete all trajectories

self.basemanip.MoveToHandPosition(matrices=[T],maxtries=1) raveLogInfo('move to dest peg') self.waitrobot() # wait for robot to complete all trajectories return True except planning_error, e: raveLogWarn(str(e)) raise planning_error('failed to put block') [docs] def GetGrasp(self, Tdisk, radius, angles): """ returns the transform of the grasp given its orientation and the location/size of the disk""" zdir = dot(Tdisk[0:3,0:3],vstack([cos(angles[0])*cos(angles[1]),cos(angles[0])*sin(angles[1]),-sin(angles[0])])) pos = Tdisk[0:3,3:4] + radius*dot(Tdisk[0:3,0:3],vstack([cos(angles[1]),-sin(angles[1]),0])) xdir = cross(Tdisk[0:3,1:2],zdir,axis=0) xdir = xdir / linalg.norm(xdir) ydir = cross(zdir,xdir,axis=0) Tgrasp = r_[c_[xdir,ydir,zdir,pos],[[0,0,0,1]]]

return [Tgrasp,dot(Tgrasp, array([[-1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]))] [docs] def hanoimove(self, disk, srcpeg, destpeg, height): """Moves the arm and manipulator to grasp a peg and place it on a different peg""" openhandfn = lambda: self.MoveToPosition([0.7],self.robot.GetActiveManipulator().GetGripperIndices()) Tdisk = disk.GetTransform() for ang2 in arange(-pi/2,1.5*pi,0.4): for ang1 in arange(-0.6,0,0.2): Tgrasps = self.GetGrasp(Tdisk, disk.radius, [ang1,ang2]) # get the grasp transform given the two angles for Tgrasp in Tgrasps: # for each of the grasps try: raveLogInfo('opening hand') openhandfn() raveLogInfo('moving hand to location') self.basemanip.MoveToHandPosition(matrices=[Tgrasp],maxtries=1) self.waitrobot() raveLogInfo('succeeded so grab the disk') self.taskmanip.CloseFingers() self.waitrobot() with self.env: self.robot.Grab(disk) raveLogInfo('try to put the disk in the destination peg') self.putblock(disk, srcpeg, destpeg, height) raveLogInfo('wait for robot to complete all trajectories') self.waitrobot() self.taskmanip.ReleaseFingers(target=disk) self.waitrobot() raveLogInfo('done with one disk') return True except planning_error,e: raveLogWarn(str(e)) with self.env: self.robot.ReleaseAllGrabbed() disk.Enable(False) openhandfn() with self.env: disk.Enable(True) return False [docs] def hanoisolve(self, n, pegfrom, pegto, pegby): if n == 1: # move the disk disk = pegfrom.disks[-1]

print('hanoimove %s from %s to %s'%(disk.GetName(), pegfrom.GetName(), pegto.GetName())) if not self.hanoimove(disk, pegfrom, pegto, self.heights[len(pegto.disks)]): raise ValueError('failed to solve hanoi') # add the disk onto the correct peg list pegto.disks.append(disk) pegfrom.disks.pop() else: self.hanoisolve(n-1, pegfrom, pegby, pegto) self.hanoisolve(1, pegfrom, pegto, pegby) self.hanoisolve(n-1, pegby, pegto, pegfrom) [docs]def main(env,options): "Main example code." while True: env.Reset() env.Load(options.scene) hanoi = HanoiPuzzle(env,env.GetRobots()[0],plannername=options.planner) hanoi.hanoisolve(3,hanoi.srcpeg,hanoi.destpeg,hanoi.peg) if options.testmode: break 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='Manipulation planning example solving the hanoi problem.', usage='openrave.py --example hanoi [options]') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('-scene',action="store",type='string',dest='scene',default='data/hanoi_co mplex2.env.xml', help='Scene file to load (default=%default)') parser.add_option('-planner',action="store",type='string',dest='planner',default=None, help='the planner to use') (options, leftargs) = parser.parse_args(args=args) OpenRAVEGlobalArguments.parseAndCreateThreadedUser(options,main,default viewer=True) if __name__ == "__main__":

run()