Hannoi Code

7/29/2019 Hannoi Code

1/7

defmain(env,options):"Main example code."whileTrue:

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 ([email protected])

## 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 easilychange the locations of thepegs, 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 caneasily change it to a different planner by changing the arguments tothe BaseManipulation problem.

.. examplepost-block:: hanoi

"""from__future__import with_statement # for python 2.5__author__ ='Rosen Diankov'


2/7

importtimeimportopenravepyifnot __openravepy_build_doc__:

fromopenravepyimport*fromnumpyimport*

[docs]classHanoiPuzzle:def__init__(self,env,robot,plannername=None):

self.env = envself.robot = robot# load the IK solverself.ikmodel =

databases.inversekinematics.InverseKinematicsModel(robot=robot,iktype=I

kParameterization.Type.Transform6D)ifnotself.ikmodel.load():

self.ikmodel.autogenerate() # autogenerate if one doesn'texist

self.lmodel =databases.linkstatistics.LinkStatisticsModel(self.robot)

ifnotself.lmodel.load():self.lmodel.autogenerate()

self.lmodel.setRobotWeights()

self.lmodel.setRobotResolutions(xyzdelta=0.002) # the pegs arereally thin

print'robot resolutions: ',robot.GetDOFResolutions()print'robot weights: ',robot.GetDOFWeights()withself.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.01disks = []diskradius = []for name in disknames:

disk = env.GetKinBody(name)

ab = disk.ComputeAABB()disk.radius = ab.extents()[1]-0.02disks.append(disk)

self.srcpeg = env.GetKinBody('srcpeg')

self.destpeg = env.GetKinBody('destpeg')self.peg = env.GetKinBody('peg')self.srcpeg.disks = disksself.destpeg.disks = []self.peg.disks = []

[docs] defwaitrobot(self):

"""busy wait for robot completion"""whilenotself.robot.GetController().IsDone():
http://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzlehttp://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzle.waitrobothttp://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzle.waitrobothttp://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzle


3/7

time.sleep(0.01)

[docs] defMoveToPosition(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"""withself.robot:

self.robot.SetActiveDOFs(indices)

self.basemanip.MoveUnsyncJoints(jointvalues=values,jointinds=indices)self.waitrobot()withself.robot:

# move the hand to the preshapeself.robot.SetActiveDOFs(indices)

self.basemanip.MoveActiveJoints(goal=values)self.waitrobot()

[docs] defputblock(self, disk, srcpeg, destpeg, height):withself.env:

srcpegbox = srcpeg.ComputeAABB()destpegbox = destpeg.ComputeAABB()# get all the transformationsThand =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 pegcan be in

for ang in arange(-pi,pi,0.3):

# find the dest positionp = Tpeg[0:3,3:4] + height * dest_upvecR = 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)withself.env:

# check the IK of the destinationif

self.robot.GetActiveManipulator().FindIKSolution(T,True) isNone:continue

# add two intermediate positions, one right above thesource peg

# and one right above the destination pegTnewhand = array(Thand)

Tnewhand[0:3,3:4] +=src_upvec*(max(srcpegbox.extents())*2.5-0.02)
http://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzle.MoveToPositionhttp://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzle.putblockhttp://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzle.putblockhttp://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzle.MoveToPosition


4/7

# check the IK of the destinationif

self.robot.GetActiveManipulator().FindIKSolution(Tnewhand,True) isNone:

print('Tnewhand invalid')

continueTnewhand2 = array(T)Tnewhand2[0:3,3:4] +=

dest_upvec*(max(destpegbox.extents())*2.5-height)# check the IK of the destinationif

self.robot.GetActiveManipulator().FindIKSolution(Tnewhand2,True) isNone:

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

trajectoriesreturnTrue

except planning_error, e:raveLogWarn(str(e))

raise planning_error('failed to put block')

[docs] defGetGrasp(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]]]
http://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzle.GetGrasphttp://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzle.GetGrasp


5/7

return [Tgrasp,dot(Tgrasp, array([[-1,0,0,0],[0,1,0,0],[0,0,-1,0],[0,0,0,1]]))]

[docs] defhanoimove(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 graspstry:

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()withself.env:

self.robot.Grab(disk)

raveLogInfo('try to put the disk in thedestination 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')returnTrue

except planning_error,e:

raveLogWarn(str(e))withself.env:

self.robot.ReleaseAllGrabbed()disk.Enable(False)

openhandfn()withself.env:

disk.Enable(True)returnFalse

[docs] defhanoisolve(self, n, pegfrom, pegto, pegby):if n ==1:

# move the diskdisk = pegfrom.disks[-1]
http://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzle.hanoimovehttp://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzle.hanoisolvehttp://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzle.hanoisolvehttp://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.HanoiPuzzle.hanoimove


6/7

print('hanoimove%s from%s to%s'%(disk.GetName(),pegfrom.GetName(), pegto.GetName()))

ifnotself.hanoimove(disk, pegfrom, pegto,self.heights[len(pegto.disks)]):

raiseValueError('failed to solve hanoi')

# add the disk onto the correct peg listpegto.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]defmain(env,options):"Main example code."whileTrue:

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

fromoptparseimport OptionParserfromopenravepy.miscimport OpenRAVEGlobalArguments

@openravepy.with_destroy[docs]defrun(args=None):

"""Command-line execution of the example.

:param args: arguments for script to parse, if not specified willuse 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_complex2.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,defaultviewer=True)

if __name__ =="__main__":
http://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.mainhttp://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.runhttp://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.runhttp://openrave.org/docs/latest_stable/openravepy/examples.hanoi/#openravepy.examples.hanoi.main


7/7

run()

Documents

Hannoi Code