def get_offset(env_mesh, robot_mesh):
ompl_setup = oa.SE3RigidBodyPlanning()
ompl_setup.setEnvironmentMesh(str(env_mesh))
ompl_setup.setRobotMesh(str(robot_mesh))
# full state
start = ompl_setup.getDefaultStartState()
# just the first geometric component
start = ompl_setup.getGeometricComponentState(start, 0)
# extract x,y,z coords
# print start
offset = {}
offset['x'] = start[0]
offset['y'] = start[1]
offset['z'] = start[2]
return offset
import sys
from os.path import abspath, dirname, join
ompl_app_root = dirname(dirname(dirname(abspath(__file__))))
ompl_resources_dir = join(ompl_app_root, 'resources/3D')
try:
from ompl import base as ob
from ompl import app as oa
except ImportError:
sys.path.insert(0, join(ompl_app_root, 'ompl/py-bindings'))
from ompl import base as ob
from ompl import app as oa
# plan in SE(3)
setup = oa.SE3RigidBodyPlanning()
# load the robot and the environment
setup.setRobotMesh(join(ompl_resources_dir, 'cubicles_robot.dae'))
setup.setEnvironmentMesh(join(ompl_resources_dir, 'cubicles_env.dae'))
# define start state
start = ob.State(setup.getSpaceInformation())
start().setX(-4.96)
start().setY(-40.62)
start().setZ(70.57)
start().rotation().setIdentity()
goal = ob.State(setup.getSpaceInformation())
goal().setX(200.49)
goal().setY(-40.62)