def environment_setup(self):
self.env = openravepy.Environment()
InitOpenRAVELogging()
module = openravepy.RaveCreateModule(self.env, 'urdf')
rospack = rospkg.RosPack()
with self.env:
# name = module.SendCommand('load /home/peppe/python_test/sawyer.urdf /home/peppe/python_test/sawyer_base.srdf')
name = module.SendCommand('loadURI ' +
rospack.get_path('fredsmp_utils') +
'/robots/sawyer/sawyer.urdf' + ' ' +
rospack.get_path('fredsmp_utils') +
'/robots/sawyer/sawyer.srdf')
self.robot = self.env.GetRobot(name)
time.sleep(0.5)
self.ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
self.robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not self.ikmodel.load():
self.ikmodel.autogenerate()
manip = self.robot.GetActiveManipulator()
self.robot.SetActiveDOFs(manip.GetArmIndices())
import atexit
from openravepy import *
import orgpmp2.orgpmp2
import time
import types
import prpy.kin
import prpy.rave
import sys
sys.path.append('data')
import problemsets
# Initialize openrave logging
from openravepy.misc import InitOpenRAVELogging
InitOpenRAVELogging()
# Problemset
problemset = 'industrial2'
# Start and end joint angles
n_states, states = problemsets.states(problemset)
start_joints = numpy.array(states[3])
end_joints = numpy.array(states[1])
# Set up openrave and load env
RaveInitialize(True, level=DebugLevel.Info)
atexit.register(RaveDestroy)
e = Environment()
atexit.register(e.Destroy)
e.Load(problemsets.env_file(problemset))
e.Load('data/robots/pr2_gpmp2spheres.robot.xml')
e.SetViewer('qtcoin')
## SYSTEM - MATH - FILE OPS ##
import sys
import os
from datetime import datetime
import time
import commands
from random import *
from itertools import izip
## Constraint Based Manipulation ##
from rodrigues import *
from TransformMatrix import *
from str2num import *
from TSR import *
InitOpenRAVELogging() # This removes the error "No handlers cold be found for logger openravepy"
env = Environment()
env.SetViewer('qtcoin')
RaveSetDebugLevel(1)
robots = []
# the following for loop will add N robots from the same xml file
# and rename them to prevent failure
for i in range(2):
robots.append(env.ReadRobotURI('robots/barrettwam.robot.xml'))
env.Add(robots[len(robots)-1]) # add the last robot in the environment so that we can rename it.
for body in env.GetBodies():
rname = body.GetName()