OpenRAVE Documentation

Source code for openravepy.examples.pr2turnlever

#!/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.
"""Shows how to set a workspace trajectory for the hand and have a robot plan it.

.. examplepre-block:: pr2turnlever

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

Shows how to instantiate a planner in python and pass in a workspace trajectory.

.. examplepost-block:: pr2turnlever

"""
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]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." env.Load(options.scene) target = env.GetKinBody('lever') if target is None: target = RaveCreateKinBody(env,'') target.InitFromBoxes(array([[0,0.1,0,0.01,0.1,0.01]]),True) target.SetName('lever') env.Add(target) T = eye(4) T[0:3,3] = [-0.2,-0.2,1] target.SetTransform(T) robot = env.GetRobots()[0] robot.SetActiveManipulator('rightarm') ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterizationType.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() with env: robot.SetActiveDOFs(ikmodel.manip.GetArmIndices()) basemanip=interfaces.BaseManipulation(robot) taskmanip=interfaces.TaskManipulation(robot) taskmanip.ReleaseFingers() robot.WaitForController(0) with env: Toffset = eye(4) Toffset[0:3,3] = array([0,0.2,0]) Ttarget0 = target.GetTransform() Ttarget1 = dot(Ttarget0,matrixFromAxisAngle([pi/2,0,0])) # with target.CreateKinBodyStateSaver(): # target.SetTransform(Ttarget1) # raw_input('as') Tgrasp0 = dot(matrixFromAxisAngle([pi/2,0,0]),matrixFromAxisAngle([0,pi/2,0])) Tgrasp0[0:3,3] = dot(Ttarget0,Toffset)[0:3,3] Tgraspoffset = dot(linalg.inv(Ttarget0),Tgrasp0) Tgrasp1 = dot(Ttarget1,Tgraspoffset) # check if ik solutions exist sol0=ikmodel.manip.FindIKSolution(Tgrasp0,IkFilterOptions.CheckEnvCollisions) assert(sol0 is not None) sol1=ikmodel.manip.FindIKSolution(Tgrasp1,IkFilterOptions.CheckEnvCollisions) assert(sol1 is not None) traj = RaveCreateTrajectory(env,'') spec = IkParameterization.GetConfigurationSpecificationFromType(IkParameterizationType.Transform6D,'linear') traj.Init(spec) for angle in arange(0,pi/2,0.05): Ttarget = dot(Ttarget0,matrixFromAxisAngle([angle,0,0])) Tgrasp = dot(Ttarget,Tgraspoffset) traj.Insert(traj.GetNumWaypoints(),poseFromMatrix(Tgrasp)) planningutils.RetimeAffineTrajectory(traj,maxvelocities=ones(7),maxaccelerations=5*ones(7)) h=misc.DrawAxes(env,Ttarget0) basemanip.MoveToHandPosition(matrices=[Tgrasp0]) robot.WaitForController(0) taskmanip.CloseFingers() robot.WaitForController(0) with env: robot.Grab(target) print 'planning for turning lever' planner = RaveCreatePlanner(env,'workspacetrajectorytracker') params = Planner.PlannerParameters() params.SetRobotActiveJoints(robot) params.SetExtraParameters('<workspacetrajectory>%s</workspacetrajectory>'%traj.serialize(0)) planner.InitPlan(robot,params) outputtraj = RaveCreateTrajectory(env,'') success=planner.PlanPath(outputtraj) assert(success) # also create reverse the trajectory and run infinitely trajectories = [outputtraj,planningutils.ReverseTrajectory(outputtraj)] while True: for traj in trajectories: robot.GetController().SetPath(traj) robot.WaitForController(0) 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='Shows how to set a workspace trajectory for the hand and have a robot plan it.', usage='openrave.py --example pr2turnlever [options]') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene',action="store",type='string',dest='scene',default='data/pr2test1.env.xml', help='scene 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?