OpenRAVE Documentation

Source code for openravepy.examples.simplemanipulation

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Copyright (C) 2011 Alan Tan
#
# 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.
"""Explicitly specify goals to get a simple navigation and manipulation demo.

.. examplepre-block:: simplemanipulation

Description
-----------

This example shows how to string in a navigation and manipulation planner to achieve a simple goto -> grab -> move task.

.. examplepost-block:: simplemanipulation

"""
from __future__ import with_statement # for python 2.5
__author__= 'Alan Tan'

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

[docs]def waitrobot(robot): """busy wait for robot completion""" while not robot.GetController().IsDone(): time.sleep(0.01)
[docs]def main(env,options): "Main example code." # load a scene from ProjectRoom environment XML file env.Load('data/pr2test2.env.xml') time.sleep(1) # 1) get the 1st robot that is inside the loaded scene # 2) assign it to the variable named 'robot' robot = env.GetRobots()[0] manip = robot.SetActiveManipulator('leftarm_torso') # set the manipulator to leftarm + torso ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() # create the interface for basic manipulation programs basemanip = interfaces.BaseManipulation(robot,plannername=options.planner) taskprob = interfaces.TaskManipulation(robot,plannername=options.planner) target=env.GetKinBody('TibitsBox1') with env: jointnames = ['l_shoulder_lift_joint','l_elbow_flex_joint','l_wrist_flex_joint','r_shoulder_lift_joint','r_elbow_flex_joint','r_wrist_flex_joint'] robot.SetActiveDOFs([robot.GetJoint(name).GetDOFIndex() for name in jointnames]) basemanip.MoveActiveJoints(goal=[1.29023451,-2.32099996,-0.69800004,1.27843491,-2.32100002,-0.69799996]) waitrobot(robot) print 'move robot base to target' with env: robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1]) basemanip.MoveActiveJoints(goal=[2.8,-1.3,0],maxiter=5000,steplength=0.15,maxtries=2) waitrobot(robot) taskprob.ReleaseFingers() waitrobot(robot) print 'move the arm to the target' Tgoal = array([[0,-1,0,3.5],[-1,0,0,-1.3],[0,0,-1,0.842],[0,0,0,1]]) res = basemanip.MoveToHandPosition(matrices=[Tgoal],seedik=16) waitrobot(robot) print 'close fingers until collision' taskprob.CloseFingers() waitrobot(robot) print 'move the arm with the target back to the initial position' with env: robot.Grab(target) basemanip.MoveManipulator(goal=[0, 0, 1.29023451, 0, -2.32099996, 0, -0.69800004, 0]) waitrobot(robot) print 'move the robot to another location' with env: robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1]) localgoal = [0,2.4,0] T = robot.GetTransform() goal = dot(T[0:3,0:3],localgoal) + T[0:3,3] with robot: robot.SetActiveDOFValues(goal) incollision = env.CheckCollision(robot) if incollision: print 'goal in collision!!' basemanip.MoveActiveJoints(goal=goal,maxiter=5000,steplength=0.15,maxtries=2) waitrobot(robot) print 'move the arm to the designated position on another table to place the target down' Tgoal = array([[0,-1,0,3.5],[-1,0,0,1.5],[0,0,-1,0.855],[0,0,0,1]]) res = basemanip.MoveToHandPosition(matrices=[Tgoal],seedik=16) waitrobot(robot) taskprob.ReleaseFingers(target=target) waitrobot(robot) print 'move manipulator to initial position' basemanip.MoveManipulator(goal=[0, 0, 1.29023451, 0, -2.32099996, 0, -0.69800004, 0]) waitrobot(robot) print 'close fingers until collision' taskprob.CloseFingers() waitrobot(robot)
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='Explicitly specify goals to get a simple navigation and manipulation demo.', usage='openrave.py --example simplemanipulation [options]') OpenRAVEGlobalArguments.addOptions(parser) 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__": run()

Questions/Feedback

Having problems with OpenRAVE?