├── tests ├── __init__.py ├── planning │ ├── __init__.py │ ├── test_MacSmoother.py.disabled │ ├── test_OMPLSimplifier.py │ ├── test_ParabolicRetimer.py │ ├── test_OpenRAVEPlanner.py │ ├── test_GreedyIKPlanner.py │ ├── test_VectorFieldPlanner.py │ ├── test_OMPL.py │ ├── methods │ │ ├── __init__.py │ │ ├── SmoothTrajectory.py │ │ ├── ShortcutPath.py │ │ ├── PlanToEndEffectorPose.py │ │ ├── RetimeTrajectory.py │ │ ├── PlanToConfiguration.py │ │ └── PlanToEndEffectorOffset.py │ ├── test_SnapPlanner.py │ ├── test_CBiRRT.py │ ├── test_TrajoptPlanner.py │ ├── test_SequenceTests.py │ ├── test_HauserParabolicSmoother.py │ ├── test_RankedTests.py.disabled │ └── test_CHOMP.py └── test_grasp_cloning.py ├── Makefile ├── src └── prpy │ ├── tsr │ ├── __init__.py │ ├── kin.py │ ├── tsr.py │ ├── util.py │ ├── generic.py │ ├── rodrigues.py │ └── tsrlibrary.py │ ├── action │ ├── __init__.py │ └── actionlibrary.py │ ├── controllers │ ├── __init__.py │ ├── trigger_controller.py │ ├── position_command_controller.py │ └── rewd_controllers.py │ ├── compatibility.py │ ├── perception │ ├── meta.py │ ├── base.py │ ├── __init__.py │ ├── perception_helper.py │ ├── simulated.py │ ├── vncc.py │ ├── apriltags.py │ ├── rock_module.py │ ├── block_detector.py │ └── simtrack.py │ ├── simulation │ ├── __init__.py │ └── servo.py │ ├── base │ ├── __init__.py │ ├── endeffector.py │ └── mobilebase.py │ ├── __init__.py │ ├── exceptions.py │ ├── planning │ ├── __init__.py │ ├── adapters.py │ ├── named.py │ ├── mac_smoother.py │ ├── exceptions.py │ ├── sbpl.py │ ├── logged.py │ ├── ik.py │ ├── openrave.py │ └── snap.py │ ├── db.py │ ├── logger.py │ ├── ik_ranking.py │ ├── named_config.py │ ├── dependency_manager.py │ ├── viz.py │ └── tactile.py ├── .gitignore ├── CMakeLists.txt ├── setup.py ├── .travis.yml ├── LICENSE ├── package.xml └── scripts └── replay-log.py /tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /tests/planning/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /src/prpy/tsr/__init__.py: -------------------------------------------------------------------------------- 1 | import rodrigues, tsr, tsrlibrary 2 | from tsr import * -------------------------------------------------------------------------------- /src/prpy/tsr/kin.py: -------------------------------------------------------------------------------- 1 | # Stub file 2 | from __future__ import absolute_import 3 | from tsr.kin import * -------------------------------------------------------------------------------- /src/prpy/tsr/tsr.py: -------------------------------------------------------------------------------- 1 | # Stub file 2 | from __future__ import absolute_import 3 | from tsr.tsr import * -------------------------------------------------------------------------------- /src/prpy/tsr/util.py: -------------------------------------------------------------------------------- 1 | # Stub file 2 | from __future__ import absolute_import 3 | from tsr.util import * -------------------------------------------------------------------------------- /src/prpy/tsr/generic.py: -------------------------------------------------------------------------------- 1 | # Stub file 2 | from __future__ import absolute_import 3 | from tsr.generic import * -------------------------------------------------------------------------------- /src/prpy/tsr/rodrigues.py: -------------------------------------------------------------------------------- 1 | # Stub file 2 | from __future__ import absolute_import 3 | from tsr.rodrigues import * -------------------------------------------------------------------------------- /src/prpy/tsr/tsrlibrary.py: -------------------------------------------------------------------------------- 1 | # Stub file 2 | from __future__ import absolute_import 3 | from tsr.tsrlibrary import * -------------------------------------------------------------------------------- /src/prpy/action/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from actionlibrary import ActionLibrary, ActionMethod, ActionError 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | 3 | # Generated by nosetest 4 | *.noseids 5 | 6 | # Generated by CBiRRT 7 | **/cmovetraj.txt 8 | 9 | doc 10 | *~ 11 | -------------------------------------------------------------------------------- /src/prpy/controllers/__init__.py: -------------------------------------------------------------------------------- 1 | from rewd_controllers import OrController, RewdOrController, RewdOrTrajectoryController 2 | from position_command_controller import PositionCommandController 3 | from trigger_controller import TriggerController 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(prpy) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | catkin_python_setup() 7 | 8 | if (CATKIN_ENABLE_TESTING) 9 | catkin_add_nosetests(tests) 10 | endif() 11 | -------------------------------------------------------------------------------- /tests/planning/test_MacSmoother.py.disabled: -------------------------------------------------------------------------------- 1 | from methods import SmoothTrajectoryTest 2 | from planning_helpers import BasePlannerTest 3 | from prpy.planning.mac_smoother import MacSmoother 4 | from unittest import TestCase 5 | 6 | class MacSmootherTests(BasePlannerTest, 7 | SmoothTrajectoryTest, 8 | TestCase): 9 | planner_factory = MacSmoother 10 | -------------------------------------------------------------------------------- /tests/planning/test_OMPLSimplifier.py: -------------------------------------------------------------------------------- 1 | from methods import ShortcutPathTest 2 | from planning_helpers import BasePlannerTest 3 | from prpy.planning.ompl import OMPLSimplifier 4 | from unittest import TestCase 5 | 6 | 7 | class OMPLSimplifierTests(BasePlannerTest, 8 | ShortcutPathTest, 9 | TestCase): 10 | planner_factory = OMPLSimplifier 11 | -------------------------------------------------------------------------------- /tests/planning/test_ParabolicRetimer.py: -------------------------------------------------------------------------------- 1 | from methods import RetimeTrajectoryTest 2 | from planning_helpers import BasePlannerTest 3 | from prpy.planning.retimer import ParabolicRetimer 4 | from unittest import TestCase 5 | 6 | class ParabolicRetimerTests(BasePlannerTest, 7 | RetimeTrajectoryTest, 8 | TestCase): 9 | planner_factory = ParabolicRetimer 10 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from distutils.core import setup 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | 5 | d = generate_distutils_setup( 6 | packages=[ 7 | 'prpy', 8 | 'prpy.action', 9 | 'prpy.base', 10 | 'prpy.controllers', 11 | 'prpy.perception', 12 | 'prpy.planning', 13 | 'prpy.simulation', 14 | ], 15 | package_dir={'': 'src'}, 16 | ) 17 | setup(**d) 18 | -------------------------------------------------------------------------------- /tests/planning/test_OpenRAVEPlanner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from methods import ( 3 | PlanToConfigurationTest, 4 | ) 5 | from planning_helpers import BasePlannerTest 6 | from prpy.planning.openrave import OpenRAVEPlanner 7 | from unittest import TestCase 8 | 9 | 10 | class OpenRAVEPlannerTest(BasePlannerTest, 11 | PlanToConfigurationTest, 12 | TestCase): 13 | planner_factory = OpenRAVEPlanner 14 | -------------------------------------------------------------------------------- /tests/planning/test_GreedyIKPlanner.py: -------------------------------------------------------------------------------- 1 | from methods import PlanToEndEffectorOffsetTest 2 | from methods.PlanToEndEffectorOffset import PlanToEndEffectorOffsetCollisionTest 3 | from planning_helpers import BasePlannerTest 4 | from prpy.planning.workspace import GreedyIKPlanner 5 | from unittest import TestCase 6 | 7 | 8 | class GreedyIKPlannerTest(BasePlannerTest, 9 | PlanToEndEffectorOffsetTest, 10 | PlanToEndEffectorOffsetCollisionTest, 11 | TestCase): 12 | planner_factory = GreedyIKPlanner 13 | -------------------------------------------------------------------------------- /tests/planning/test_VectorFieldPlanner.py: -------------------------------------------------------------------------------- 1 | from methods import PlanToEndEffectorOffsetTest 2 | from methods.PlanToEndEffectorOffset import PlanToEndEffectorOffsetCollisionTest 3 | from planning_helpers import BasePlannerTest 4 | from prpy.planning.vectorfield import VectorFieldPlanner 5 | from unittest import TestCase 6 | 7 | 8 | class VectorFieldPlannerTest(BasePlannerTest, 9 | PlanToEndEffectorOffsetTest, 10 | PlanToEndEffectorOffsetCollisionTest, 11 | TestCase): 12 | planner_factory = VectorFieldPlanner 13 | -------------------------------------------------------------------------------- /src/prpy/compatibility.py: -------------------------------------------------------------------------------- 1 | from distutils.version import LooseVersion 2 | import numpy 3 | 4 | # handle older versions of numpy without norm axis argument 5 | if LooseVersion(numpy.__version__) < LooseVersion('1.8.0'): 6 | oldnorm = numpy.linalg.norm 7 | def newnorm(x, ord=None, axis=None): 8 | if axis is None: 9 | return oldnorm(x, ord=ord) 10 | elif isinstance(axis, (int,long)): 11 | return numpy.apply_along_axis(lambda x: oldnorm(x,ord=ord), axis, x) 12 | else: 13 | raise NotImplementedError('older numpy without norm axis') 14 | numpy.linalg.norm = newnorm 15 | -------------------------------------------------------------------------------- /tests/planning/test_OMPL.py: -------------------------------------------------------------------------------- 1 | from methods import ( 2 | PlanToConfigurationTest, 3 | PlanToConfigurationStraightLineTest, 4 | PlanToConfigurationCompleteTest, 5 | ) 6 | from planning_helpers import BasePlannerTest 7 | from prpy.planning.ompl import OMPLRangedPlanner 8 | from unittest import TestCase 9 | 10 | 11 | class OMPLPlannerTests(BasePlannerTest, 12 | PlanToConfigurationTest, 13 | PlanToConfigurationStraightLineTest, 14 | PlanToConfigurationCompleteTest, 15 | TestCase): 16 | planner_factory = lambda _: OMPLRangedPlanner(fraction=0.2) 17 | -------------------------------------------------------------------------------- /src/prpy/perception/meta.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import functools 4 | 5 | from base import PerceptionModule, PerceptionMethod 6 | 7 | class MetaModule(PerceptionModule): 8 | def __init__(self, modules): 9 | super(PerceptionModule, self).__init__() 10 | self.modules = modules; 11 | 12 | def __str__(self): 13 | return 'MetaModule' 14 | 15 | class RunAll(MetaModule): 16 | def __init__(self, modules): 17 | super(MetaModule, self).__init__(modules) 18 | 19 | def __str__(self): 20 | return 'RunAll' 21 | 22 | @PerceptionMethod 23 | def DetectObjects(self, robot, **kw_args): 24 | for module in modules: 25 | module.DetectObjects(robot, kw_args); 26 | -------------------------------------------------------------------------------- /tests/planning/methods/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | __all__ = [ 4 | 'PlanToConfigurationTest', 5 | 'PlanToEndEffectorPoseTest', 6 | 'PlanToEndEffectorOffsetTest', 7 | 'RetimeTrajectoryTest', 8 | 'ShortcutPathTest', 9 | 'SmoothTrajectoryTest', 10 | ] 11 | 12 | from PlanToConfiguration import ( 13 | PlanToConfigurationTest, 14 | PlanToConfigurationCompleteTest, 15 | PlanToConfigurationStraightLineTest, 16 | ) 17 | from PlanToEndEffectorPose import PlanToEndEffectorPoseTest 18 | from PlanToEndEffectorOffset import PlanToEndEffectorOffsetTest 19 | from RetimeTrajectory import RetimeTrajectoryTest 20 | from ShortcutPath import ShortcutPathTest 21 | from SmoothTrajectory import SmoothTrajectoryTest 22 | -------------------------------------------------------------------------------- /tests/planning/test_SnapPlanner.py: -------------------------------------------------------------------------------- 1 | from methods import ( 2 | PlanToConfigurationTest, 3 | PlanToConfigurationStraightLineTest, 4 | ) 5 | from methods.PlanToConfiguration import PlanToConfigurationTestCollisionTest 6 | from planning_helpers import BasePlannerTest 7 | from prpy.planning.snap import SnapPlanner 8 | from unittest import TestCase 9 | 10 | 11 | class SnapPlannerTest(BasePlannerTest, 12 | PlanToConfigurationTest, 13 | PlanToConfigurationStraightLineTest, 14 | PlanToConfigurationTestCollisionTest, 15 | TestCase): 16 | planner_factory = SnapPlanner 17 | 18 | def setUp(self): 19 | super(SnapPlannerTest, self).setUp() 20 | 21 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: trusty 2 | sudo: required 3 | language: generic 4 | cache: 5 | - apt 6 | # Install test fixture dependencies. 7 | before_install: 8 | - mkdir -p "${HOME}/workspace/src" 9 | - cd "${HOME}/workspace" 10 | - curl -sSo distribution.yml "${DISTRIBUTION}" 11 | - git clone https://github.com/personalrobotics/pr-cleanroom.git scripts 12 | - ./scripts/internal-setup.sh 13 | - export PACKAGE_NAMES="$(./scripts/internal-get-packages.py distribution.yml ${REPOSITORY})" 14 | install: 15 | - mv "${TRAVIS_BUILD_DIR}" src 16 | - ./scripts/internal-distro.py --workspace=src distribution.yml --repository "${REPOSITORY}" 17 | script: 18 | - ./scripts/internal-build.sh ${PACKAGE_NAMES} 19 | - travis_wait ./scripts/internal-test.sh ${PACKAGE_NAMES} 20 | after_script: 21 | - ./scripts/view-all-results.sh test_results 22 | -------------------------------------------------------------------------------- /tests/planning/test_CBiRRT.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from methods import ( 3 | PlanToConfigurationTest, 4 | PlanToConfigurationStraightLineTest, 5 | PlanToConfigurationCompleteTest, 6 | PlanToEndEffectorPoseTest, 7 | PlanToEndEffectorOffsetTest, 8 | ) 9 | from planning_helpers import BasePlannerTest 10 | from prpy.planning.cbirrt import CBiRRTPlanner 11 | from unittest import TestCase 12 | 13 | 14 | class CBiRRTPlannerTest(BasePlannerTest, 15 | PlanToConfigurationTest, 16 | PlanToConfigurationStraightLineTest, 17 | PlanToConfigurationCompleteTest, 18 | PlanToEndEffectorPoseTest, 19 | PlanToEndEffectorOffsetTest, 20 | TestCase): 21 | planner_factory = CBiRRTPlanner 22 | -------------------------------------------------------------------------------- /tests/planning/test_TrajoptPlanner.py: -------------------------------------------------------------------------------- 1 | from methods import ( 2 | PlanToConfigurationTest, 3 | PlanToConfigurationStraightLineTest, 4 | PlanToEndEffectorPoseTest, 5 | ) 6 | from methods.PlanToConfiguration import ( 7 | PlanToConfigurationTestCollisionTest, 8 | PlanToConfigurationCompleteTest, 9 | ) 10 | from planning_helpers import BasePlannerTest 11 | from unittest import TestCase 12 | from or_trajopt import TrajoptPlanner 13 | 14 | 15 | class TrajoptPlannerTest(BasePlannerTest, 16 | PlanToConfigurationTest, 17 | PlanToEndEffectorPoseTest, 18 | PlanToConfigurationCompleteTest, 19 | PlanToConfigurationTestCollisionTest, 20 | PlanToConfigurationStraightLineTest, 21 | TestCase): 22 | planner_factory = TrajoptPlanner 23 | 24 | def setUp(self): 25 | super(TrajoptPlannerTest, self).setUp() 26 | -------------------------------------------------------------------------------- /tests/planning/methods/SmoothTrajectory.py: -------------------------------------------------------------------------------- 1 | 2 | class SmoothTrajectoryTest(object): 3 | def setUp(self): 4 | from openravepy import RaveCreateTrajectory 5 | 6 | cspec = self.robot.GetActiveConfigurationSpecification('linear') 7 | 8 | self.feasible_path = RaveCreateTrajectory(self.env, '') 9 | self.feasible_path.Init(cspec) 10 | self.feasible_path.Insert(0, self.waypoint1) 11 | self.feasible_path.Insert(1, self.waypoint2) 12 | self.feasible_path.Insert(2, self.waypoint3) 13 | 14 | def test_SmoothTrajectory_DoesNotModifyBoundaryPoints(self): 15 | from numpy.testing import assert_allclose 16 | 17 | # Setup/Test 18 | traj = self.planner.RetimeTrajectory(self.robot, self.feasible_path) 19 | 20 | # Assert 21 | cspec = self.robot.GetActiveConfigurationSpecification('linear') 22 | self.assertGreaterEqual(traj.GetNumWaypoints(), 2) 23 | 24 | first_waypoint = traj.GetWaypoint(0, cspec) 25 | last_waypoint = traj.GetWaypoint(traj.GetNumWaypoints() - 1, cspec) 26 | assert_allclose(first_waypoint, self.waypoint1) 27 | assert_allclose(last_waypoint, self.waypoint3) 28 | 29 | -------------------------------------------------------------------------------- /src/prpy/action/actionlibrary.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | class ActionError(Exception): 4 | pass 5 | 6 | class ActionMethod(object): 7 | 8 | def __init__(self, func): 9 | """ 10 | Register this action with the action library 11 | """ 12 | self.func = func 13 | ActionLibrary.add_action(func) 14 | 15 | def __call__(self, instance, robot, *args, **kw_args): 16 | self.func(instance, robot, *args, **kw_args) 17 | 18 | class ActionLibrary(object): 19 | actions = [] 20 | 21 | def has_action(self, name): 22 | all_actions = self.get_actions() 23 | return name in all_actions 24 | 25 | def get_action(self, name): 26 | for a in self.actions: 27 | if a.__name__ == name: 28 | return a 29 | return None 30 | 31 | def get_actions(self): 32 | """ 33 | Return all the action names registered 34 | """ 35 | return [action.__name__ for action in self.actions] 36 | 37 | @classmethod 38 | def add_action(cls, func): 39 | """ 40 | @param func The action method to register 41 | """ 42 | cls.actions.append(func) 43 | 44 | -------------------------------------------------------------------------------- /tests/planning/test_SequenceTests.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | from planning_helpers import FailPlanner, MetaPlannerTests, SuccessPlanner 3 | from prpy.planning.base import Sequence 4 | 5 | 6 | class SequenceTests(MetaPlannerTests, 7 | TestCase): 8 | def test_FirstPlannerSucceeds_SecondPlannerIsNotCalled(self): 9 | first_planner = SuccessPlanner(self.traj) 10 | second_planner = SuccessPlanner(self.traj) 11 | 12 | planner = Sequence(first_planner, second_planner) 13 | planner.PlanTest(self.robot) 14 | 15 | self.assertEqual(first_planner.num_calls, 1) 16 | self.assertEqual(second_planner.num_calls, 0) 17 | 18 | def test_FirstPlannerFails_SecondPlannerIsCalled(self): 19 | first_planner = FailPlanner() 20 | second_planner = SuccessPlanner(self.traj) 21 | 22 | planner = Sequence(first_planner, second_planner) 23 | planner.PlanTest(self.robot) 24 | 25 | self.assertEqual(first_planner.num_calls, 1) 26 | self.assertEqual(second_planner.num_calls, 1) 27 | 28 | def test_AllPlannersFails_ThrowsPlanningError(self): 29 | from prpy.planning.base import PlanningError 30 | 31 | planner = Sequence(FailPlanner(), FailPlanner()) 32 | 33 | with self.assertRaises(PlanningError): 34 | planner.PlanTest(self.robot) 35 | -------------------------------------------------------------------------------- /tests/planning/methods/ShortcutPath.py: -------------------------------------------------------------------------------- 1 | 2 | class ShortcutPathTest(object): 3 | def setUp(self): 4 | from openravepy import RaveCreateTrajectory 5 | 6 | self.input_path = RaveCreateTrajectory(self.env, '') 7 | self.input_path.Init( 8 | self.robot.GetActiveConfigurationSpecification('linear')) 9 | self.input_path.Insert(0, self.waypoint1) 10 | self.input_path.Insert(1, self.waypoint2) 11 | self.input_path.Insert(2, self.waypoint3) 12 | 13 | def test_ShortcutPath_ShortcutExists_ReducesLength(self): 14 | from numpy.testing import assert_allclose 15 | 16 | # Setup/Test 17 | smoothed_path = self.planner.ShortcutPath(self.robot, self.input_path) 18 | 19 | # Assert 20 | self.assertEquals(smoothed_path.GetConfigurationSpecification(), 21 | self.input_path.GetConfigurationSpecification()) 22 | self.assertGreaterEqual(smoothed_path.GetNumWaypoints(), 2) 23 | 24 | n = smoothed_path.GetNumWaypoints() 25 | assert_allclose(smoothed_path.GetWaypoint(0), self.waypoint1) 26 | assert_allclose(smoothed_path.GetWaypoint(n - 1), self.waypoint3) 27 | 28 | self.assertLess(self.ComputeArcLength(smoothed_path), 29 | 0.5 * self.ComputeArcLength(self.input_path)) 30 | 31 | # TODO: Test some of the error cases. 32 | -------------------------------------------------------------------------------- /tests/planning/test_HauserParabolicSmoother.py: -------------------------------------------------------------------------------- 1 | from methods import RetimeTrajectoryTest, SmoothTrajectoryTest 2 | from planning_helpers import BasePlannerTest 3 | from prpy.planning.retimer import HauserParabolicSmoother 4 | from unittest import TestCase 5 | 6 | # HauserParabolicSmoother should just time the trajectory, without changing the 7 | # geometric path, if do_blend and do_shortcut are False. 8 | class HauserParabolicSmootherRetimingTests(BasePlannerTest, 9 | RetimeTrajectoryTest, 10 | TestCase): 11 | planner_factory = lambda _: HauserParabolicSmoother( 12 | do_blend=False, do_shortcut=False) 13 | 14 | 15 | # Both blending and shortcutting should reduce the duration of the trajectory. 16 | class HauserParabolicSmootherSmoothingTests(BasePlannerTest, 17 | SmoothTrajectoryTest, 18 | TestCase): 19 | planner_factory = lambda _: HauserParabolicSmoother( 20 | do_blend=False, do_shortcut=True) 21 | 22 | 23 | class HauserParabolicSmootherBlendingTests(BasePlannerTest, 24 | SmoothTrajectoryTest, 25 | TestCase): 26 | planner_factory = lambda _: HauserParabolicSmoother( 27 | do_blend=True, do_shortcut=False) 28 | -------------------------------------------------------------------------------- /src/prpy/perception/base.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import functools 4 | 5 | class PerceptionException(Exception): 6 | pass 7 | 8 | class PerceptionMethod(object): 9 | 10 | def __init__(self, func): 11 | self.func = func 12 | 13 | def __call__(self, instance, robot, *args, **kw_args): 14 | return self.func(instance, robot, *args, **kw_args) 15 | 16 | def __get__(self, instance, instancetype): 17 | # Bind the self reference and use update_wrapper to propagate the 18 | # function's metadata (e.g. name and docstring). 19 | wrapper = functools.partial(self.__call__, instance) 20 | functools.update_wrapper(wrapper, self.func) 21 | wrapper.is_perception_method = True 22 | return wrapper 23 | 24 | class PerceptionModule(object): 25 | def has_perception_method(self, method_name): 26 | """ 27 | Check if this module has the desired PerceptionMethod 28 | """ 29 | if hasattr(self, method_name): 30 | method = getattr(self, method_name) 31 | if hasattr(method, 'is_perception_method'): 32 | return method.is_perception_method 33 | else: 34 | return False 35 | else: 36 | return False 37 | 38 | def get_perception_method_names(self): 39 | """ 40 | @return A list of all the PerceptionMethod functions 41 | defined for this module 42 | """ 43 | return filter(lambda method_name: self.has_perception_method(method_name), dir(self)) 44 | 45 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2013, Carnegie Mellon University 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | - Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | - Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | - Neither the name of Carnegie Mellon University nor the names of its 13 | contributors may be used to endorse or promote products derived from this 14 | software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | POSSIBILITY OF SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /src/prpy/controllers/trigger_controller.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | from . import OrController 4 | 5 | 6 | class TriggerController(OrController): 7 | """A controller for triggering actions 8 | 9 | An interface for using trigger controller 10 | is "compatible" with how we use OpenRAVE controllers 11 | """ 12 | def __init__(self, namespace, controller_name, simulated=False): 13 | self.logger = logging.getLogger(__name__) 14 | self.namespace = namespace 15 | self.controller_name = controller_name 16 | self._current_cmd = None 17 | self.simulated = simulated 18 | if not simulated: 19 | from ros_control_client_py import TriggerClient 20 | self.controller_client = TriggerClient(namespace, controller_name) 21 | self.logger.info("Trigger Controller initialized") 22 | 23 | def Trigger(self, timeout=None): 24 | """Tigger the controlled action 25 | 26 | :param timeout: if not None, block until timeout 27 | :type timeout: double or None 28 | """ 29 | if self.simulated: 30 | pass 31 | 32 | if not self.IsDone(): 33 | from ros_control_client_py import TriggerFailed 34 | raise TriggerFailed("Trigger action already \ 35 | in progress and cannot be preempted", 36 | None) 37 | 38 | if not self.simulated: 39 | self._current_cmd = self.controller_client.execute() 40 | if timeout is not None and timeout >= 0.0: 41 | self._current_cmd.result(timeout) 42 | 43 | def IsDone(self): 44 | return (self.simulated or 45 | self._current_cmd is None or 46 | self._current_cmd.done()) 47 | -------------------------------------------------------------------------------- /src/prpy/controllers/position_command_controller.py: -------------------------------------------------------------------------------- 1 | import logging 2 | from . import OrController 3 | 4 | 5 | class PositionCommandController(OrController): 6 | """A controller for stateful position control 7 | 8 | An interface to using the position_command_controller package 9 | that is "compatible" with how we use OpenRAVE controllers 10 | """ 11 | def __init__(self, namespace, controller_name, simulated=False, 12 | connection_timeout=10.0): 13 | if simulated: 14 | raise NotImplementedError('Simulation not supported on ' 15 | 'PositionCommandController') 16 | from ros_control_client_py import SetPositionClient 17 | self.logger = logging.getLogger(__name__) 18 | self.namespace = namespace 19 | self.controller_name = controller_name 20 | self.controller_client = SetPositionClient(namespace, controller_name, 21 | connection_timeout) 22 | self._current_cmd = None 23 | self.logger.info('Position Command Controller {}/{} initialized' 24 | .format(namespace, controller_name)) 25 | 26 | def SetDesired(self, position): 27 | from ros_control_client_py import SetPositionFailed 28 | if not self.IsDone(): 29 | raise SetPositionFailed('PositionCommand action already ' 30 | 'in progress and cannot be preempted', 31 | position, None) 32 | 33 | self.logger.info('Sending position: {}'.format(position)) 34 | self._current_cmd = self.controller_client.execute(position) 35 | 36 | def IsDone(self): 37 | return self._current_cmd is None or self._current_cmd.done() 38 | -------------------------------------------------------------------------------- /src/prpy/simulation/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | from servo import ServoSimulator 32 | -------------------------------------------------------------------------------- /src/prpy/base/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | from robot import Robot 32 | from manipulator import Manipulator 33 | from endeffector import EndEffector 34 | from mobilebase import MobileBase 35 | -------------------------------------------------------------------------------- /src/prpy/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import base, dependency_manager, logger, ik_ranking, planning, perception, simulation, tsr, viz 32 | from named_config import ConfigurationLibrary 33 | from clone import Clone, Cloned 34 | from bind import bind_subclass 35 | import compatibility -------------------------------------------------------------------------------- /src/prpy/perception/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2015, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Jennifer King 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | from base import PerceptionModule, PerceptionMethod 32 | from apriltags import ApriltagsModule 33 | from simulated import SimulatedPerceptionModule 34 | from rock_module import RockModule 35 | from simtrack import SimtrackModule 36 | from perception_helper import RemoveAllObjects 37 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | prpy 4 | 4.0.0 5 | 6 | Python utilities used by the Personal Robotics Laboratory. 7 | 8 | https://github.com/personalrobotics/prpy 9 | https://github.com/personalrobotics/prpy/issues 10 | https://github.com/personalrobotics/prpy.git 11 | Michael Koval 12 | Michael Koval 13 | Pras Velagapudi 14 | Jennifer King 15 | Siddhartha Srinivasa 16 | Chris Dellin 17 | Shervin Javdani 18 | Stefanos Nikolaidis 19 | BSD 20 | catkin 21 | python 22 | rospy 23 | python-catkin-pkg 24 | python-enum 25 | python-numpy 26 | geometry_msgs 27 | manipulation2 28 | openrave 29 | openrave_catkin 30 | python-scipy 31 | python-termcolor 32 | python-trollius 33 | python-rospkg 34 | python-yaml 35 | python-lxml 36 | tsr 37 | python-nose 38 | 39 | cbirrt2 40 | tsr 41 | or_cdchomp 42 | or_ompl 43 | or_parabolicsmoother 44 | or_trajopt 45 | 46 | doxypy 47 | 48 | -------------------------------------------------------------------------------- /src/prpy/exceptions.py: -------------------------------------------------------------------------------- 1 | class PrPyException(Exception): 2 | """ 3 | Generic PrPy exception. 4 | """ 5 | 6 | 7 | class TrajectoryException(PrPyException): 8 | """ 9 | Trajectory failed to execute. 10 | """ 11 | 12 | 13 | class TrajectoryNotExecutable(TrajectoryException): 14 | """ 15 | Trajectory could not begin execution. 16 | 17 | This exception typically indicates that some precondition of trajectory 18 | execution was violated, such as the robot starting at a different 19 | configuration, the trajectory not being in the correct format. 20 | 21 | This exception indicates that the trajectory was not even attempted due to 22 | one of these conditions. 23 | """ 24 | 25 | 26 | class TrajectoryAborted(TrajectoryException): 27 | """ 28 | Trajectory was aborted. 29 | """ 30 | 31 | 32 | class TrajectoryStalled(TrajectoryException): 33 | """ 34 | Trajectory stalled. 35 | """ 36 | 37 | 38 | class SynchronizationException(PrPyException): 39 | """ 40 | Controller synchronization failed. 41 | """ 42 | 43 | 44 | class SerializationException(PrPyException): 45 | """ 46 | Serialization failed. 47 | """ 48 | 49 | 50 | class UnsupportedTypeSerializationException(SerializationException): 51 | """ 52 | Serialization failed due to an unknown type. 53 | """ 54 | def __init__(self, value): 55 | self.value = value 56 | self.type = type(self.value) 57 | 58 | super(UnsupportedTypeSerializationException, self).__init__( 59 | 'Serializing type "{:s}.{:s}" is not supported.'.format( 60 | self.type.__module__, self.type.__name__)) 61 | 62 | 63 | class UnsupportedTypeDeserializationException(SerializationException): 64 | """ 65 | Deserialization failed due to an unknown type. 66 | """ 67 | def __init__(self, type_name): 68 | self.type_name = type_name 69 | 70 | super(UnsupportedTypeDeserializationException, self).__init__( 71 | 'Deserializing type "{:s}" is not supported.'.format(type_name)) 72 | -------------------------------------------------------------------------------- /src/prpy/perception/perception_helper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2016, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Gilwoo Lee 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | def RemoveAllObjects(env, robot, kept_bodies=None): 32 | """ 33 | Remove everything from the environment except robot and kept_bodies. 34 | The environment must be locked while calling this method. 35 | @param env OpenRAVE environment to remove objects 36 | @param robot Robot in env to keep 37 | @param kept_bodies Bodies to keep 38 | """ 39 | if kept_bodies is None: 40 | kept_bodies = [] 41 | 42 | for o in env.GetBodies(): 43 | if o != robot and o not in kept_bodies: 44 | env.Remove(o) 45 | 46 | 47 | -------------------------------------------------------------------------------- /src/prpy/planning/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | from base import ( 32 | FirstSupported, 33 | MethodMask, 34 | Planner, 35 | PlanningError, 36 | Ranked, 37 | Sequence, 38 | UnsupportedPlanningError, 39 | ) 40 | from chomp import CHOMPPlanner 41 | from cbirrt import CBiRRTPlanner 42 | from ompl import OMPLPlanner 43 | from mk import MKPlanner 44 | from snap import SnapPlanner 45 | from named import NamedPlanner 46 | from ik import IKPlanner 47 | from sbpl import SBPLPlanner 48 | from openrave import BiRRTPlanner 49 | from tsrplanner import TSRPlanner 50 | from workspace import GreedyIKPlanner 51 | from vectorfield import VectorFieldPlanner 52 | -------------------------------------------------------------------------------- /src/prpy/perception/simulated.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2015, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Jennifer King 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import logging 32 | from base import PerceptionModule, PerceptionMethod 33 | 34 | logger = logging.getLogger(__name__) 35 | 36 | class SimulatedPerceptionModule(PerceptionModule): 37 | 38 | def __init__(self): 39 | super(SimulatedPerceptionModule, self).__init__() 40 | 41 | def __str__(self): 42 | return 'Simulated' 43 | 44 | @PerceptionMethod 45 | def DetectObjects(self, robot, **kw_args): 46 | """ 47 | Copies all objects in the ground truth environment into 48 | the robots environment 49 | """ 50 | logger.warn('DetectObjects is not implemented in simulation') 51 | -------------------------------------------------------------------------------- /tests/planning/methods/PlanToEndEffectorPose.py: -------------------------------------------------------------------------------- 1 | from prpy.clone import CloneException 2 | from prpy.planning.base import PlanningError 3 | 4 | class PlanToEndEffectorPoseTest(object): 5 | def test_PlanToEndEffectorPose_GoalIsFeasible_FindsSolution(self): 6 | from numpy.testing import assert_allclose 7 | 8 | # Setup 9 | with self.env: 10 | self.robot.SetActiveDOFValues(self.config_feasible_goal) 11 | goal_ik = self.manipulator.GetEndEffectorTransform() 12 | 13 | self.robot.SetActiveDOFValues(self.config_feasible_start2) 14 | 15 | # Test 16 | path = self.planner.PlanToEndEffectorPose(self.robot, goal_ik) 17 | 18 | # Assert 19 | self.ValidatePath(path) 20 | 21 | first_waypoint = path.GetWaypoint(0) 22 | assert_allclose(first_waypoint, self.config_feasible_start2) 23 | 24 | with self.env: 25 | last_waypoint = path.GetWaypoint(path.GetNumWaypoints() - 1) 26 | self.robot.SetActiveDOFValues(last_waypoint) 27 | last_ik = self.manipulator.GetEndEffectorTransform() 28 | 29 | self.assertTransformClose(last_ik, goal_ik) 30 | self.assertFalse(self.CollisionCheckPath(path)) 31 | 32 | def test_PlanToEndEffectorPose_StartInCollision_Throws(self): 33 | # Setup 34 | with self.env: 35 | self.robot.SetActiveDOFValues(self.config_feasible_goal) 36 | goal_ik = self.manipulator.GetEndEffectorTransform() 37 | 38 | self.robot.SetActiveDOFValues(self.config_env_collision) 39 | 40 | # Test/Assert 41 | with self.assertRaises(PlanningError): 42 | self.planner.PlanToEndEffectorPose(self.robot, goal_ik) 43 | 44 | def test_PlanToEndEffectorPose_StartInSelfCollision_Throws(self): 45 | # Setup 46 | with self.env: 47 | self.robot.SetActiveDOFValues(self.config_feasible_goal) 48 | goal_ik = self.manipulator.GetEndEffectorTransform() 49 | 50 | self.robot.SetActiveDOFValues(self.config_self_collision) 51 | 52 | # Test/Assert 53 | with self.assertRaises((PlanningError, CloneException)): 54 | self.planner.PlanToEndEffectorPose(self.robot, goal_ik) 55 | 56 | def test_PlanToEndEffectorPose_GoalInCollision_Throws(self): 57 | # Setup 58 | with self.env: 59 | self.robot.SetActiveDOFValues(self.config_env_collision) 60 | goal_ik = self.manipulator.GetEndEffectorTransform() 61 | 62 | self.robot.SetActiveDOFValues(self.config_feasible_start) 63 | 64 | # Test/Assert 65 | with self.assertRaises(PlanningError): 66 | self.planner.PlanToEndEffectorPose(self.robot, goal_ik) 67 | 68 | def test_PlanToEndEffectorPose_GoalInSelfCollision_Throws(self): 69 | # Setup 70 | with self.env: 71 | self.robot.SetActiveDOFValues(self.config_self_collision) 72 | goal_ik = self.manipulator.GetEndEffectorTransform() 73 | 74 | self.robot.SetActiveDOFValues(self.config_feasible_start) 75 | 76 | # Test/Assert 77 | with self.assertRaises(PlanningError): 78 | self.planner.PlanToEndEffectorPose(self.robot, goal_ik) 79 | -------------------------------------------------------------------------------- /src/prpy/db.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013, Carnegie Mellon University 2 | # All rights reserved. 3 | # Authors: Michael Koval 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # - Redistributions of source code must retain the above copyright notice, this 9 | # list of conditions and the following disclaimer. 10 | # - Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # - Neither the name of Carnegie Mellon University nor the names of its 14 | # contributors may be used to endorse or promote products derived from this 15 | # software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | import openravepy 30 | import bind 31 | 32 | class KinBodyDatabase(object): 33 | DATABASE_NAME = 'prpy' 34 | DATABASE_TABLE = 'ordata' 35 | 36 | def __init__(self): 37 | try: 38 | import pymongo 39 | except ImportError: 40 | raise Exception('KinBodyDatabase requires MongoDB and pymongo to be installed.') 41 | 42 | self.client = pymongo.MongoClient() 43 | self.database = self.client[DATABASE_NAME] 44 | self.table = self.database[DATABASE_TABLE] 45 | 46 | def bind(self): 47 | openravepy.Environment.ReadKinBodyDB = self.ReadKinBodyDB 48 | 49 | def ReadKinBodyDB(self, env, name): 50 | matches = self.table.find({ 'name': name }) 51 | 52 | # Check for missing and duplicate entries. 53 | num_matches = matches.count() 54 | if num_matches == 0: 55 | raise KeyError('There is no object named "{0:s}".'.format(name)) 56 | elif num_matches > 1: 57 | raise KeyError('There are {0:d} objects named "{1:s}".'.format( 58 | num_matches, name)) 59 | 60 | # Construct the KinBody. 61 | metadata = matches[0] 62 | kinbody = env.ReadKinBodyXMLData(metadata['openrave_xml']) 63 | kinbody.SetName(metadata['name']) 64 | 65 | # Attach the metadata. 66 | bind.InstanceDeduplicator.add_canonical(kinbody) 67 | kinbody.metadata = metadata 68 | 69 | return kinbody 70 | 71 | def ReadRobotDB(self, env, name): 72 | kinbody = self.ReadKinBodyDB(env, name) 73 | if not isinstance(kinbody, openravepy.Robot): 74 | kinbody.Destroy() 75 | raise IOError('Type "{0:s}" is not a robot.'.format(name)) 76 | 77 | return kinbody 78 | -------------------------------------------------------------------------------- /src/prpy/planning/adapters.py: -------------------------------------------------------------------------------- 1 | import numpy 2 | from openravepy import Environment 3 | from .base import ( 4 | LockedPlanningMethod, 5 | Planner, 6 | ) 7 | from ..kin import ( 8 | H_from_op_diff, 9 | invert_H, 10 | ) 11 | from tsr.tsr import ( 12 | TSR, 13 | TSRChain, 14 | ) 15 | from ..util import GetManipulatorIndex 16 | 17 | 18 | class PlanToEndEffectorOffsetTSRAdapter(Planner): 19 | epsilon = 1e-3 20 | 21 | 22 | def __init__(self, delegate_planner): 23 | """ Plan PlanToEndEffectorOffset using PlanToTSR on a delegate planner. 24 | 25 | @param delegate_planner planner that supports goal and constraint TSRs 26 | """ 27 | super(PlanToEndEffectorOffsetTSRAdapter, self).__init__() 28 | 29 | self.delegate_planner = delegate_planner 30 | 31 | @LockedPlanningMethod 32 | def PlanToEndEffectorOffset(self, robot, direction, distance, **kwargs): 33 | """ Plan to a desired end-effector offset with fixed orientation. 34 | 35 | This function converts a PlanToEndEffectorOffset query into: (1) a goal 36 | TSR at "distance" away from the start pose along "direction" and (2) a 37 | constraint TSR that keeps the end-effector's position constrained to 38 | that line segment with fixed orientation. Those TSRs are forwarded to 39 | PlanToTSR on the delegate planner. 40 | 41 | @param robot 42 | @param direction unit vector in the world frame 43 | @param distance distance to move, in meters 44 | @param **kwargs arguments forwarded to the delegate planner 45 | @return output trajectory 46 | """ 47 | chains = self.CreateTSRChains(robot, direction, distance) 48 | return self.delegate_planner.PlanToTSR(robot, chains, **kwargs) 49 | 50 | 51 | @classmethod 52 | def CreateTSRChains(cls, robot, direction, distance): 53 | direction = numpy.array(direction, dtype=float) 54 | 55 | if direction.shape != (3,): 56 | raise ValueError('Direction must be a three-dimensional vector.') 57 | 58 | if not (distance >= 0): 59 | raise ValueError('Distance must be non-negative; got {:f}.'.format( 60 | distance)) 61 | 62 | 63 | manip, manip_index = GetManipulatorIndex(robot) 64 | H_world_ee = manip.GetEndEffectorTransform() 65 | direction = direction / numpy.linalg.norm(direction) 66 | 67 | # 'object frame w' is at ee, z pointed along direction to move 68 | H_world_w = H_from_op_diff(H_world_ee[0:3, 3], direction) 69 | H_w_ee = numpy.dot(invert_H(H_world_w), H_world_ee) 70 | Hw_end = numpy.eye(4) 71 | Hw_end[2, 3] = distance 72 | 73 | goal_chain = TSRChain(sample_goal=True, TSR=TSR( 74 | T0_w=numpy.dot(H_world_w, Hw_end), 75 | Tw_e=H_w_ee, 76 | Bw=numpy.zeros((6, 2)), 77 | manipindex=manip_index)) 78 | constraint_chain = TSRChain(constrain=True, TSR=TSR( 79 | T0_w=H_world_w, 80 | Tw_e=H_w_ee, 81 | Bw=numpy.array([ 82 | [-cls.epsilon, cls.epsilon], 83 | [-cls.epsilon, cls.epsilon], 84 | [min(0.0, distance), max(0.0, distance)], 85 | [-cls.epsilon, cls.epsilon], 86 | [-cls.epsilon, cls.epsilon], 87 | [-cls.epsilon, cls.epsilon]]), 88 | manipindex=manip_index)) 89 | 90 | return [goal_chain, constraint_chain] 91 | -------------------------------------------------------------------------------- /src/prpy/controllers/rewd_controllers.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | 4 | class OrController(object): 5 | """Dummy/empty OpenRAVE controller class""" 6 | 7 | def GetControlDOFIndices(self): 8 | raise NotImplementedError("GetControlDOFIndices not implemented") 9 | 10 | def GetNamespace(self): 11 | return self.namespace 12 | 13 | def GetRobot(self): 14 | return self.robot 15 | 16 | def Reset(self): 17 | raise NotImplementedError("Reset not implemented") 18 | 19 | def SetDesired(self, positions): 20 | raise NotImplementedError("SetDesired not implemented") 21 | 22 | def SetPath(self, traj): 23 | raise NotImplementedError("SetPath not implemented") 24 | 25 | def SimulationStep(self, dt): 26 | raise NotImplementedError("SimulatedStep not implemented") 27 | 28 | def IsDone(self): 29 | raise NotImplementedError("IsDone not implemented") 30 | 31 | def GetTime(self): 32 | raise NotImplementedError("GetTime not implemented") 33 | 34 | def GetVelocity(self): 35 | raise NotImplementedError("GetVelocity not implemented") 36 | 37 | def GetTorque(self): 38 | raise NotImplementedError("GetTorque not implemented") 39 | 40 | 41 | class RewdOrController(OrController): 42 | """A super class for initializing all RewdOrControllers""" 43 | def __init__(self, robot, namespace, joint_names, simulated=False): 44 | self.logger = logging.getLogger(__name__) 45 | self.robot = robot 46 | self.namespace = namespace 47 | self.joint_names = joint_names 48 | self.simulated = simulated 49 | self.logger.info("Rewd Controller initialized") 50 | 51 | 52 | class RewdOrTrajectoryController(RewdOrController): 53 | """A controller for trajectory execution 54 | 55 | An interface for using trajectory controllers that 56 | is "compatible" with how we use OpenRAVE controllers 57 | """ 58 | def __init__(self, robot, namespace, controller_name, joint_names, 59 | simulated=False): 60 | super(RewdOrTrajectoryController, self).__init__(robot, 61 | namespace, 62 | joint_names, 63 | simulated) 64 | if simulated: 65 | raise NotImplementedError('Simulation not supported in ' 66 | 'RewdOrTrajectoryController') 67 | 68 | from ros_control_client_py import FollowJointTrajectoryClient 69 | 70 | self.controller_client = FollowJointTrajectoryClient(namespace, 71 | controller_name) 72 | self.current_trajectory = None 73 | self.logger.info('Rewd Trajectory Controller initialized') 74 | 75 | def SetPath(self, traj): 76 | from ros_control_client_py import TrajectoryExecutionFailed 77 | from ros_control_client_py.util import or_to_ros_trajectory 78 | if not self.IsDone(): 79 | raise TrajectoryExecutionFailed('Currently executing another ' 80 | 'trajectory', traj, None) 81 | 82 | ros_traj = or_to_ros_trajectory(self.GetRobot(), traj) 83 | self.current_trajectory = self.controller_client.execute(ros_traj) 84 | 85 | def IsDone(self): 86 | return (self.current_trajectory is None or 87 | self.current_trajectory.done()) 88 | 89 | def GetTime(self): 90 | # TODO implement with self.current_trajectory.partial_result() 91 | raise NotImplementedError('GetTime not yet implemented in ' 92 | 'RewdOrTrajectoryController') 93 | -------------------------------------------------------------------------------- /tests/planning/methods/RetimeTrajectory.py: -------------------------------------------------------------------------------- 1 | 2 | class RetimeTrajectoryTest(object): 3 | def setUp(self): 4 | from openravepy import planningutils, Planner, RaveCreateTrajectory 5 | 6 | cspec = self.robot.GetActiveConfigurationSpecification('linear') 7 | 8 | self.feasible_path = RaveCreateTrajectory(self.env, '') 9 | self.feasible_path.Init(cspec) 10 | self.feasible_path.Insert(0, self.waypoint1) 11 | self.feasible_path.Insert(1, self.waypoint2) 12 | self.feasible_path.Insert(2, self.waypoint3) 13 | 14 | self.dt = 0.01 15 | self.tolerance = 0.1 # 10% error 16 | 17 | def test_RetimeTrajectory(self): 18 | import numpy 19 | from numpy.testing import assert_allclose 20 | from openravepy import planningutils, Planner 21 | 22 | # Setup/Test 23 | traj = self.planner.RetimeTrajectory(self.robot, self.feasible_path) 24 | 25 | # Assert 26 | position_cspec = self.feasible_path.GetConfigurationSpecification() 27 | velocity_cspec = position_cspec.ConvertToDerivativeSpecification(1) 28 | zero_dof_values = numpy.zeros(position_cspec.GetDOF()) 29 | 30 | # Verify that the trajectory passes through the original waypoints. 31 | waypoints = [self.waypoint1, self.waypoint2, self.waypoint3] 32 | waypoint_indices = [None] * len(waypoints) 33 | 34 | for iwaypoint in xrange(traj.GetNumWaypoints()): 35 | joint_values = traj.GetWaypoint(iwaypoint, position_cspec) 36 | 37 | # Compare the waypoint against every input waypoint. 38 | for icandidate, candidate_waypoint in enumerate(waypoints): 39 | if numpy.allclose(joint_values, candidate_waypoint): 40 | self.assertIsNone(waypoint_indices[icandidate], 41 | 'Input waypoint {} appears twice in the output' 42 | ' trajectory (indices: {} and {})'.format( 43 | icandidate, waypoint_indices[icandidate], 44 | iwaypoint)) 45 | 46 | waypoint_indices[icandidate] = iwaypoint 47 | 48 | self.assertEquals(waypoint_indices[0], 0) 49 | self.assertEquals(waypoint_indices[-1], traj.GetNumWaypoints() - 1) 50 | 51 | for iwaypoint in waypoint_indices: 52 | self.assertIsNotNone(iwaypoint) 53 | 54 | # Verify that the velocity at the waypoint is zero. 55 | joint_velocities = traj.GetWaypoint(iwaypoint, velocity_cspec) 56 | assert_allclose(joint_velocities, zero_dof_values) 57 | 58 | # Verify the trajectory between waypoints. 59 | for t in numpy.arange(self.dt, traj.GetDuration(), self.dt): 60 | iafter = traj.GetFirstWaypointIndexAfterTime(t) 61 | ibefore = iafter - 1 62 | 63 | joint_values = traj.Sample(t, position_cspec) 64 | joint_values_before = traj.GetWaypoint(ibefore, position_cspec) 65 | joint_values_after = traj.GetWaypoint(iafter, position_cspec) 66 | 67 | distance_full = numpy.linalg.norm( 68 | joint_values_after - joint_values_before) 69 | distance_before = numpy.linalg.norm( 70 | joint_values - joint_values_before) 71 | distance_after = numpy.linalg.norm( 72 | joint_values - joint_values_after) 73 | deviation = distance_before + distance_after - distance_full 74 | self.assertLess(deviation, self.tolerance * distance_full) 75 | 76 | # Check joint limits and dynamic feasibility. 77 | params = Planner.PlannerParameters() 78 | params.SetRobotActiveJoints(self.robot) 79 | 80 | planningutils.VerifyTrajectory(params, traj, self.dt) 81 | 82 | # TODO: Test failure cases. 83 | -------------------------------------------------------------------------------- /src/prpy/base/endeffector.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import openravepy 32 | 33 | 34 | class EndEffector(openravepy.Robot.Link): 35 | def __init__(self, manipulator): 36 | self.manipulator = manipulator 37 | 38 | def CloneBindings(self, parent): 39 | from ..clone import Cloned 40 | 41 | self.manipulator = Cloned(parent.manipulator, 42 | into=self.GetParent().GetEnv()) 43 | if hasattr(parent, 'configurations'): 44 | self.configurations = parent.configurations 45 | 46 | def GetIndices(self): 47 | """Gets the DOF indicies associated with this end-effector. 48 | @return list of DOF indices 49 | """ 50 | indices = self.manipulator.GetChildDOFIndices() 51 | indices.sort() 52 | return indices 53 | 54 | def GetDOFValues(self): 55 | """Gets the current DOF values of this end-effector. 56 | These DOF values correspond to the DOF indices returned by 57 | \ref GetIndices. 58 | @return list of DOF values 59 | """ 60 | return self.manipulator.GetRobot().GetDOFValues(self.GetIndices()) 61 | 62 | def SetDOFValues(self, dof_values, 63 | limits_action=openravepy.KinBody.CheckLimitsAction.CheckLimits): 64 | """Sets the current DOF values of this end-effector. 65 | These DOF values correspond to the DOF indices returned by 66 | \ref GetIndices. Note that this method only changes the DOF values in 67 | OpenRAVE: it set a position set-point on the real robot. 68 | @param dof_values DOF values 69 | @param limits_action whether to enforce joint limits 70 | """ 71 | self.manipulator.GetRobot().SetDOFValues(dof_values, self.GetIndices(), limits_action) 72 | 73 | def SetActive(self): 74 | """Sets this end-effector as active. 75 | This both: (1) sets the current manipulator as active and (2) sets the 76 | active DOF values to thosse associated with this end-effector. 77 | """ 78 | self.manipulator.GetRobot().SetActiveManipulator(self.manipulator) 79 | self.manipulator.GetRobot().SetActiveDOFs(self.manipulator.GetArmIndices()) 80 | -------------------------------------------------------------------------------- /src/prpy/planning/named.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | import logging, numpy, openravepy 31 | from base import Planner, PlanningError, LockedPlanningMethod 32 | 33 | class NamedPlanner(Planner): 34 | def __init__(self, delegate_planner=None): 35 | """ 36 | @param delegate_planner planner used for PlanToConfiguration 37 | """ 38 | super(NamedPlanner, self).__init__() 39 | self.delegate_planner = delegate_planner 40 | 41 | def __str__(self): 42 | return 'NamedPlanner' 43 | 44 | @LockedPlanningMethod 45 | def PlanToNamedConfiguration(self, robot, name, **kw_args): 46 | """ Plan to a named configuration. 47 | 48 | The configuration is looked up by name in robot.configurations. Any 49 | DOFs not specified in the named configuration are ignored. Planning is 50 | performed by PlanToConfiguration using a delegate planner. If not 51 | specified, this defaults to robot.planner. 52 | 53 | @param name name of a saved configuration 54 | @param **kw_args optional arguments passed to PlanToConfiguration 55 | @returns traj trajectory 56 | """ 57 | try: 58 | configurations = robot.configurations 59 | except AttributeError: 60 | raise PlanningError('{:s} does not have a table of named' 61 | ' configurations.'.format(robot)) 62 | 63 | try: 64 | saved_dof_indices, saved_dof_values = robot.configurations.get_configuration(name) 65 | except KeyError: 66 | raise PlanningError('{0:s} does not have named configuration "{1:s}".'.format(robot, name)) 67 | 68 | arm_dof_indices = robot.GetActiveDOFIndices() 69 | arm_dof_values = robot.GetActiveDOFValues() 70 | 71 | # Extract the active DOF values from the configuration. 72 | for arm_dof_index, arm_dof_value in zip(saved_dof_indices, saved_dof_values): 73 | if arm_dof_index in arm_dof_indices: 74 | i = list(arm_dof_indices).index(arm_dof_index) 75 | arm_dof_values[i] = arm_dof_value 76 | 77 | # Delegate planning to another planner. 78 | planner = self.delegate_planner or robot.planner 79 | return planner.PlanToConfiguration(robot, arm_dof_values, **kw_args) 80 | 81 | -------------------------------------------------------------------------------- /tests/planning/test_RankedTests.py.disabled: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | from threading import Thread 3 | from planning_helpers import FailPlanner, MetaPlannerTests, SuccessPlanner 4 | from prpy.planning.base import Ranked 5 | 6 | 7 | class RankedTests(MetaPlannerTests, 8 | TestCase): 9 | def test_FirstPlannerSucceeds_ReturnsImmediately(self): 10 | first_planner = SuccessPlanner(self.traj, delay=True) 11 | second_planner = FailPlanner() 12 | planner = Ranked(first_planner, second_planner) 13 | 14 | # Run the metaplanner in a separate thread so we have control over when 15 | # each delegate planner terminates. 16 | def test_planner(): 17 | planner.PlanTest(self.robot) 18 | 19 | test_thread = Thread(target=test_planner) 20 | test_thread.start() 21 | first_planner.wait_for_start() 22 | second_planner.wait_for_start() 23 | 24 | # Force the first planner to return success. The meta-planner should 25 | # immediately return success and the second planner should still be 26 | # running in the background. 27 | first_planner.finish() 28 | test_thread.join(timeout=self.join_timeout) 29 | self.assertFalse(test_thread.isAlive()) 30 | 31 | # Clean up by terminating the second planner. 32 | second_planner.finish() 33 | test_thread.join() 34 | 35 | def test_FirstPlannerFails_ReturnsResultOfSecondPlanner(self): 36 | first_planner = FailPlanner(delay=True) 37 | second_planner = SuccessPlanner(self.traj, delay=True) 38 | planner = Ranked(first_planner, second_planner) 39 | 40 | # Run the metaplanner in a separate thread so we have control over when 41 | # each delegate planner terminates. 42 | def test_planner(): 43 | planner.PlanTest(self.robot) 44 | 45 | test_thread = Thread(target=test_planner) 46 | test_thread.start() 47 | first_planner.wait_for_start() 48 | second_planner.wait_for_start() 49 | 50 | # Force the first planner to return failure. The meta-planner should 51 | # wait for the second planner before declaring failure. 52 | first_planner.finish() 53 | 54 | # Force the second planner to return. This trajectory should be 55 | # immediately returned by the meta-planner. 56 | second_planner.finish() 57 | test_thread.join(timeout=self.join_timeout) 58 | self.assertFalse(test_thread.isAlive()) 59 | 60 | # Clean up. 61 | test_thread.join() 62 | 63 | def test_SecondPlannerSucceeds_WaitsForFirstPlanner(self): 64 | first_planner = SuccessPlanner(self.traj, delay=True) 65 | second_planner = SuccessPlanner(self.traj, delay=True) 66 | planner = Ranked(first_planner, second_planner) 67 | 68 | # Run the metaplanner in a separate thread so we have control over when 69 | # each delegate planner terminates. 70 | def test_planner(): 71 | planner.PlanTest(self.robot) 72 | 73 | test_thread = Thread(target=test_planner) 74 | test_thread.start() 75 | first_planner.wait_for_start() 76 | second_planner.wait_for_start() 77 | 78 | # Force the second planner to return success. The metaplanner should 79 | # continue waiting for the first planner. 80 | second_planner.finish() 81 | test_thread.join(timeout=self.join_timeout) 82 | self.assertTrue(test_thread.isAlive()) 83 | 84 | # Terminate the first planner. The result should be instantly returned. 85 | # TODO: Also check that the correct trajectory was returned. 86 | first_planner.finish() 87 | test_thread.join(timeout=self.join_timeout) 88 | self.assertFalse(test_thread.isAlive()) 89 | 90 | def test_AllPlannersFail_ThrowsPlanningError(self): 91 | planner = Ranked(FailPlanner(), FailPlanner()) 92 | with self.assertRaises(prpy.planning.PlanningError): 93 | planner.PlanTest(self.robot) 94 | -------------------------------------------------------------------------------- /src/prpy/logger.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import collections, logging, sys, warnings 32 | 33 | class ColoredFormatter(logging.Formatter): 34 | def __init__(self, default): 35 | self._default_formatter = default 36 | self._color_table = collections.defaultdict(lambda: list()) 37 | self._color_table[logging.CRITICAL] = [ 'red' ] 38 | self._color_table[logging.ERROR] = [ 'red' ] 39 | self._color_table[logging.WARNING] = [ 'yellow' ] 40 | self._color_table[logging.DEBUG] = [ 'green' ] 41 | 42 | # Import termcolor now to fail-fast. 43 | import termcolor 44 | self.termcolor = termcolor 45 | 46 | def format(self, record): 47 | color_options = self._color_table[record.levelno] 48 | message = self._default_formatter.format(record) 49 | return self.termcolor.colored(message, *color_options) 50 | 51 | def initialize_logging(): 52 | formatter = logging.Formatter('[%(levelname)s] [%(name)s:%(filename)s:%(lineno)d]:%(funcName)s: %(message)s') 53 | 54 | # Remove all of the existing handlers. 55 | base_logger = logging.getLogger() 56 | for handler in list(base_logger.handlers): 57 | base_logger.removeHandler(handler) 58 | 59 | # Add the custom handler. 60 | handler = logging.StreamHandler(sys.stdout) 61 | handler.setLevel(logging.DEBUG) 62 | handler.setFormatter(formatter) 63 | base_logger.addHandler(handler) 64 | base_logger.setLevel(logging.INFO) 65 | 66 | # Colorize logging output if the termcolor package is available. 67 | try: 68 | color_formatter = ColoredFormatter(formatter) 69 | handler.setFormatter(color_formatter) 70 | except ImportError: 71 | logging.warning('Install termcolor to colorize log messages.') 72 | 73 | # Disable spammy and ROS loggers. 74 | spammy_logger_names = [ 75 | 'rospy.topics', 76 | 'openravepy.inversekinematics', 77 | 'openravepy.databases.inversekinematics', 78 | ] 79 | for spammy_logger_name in spammy_logger_names: 80 | spammy_logger = logging.getLogger(spammy_logger_name) 81 | spammy_logger.setLevel(logging.WARNING) 82 | 83 | # Enable deprecation warnings, which are off by default in Python 2.7. 84 | warnings.simplefilter('default') 85 | 86 | return base_logger 87 | 88 | def remove_ros_logger(): 89 | logger = logging.getLogger() 90 | new_handlers = [] 91 | 92 | for handler in logger.handlers: 93 | if type(handler).__name__ != 'RosStreamHandler': 94 | new_handlers.append(handler) 95 | 96 | logger.handlers = new_handlers 97 | 98 | -------------------------------------------------------------------------------- /src/prpy/ik_ranking.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import numpy 32 | 33 | 34 | def NoRanking(robot, ik_solutions): 35 | """ 36 | Return IK solutions with an arbitrary ranking. 37 | """ 38 | return numpy.ones(ik_solutions.shape[0]) 39 | 40 | 41 | def JointLimitAvoidance(robot, ik_solutions): 42 | """ 43 | Score IK solutions by their distance from joint limits. This is implemented 44 | with a quadratic loss function that measures distance from limits. 45 | """ 46 | with robot.GetEnv(): 47 | lower_limits, upper_limits = robot.GetActiveDOFLimits() 48 | 49 | lower_distance = ik_solutions - lower_limits 50 | upper_distance = upper_limits - ik_solutions 51 | distance = numpy.minimum(lower_distance, upper_distance) 52 | return -numpy.sum(distance**2, axis=1) 53 | 54 | 55 | class NominalConfiguration(object): 56 | def __init__(self, q_nominal, max_deviation=2*numpy.pi): 57 | """ 58 | Score IK solutions by their distance from a nominal configuration. 59 | @param q_nominal nominal configuration 60 | @param max_deviation specify a maximum allowable per-joint deviation 61 | from the nominal configuration, default is 2*PI 62 | """ 63 | self.q_nominal = q_nominal 64 | self.max_deviation = max_deviation 65 | 66 | def __call__(self, robot, ik_solutions): 67 | assert ik_solutions.shape[1] == self.q_nominal.shape[0] 68 | L_2 = numpy.linalg.norm(ik_solutions - self.q_nominal, 69 | axis=1, ord=2) 70 | 71 | # Ignore IK solutions that are more than the specified distance away. 72 | # (This means a closer solution must exist!) 73 | if self.max_deviation is not None: 74 | L_inf = numpy.linalg.norm(ik_solutions - self.q_nominal, 75 | axis=1, ord=numpy.inf) 76 | L_2[L_inf > self.max_deviation] = numpy.inf 77 | 78 | return L_2 79 | 80 | 81 | 82 | class MultipleNominalConfigurations(object): 83 | def __init__(self, q_nominal_list, max_deviation=2*numpy.pi): 84 | """ 85 | Score IK solutions by their summed distance to multiple configurations 86 | @param q_nominal_list list of configurations to compare distance to 87 | @param max_deviation specify a maximum allowable per-joint deviation 88 | from the nominal configuration, default is 2*PI 89 | """ 90 | self.all_scorers = [NominalConfiguration(q_nominal, max_deviation) for q_nominal in q_nominal_list] 91 | self.max_deviation = max_deviation 92 | 93 | def __call__(self, robot, ik_solutions): 94 | return sum([scorer(robot, ik_solutions) for scorer in self.all_scorers]) 95 | 96 | -------------------------------------------------------------------------------- /tests/planning/methods/PlanToConfiguration.py: -------------------------------------------------------------------------------- 1 | from prpy.clone import CloneException 2 | from prpy.planning.base import PlanningError 3 | from prpy.planning.exceptions import ( 4 | CollisionPlanningError, 5 | SelfCollisionPlanningError, 6 | JointLimitError) 7 | 8 | class PlanToConfigurationTest(object): 9 | def test_PlanToConfiguration_StartInCollision_Throws(self): 10 | # Setup 11 | with self.env: 12 | self.robot.SetActiveDOFValues(self.config_env_collision) 13 | 14 | # Test/Assert 15 | with self.assertRaises(PlanningError): 16 | self.planner.PlanToConfiguration( 17 | self.robot, self.config_feasible_goal) 18 | 19 | def test_PlanToConfiguration_StartInSelfCollision_Throws(self): 20 | # Setup 21 | with self.env: 22 | self.robot.SetActiveDOFValues(self.config_self_collision) 23 | 24 | # Test/Assert 25 | with self.assertRaises((PlanningError, CloneException)): 26 | self.planner.PlanToConfiguration( 27 | self.robot, self.config_feasible_goal) 28 | 29 | def test_PlanToConfiguration_GoalInCollision_Throws(self): 30 | # Setup 31 | with self.env: 32 | self.robot.SetActiveDOFValues(self.config_feasible_start) 33 | 34 | # Test/Assert 35 | with self.assertRaises(PlanningError): 36 | self.planner.PlanToConfiguration( 37 | self.robot, self.config_env_collision) 38 | 39 | def test_PlanToConfiguration_GoalInSelfCollision_Throws(self): 40 | # Setup 41 | with self.env: 42 | self.robot.SetActiveDOFValues(self.config_feasible_start) 43 | 44 | # Test/Assert 45 | with self.assertRaises(PlanningError): 46 | self.planner.PlanToConfiguration( 47 | self.robot, self.config_self_collision) 48 | 49 | def PlanFromStartToGoalConfiguration(self, config_start, config_goal): 50 | from numpy.testing import assert_allclose 51 | 52 | # Setup 53 | with self.env: 54 | self.robot.SetActiveDOFValues(config_start) 55 | 56 | # Test 57 | path = self.planner.PlanToConfiguration(self.robot, config_goal) 58 | 59 | # Assert. 60 | self.assertEquals(path.GetEnv(), self.env) 61 | self.assertEquals( 62 | self.robot.GetActiveConfigurationSpecification('linear'), 63 | path.GetConfigurationSpecification()) 64 | self.assertGreaterEqual(path.GetNumWaypoints(), 1) 65 | first_waypoint = path.GetWaypoint(0) 66 | last_waypoint = path.GetWaypoint(path.GetNumWaypoints() - 1) 67 | 68 | self.ValidatePath(path) 69 | assert_allclose(first_waypoint, config_start) 70 | assert_allclose(last_waypoint, config_goal) 71 | self.assertFalse(self.CollisionCheckPath(path)) 72 | 73 | 74 | class PlanToConfigurationStraightLineTest(object): 75 | def test_PlanToConfiguration_AlreadyAtGoal_FindsSolution(self): 76 | self.PlanFromStartToGoalConfiguration(self.waypoint1, self.waypoint1) 77 | self.PlanFromStartToGoalConfiguration(self.waypoint2, self.waypoint2) 78 | self.PlanFromStartToGoalConfiguration(self.waypoint3, self.waypoint3) 79 | 80 | def test_PlanToConfiguration_StraightLineIsFeasible_FindsSolution(self): 81 | self.PlanFromStartToGoalConfiguration(self.waypoint1, self.waypoint2) 82 | self.PlanFromStartToGoalConfiguration(self.waypoint2, self.waypoint3) 83 | self.PlanFromStartToGoalConfiguration(self.waypoint3, self.waypoint1) 84 | 85 | 86 | class PlanToConfigurationCompleteTest(object): 87 | def test_PlanToConfiguration_GoalIsFeasible_FindsSolution(self): 88 | self.PlanFromStartToGoalConfiguration( 89 | self.config_feasible_start2, self.config_feasible_goal) 90 | 91 | 92 | class PlanToConfigurationTestCollisionTest(object): 93 | """ Expects collision-specific error""" 94 | 95 | def test_PlanToConfiguration_GoalInCollision_Throws_Collision(self): 96 | # Setup 97 | with self.env: 98 | self.robot.SetActiveDOFValues(self.config_feasible_start) 99 | 100 | # Test/Assert 101 | with self.assertRaises((CollisionPlanningError, 102 | SelfCollisionPlanningError, 103 | JointLimitError)): 104 | self.planner.PlanToConfiguration( 105 | self.robot, self.config_env_collision) 106 | -------------------------------------------------------------------------------- /src/prpy/planning/mac_smoother.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | from ..util import (CopyTrajectory, SimplifyTrajectory, SetTrajectoryTags, 32 | IsTimedTrajectory) 33 | from base import (BasePlanner, PlanningError, ClonedPlanningMethod, 34 | UnsupportedPlanningError, Tags) 35 | from openravepy import (Planner, PlannerStatus, RaveCreatePlanner, 36 | openrave_exception) 37 | 38 | 39 | class MacSmoother(BasePlanner): 40 | def __init__(self): 41 | super(MacSmoother, self).__init__() 42 | 43 | self.blender = RaveCreatePlanner(self.env, 'PrSplineMacBlender') 44 | if self.blender is None: 45 | raise UnsupportedPlanningError( 46 | 'Unable to create PrSplineMacBlender planner. Is or_pr_spline' 47 | ' installed and in your OPENRAVE_PLUGIN path?' 48 | ) 49 | 50 | self.retimer = RaveCreatePlanner(self.env, 'PrSplineMacTimer') 51 | if self.retimer is None: 52 | raise UnsupportedPlanningError( 53 | 'Unable to create PrSplineMacRetimer planner. Is or_pr_spline' 54 | ' installed and in your OPENRAVE_PLUGIN path?' 55 | ) 56 | 57 | @ClonedPlanningMethod 58 | def RetimeTrajectory(self, robot, path): 59 | # Copy the input trajectory into the planning environment. This is 60 | # necessary for two reasons: (1) the input trajectory may be in another 61 | # environment and/or (2) the retimer modifies the trajectory in-place. 62 | output_traj = CopyTrajectory(path, env=self.env) 63 | 64 | # Remove co-linear waypoints. 65 | if not IsTimedTrajectory(output_traj): 66 | output_traj = SimplifyTrajectory(output_traj, robot) 67 | 68 | # Blend the piecewise-linear input trajectory. The blender outputs a 69 | # collision-free path, consisting of piecewise-linear segments and 70 | # quintic blends through waypoints. 71 | try: 72 | params = Planner.PlannerParameters() 73 | self.blender.InitPlan(robot, params) 74 | status = self.blender.PlanPath(output_traj) 75 | if status not in [PlannerStatus.HasSolution, 76 | PlannerStatus.InterruptedWithSolution]: 77 | raise PlanningError('Blending trajectory failed.') 78 | except openrave_exception as e: 79 | raise PlanningError('Blending trajectory failed: ' + str(e)) 80 | 81 | # Find the time-optimal trajectory that follows the blended path 82 | # subject to joint velocity and acceleration limits. 83 | try: 84 | params = Planner.PlannerParameters() 85 | self.retimer.InitPlan(robot, params) 86 | status = self.retimer.PlanPath(output_traj) 87 | if status not in [PlannerStatus.HasSolution, 88 | PlannerStatus.InterruptedWithSolution]: 89 | raise PlanningError('Timing trajectory failed.') 90 | except openrave_exception as e: 91 | raise PlanningError('Timing trajectory failed: ' + str(e)) 92 | 93 | SetTrajectoryTags(output_traj, {Tags.SMOOTH: True}, append=True) 94 | return output_traj 95 | -------------------------------------------------------------------------------- /src/prpy/named_config.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import numpy 32 | 33 | class ConfigurationLibrary(object): 34 | """ 35 | Library of named configurations. Configurations are specified in terms of 36 | *groups* of DOF indices. Groups must be added with *add_group* before a 37 | configuration may be added. 38 | """ 39 | def __init__(self): 40 | self._indices = set() 41 | self._groups = dict() 42 | self._configs = dict() 43 | 44 | """ 45 | Load named configurations from disk. The root of the YAML file should be of 46 | the following format: 47 | 48 | configurations: 49 | configuration1: 50 | group1: [ 0, 0, ..dof_values... ] 51 | group2: [ ... ] 52 | ... 53 | configuration2: 54 | ... 55 | ... 56 | 57 | :param path: path to an input YAML file 58 | """ 59 | def load_yaml(self, path): 60 | with open(path, 'rb') as config_file: 61 | import yaml 62 | config_yaml = yaml.load(config_file) 63 | 64 | for name, groups in config_yaml['configurations'].items(): 65 | self.add_configuration(name, **groups) 66 | 67 | """ 68 | :param name: group name 69 | :param dof_indices: DOF indices 70 | """ 71 | def add_group(self, name, dof_indices): 72 | duplicate_indices = self._indices.intersection(dof_indices) 73 | 74 | if name in self._groups: 75 | raise Exception('There is already a group named %s.' % name) 76 | elif duplicate_indices: 77 | sorted_duplicates = sorted(list(duplicate_indices)) 78 | raise Exception('Indices [ %s ] are already bound to names.'\ 79 | % ', '.join(map(str, sorted_duplicates))) 80 | 81 | self._groups[name] = numpy.array(dof_indices).copy() 82 | self._indices |= set(dof_indices) 83 | 84 | """ 85 | :param name: configuration name 86 | :param **kw_args: an array of joint values for each group 87 | """ 88 | def add_configuration(self, name, **kw_args): 89 | all_indices = list() 90 | all_values = list() 91 | 92 | for group_name, dof_values in kw_args.items(): 93 | if group_name not in self._groups: 94 | raise Exception('There is no group named %s.' % group_name) 95 | 96 | group_indices = self._groups[group_name] 97 | if len(group_indices) != len(dof_values): 98 | raise Exception('Expected %d values; got %d.'\ 99 | % (len(group_indices), len(dof_values))) 100 | 101 | all_indices.extend(group_indices) 102 | all_values.extend(dof_values) 103 | 104 | self._configs[name] = (all_indices, all_values) 105 | return all_indices, all_values 106 | 107 | """ 108 | :return (dof_indices, dof_values): DOF indices and corresponding DOF values 109 | """ 110 | def get_configuration(self, name): 111 | if name in self._configs: 112 | return self._configs[name] 113 | else: 114 | raise KeyError('There is no configuration named %s.' % name) 115 | 116 | """ 117 | : return known_configurations: A list of named configurations 118 | """ 119 | def get_configuration_list(self): 120 | return self._configs.keys() 121 | -------------------------------------------------------------------------------- /scripts/replay-log.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # This script allows the planning requests produced by the Logged 4 | # metaplanner (using PrPy's serialization facilities) to be replayed. 5 | # Downstream users may need to add some additional features 6 | # (e.g. initializing robot-specific helpers such as HerbPy) 7 | # in their log replay facilities so that planners can make use of them. 8 | 9 | # Copyright (c) 2016, Carnegie Mellon University 10 | # All rights reserved. 11 | # Authors: Chris Dellin 12 | # 13 | # Redistribution and use in source and binary forms, with or without 14 | # modification, are permitted provided that the following conditions are met: 15 | # 16 | # - Redistributions of source code must retain the above copyright notice, this 17 | # list of conditions and the following disclaimer. 18 | # - Redistributions in binary form must reproduce the above copyright notice, 19 | # this list of conditions and the following disclaimer in the documentation 20 | # and/or other materials provided with the distribution. 21 | # - Neither the name of Carnegie Mellon University nor the names of its 22 | # contributors may be used to endorse or promote products derived from this 23 | # software without specific prior written permission. 24 | # 25 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 28 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 29 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | # POSSIBILITY OF SUCH DAMAGE. 36 | 37 | from __future__ import absolute_import 38 | from __future__ import division 39 | from __future__ import print_function 40 | from __future__ import unicode_literals 41 | 42 | import argparse 43 | import atexit 44 | import math 45 | 46 | import numpy 47 | import openravepy 48 | import yaml 49 | 50 | import prpy.planning 51 | import prpy.serialization 52 | 53 | parser = argparse.ArgumentParser(description='replay planning request log file') 54 | parser.add_argument('--logfile', required=True) 55 | parser.add_argument('--planner', default='cbirrt', help='cbirrt OMPL_RRTConnect birrt') 56 | parser.add_argument('--viewer', '-v', type=str, default='interactivemarker', help='The viewer to attach (none for no viewer)') 57 | args = parser.parse_args() 58 | 59 | # start openrave 60 | openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info) 61 | atexit.register(openravepy.RaveDestroy) 62 | env = openravepy.Environment() 63 | atexit.register(env.Destroy) 64 | 65 | planner = None 66 | if args.planner == 'cbirrt': 67 | planner = prpy.planning.CBiRRTPlanner() 68 | if args.planner == 'OMPL_RRTConnect': 69 | planner = prpy.planning.ompl.OMPLPlanner('RRTConnect') 70 | if args.planner == 'birrt': 71 | planner = prpy.planning.openrave.OpenRAVEPlanner('birrt') 72 | 73 | # read log file 74 | yamldict = yaml.safe_load(open(args.logfile)) 75 | 76 | # deserialize environment 77 | prpy.serialization.deserialize_environment(yamldict['environment'], env=env) 78 | 79 | # load planning request 80 | try: 81 | method = getattr(planner, yamldict['request']['method']) 82 | except AttributeError: 83 | raise RuntimeError('That planner does not support planning method {}!'.format(yamldict['request']['method'])) 84 | method_args = [] 85 | for method_arg in yamldict['request']['args']: 86 | method_args.append(prpy.serialization.deserialize(env, method_arg)) 87 | method_kwargs = {} 88 | for key,value in yamldict['request']['kw_args'].items(): 89 | method_kwargs[key] = prpy.serialization.deserialize(env, value) 90 | 91 | # attempt to retrieve robot 92 | if 'robot' in method_kwargs: 93 | robot = method_kwargs['robot'] 94 | elif len(method_args) > 0: 95 | robot = method_args[0] 96 | else: 97 | raise RuntimeError('could not retrieve robot!') 98 | 99 | # load ik solver for robot in case it's needed 100 | ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(robot, 101 | iktype=openravepy.IkParameterizationType.Transform6D) 102 | if not ikmodel.load(): 103 | ikmodel.autogenerate() 104 | 105 | # call planning method itself ... 106 | print('calling planning method ...') 107 | traj = method(*method_args, **method_kwargs) 108 | 109 | # retime/view resulting trajectory 110 | if args.viewer != 'none': 111 | env.SetViewer(args.viewer) 112 | openravepy.planningutils.RetimeActiveDOFTrajectory(traj, robot) 113 | try: 114 | while traj is not None: 115 | raw_input('Press [Enter] to run the trajectory, [Ctrl]+[C] to quit ...') 116 | with env: 117 | robot.GetController().SetPath(traj) 118 | except KeyboardInterrupt: 119 | print() 120 | -------------------------------------------------------------------------------- /src/prpy/planning/exceptions.py: -------------------------------------------------------------------------------- 1 | 2 | class PlanningError(Exception): 3 | KNOWN_KWARGS = set(['deterministic']) 4 | 5 | def __init__(self, *args, **kwargs): 6 | super(PlanningError, self).__init__(*args) 7 | 8 | assert self.KNOWN_KWARGS.issuperset(kwargs.keys()) 9 | self.deterministic = kwargs.get('deterministic', None) 10 | 11 | 12 | class UnsupportedPlanningError(PlanningError): 13 | def __init__(self, *args): 14 | super(UnsupportedPlanningError, self).__init__( 15 | *args, deterministic=True) 16 | 17 | 18 | class ConstraintViolationPlanningError(PlanningError): 19 | def __init__(self, 20 | constraint_name, 21 | threshold=None, 22 | violation_by=None, 23 | base_message='Violates constraint', 24 | deterministic=None): 25 | self.constraint_name = constraint_name 26 | self.threshold = threshold 27 | self.violation_by = violation_by 28 | 29 | super(ConstraintViolationPlanningError, self).__init__( 30 | '{:s}: {:s}'.format( 31 | base_message, 32 | self.constraint_name 33 | ), 34 | deterministic=deterministic 35 | ) 36 | 37 | 38 | class CollisionPlanningError(PlanningError): 39 | def __init__(self, link1, link2, base_message='Detected collision', 40 | deterministic=None): 41 | self.link1 = link1 42 | self.link2 = link2 43 | 44 | super(CollisionPlanningError, self).__init__( 45 | '{:s}: {:s} x {:s}.'.format( 46 | base_message, 47 | self._get_link_str(link1), 48 | self._get_link_str(link2) 49 | ), 50 | deterministic=deterministic 51 | ) 52 | 53 | @classmethod 54 | def FromReport(cls, report, deterministic=None): 55 | return cls(report.plink1, report.plink2, deterministic=deterministic) 56 | 57 | @staticmethod 58 | def _get_link_str(link): 59 | if link is not None: 60 | return '<{:s}, {:s}>'.format( 61 | link.GetParent().GetName(), link.GetName()) 62 | else: 63 | return '' 64 | 65 | 66 | class JointLimitError(PlanningError): 67 | def __init__(self, robot, dof_index, dof_value, dof_limit, description, 68 | deterministic=None): 69 | self.robot = robot 70 | self.dof_index = dof_index 71 | self.dof_value = dof_value 72 | self.dof_limit = dof_limit 73 | 74 | joint = robot.GetJointFromDOFIndex(dof_index) 75 | if dof_value < dof_limit: 76 | direction = 'lower' 77 | comparison = '<' 78 | else: 79 | direction = 'upper' 80 | comparison = '>' 81 | 82 | super(JointLimitError, self).__init__( 83 | 'Robot "{robot_name:s}" joint "{joint_name:s} axis {joint_axis:d}' 84 | ' violates {direction:s} {description:s} limit:' 85 | ' {dof_value:.5f} {comparison:s} {dof_limit:.5f}'.format( 86 | robot_name=robot.GetName(), 87 | joint_name=joint.GetName(), 88 | joint_axis=dof_index - joint.GetDOFIndex(), 89 | dof_value=dof_value, 90 | dof_limit=dof_limit, 91 | comparison=comparison, 92 | direction=direction, 93 | description=description), 94 | deterministic=deterministic) 95 | 96 | 97 | class SelfCollisionPlanningError(CollisionPlanningError): 98 | pass 99 | 100 | 101 | class TimeoutPlanningError(PlanningError): 102 | def __init__(self, timelimit=None, deterministic=None): 103 | if timelimit is not None: 104 | message = 'Exceeded {:.3f} s time limit.'.format(timelimit) 105 | else: 106 | message = 'Exceeded time limit.' 107 | 108 | super(TimeoutPlanningError, self).__init__( 109 | message, deterministic=deterministic) 110 | 111 | 112 | class MetaPlanningError(PlanningError): 113 | """ 114 | A metaplanning error indicates that a planning operation that calls one or 115 | more other planners internally failed due to the internal planning calls 116 | failing. 117 | """ 118 | def __init__(self, message, errors, deterministic=None): 119 | PlanningError.__init__(self, message, deterministic=deterministic) 120 | self.errors = errors 121 | 122 | # TODO: Print the inner exceptions. 123 | 124 | 125 | class ClonedPlanningError(PlanningError): 126 | """ 127 | A cloned planning error indicates that planning failed because a 128 | ClonedPlanningMethod was unable to clone the environment successfully. 129 | 130 | This most commonly occurs when a planner attempts to clone while in 131 | collision, which can corrupt the environment before the planner would 132 | have a chance to detect the collision. 133 | """ 134 | def __init__(self, cloning_error): 135 | super(ClonedPlanningError, self).__init__( 136 | "Failed to clone: {:s}".format(cloning_error), 137 | deterministic=True) 138 | self.error = cloning_error 139 | -------------------------------------------------------------------------------- /src/prpy/planning/sbpl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Evan Shapiro 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | from base import BasePlanner, PlanningError, ClonedPlanningMethod, UnsupportedPlanningError 32 | import openravepy 33 | 34 | class SBPLPlanner(BasePlanner): 35 | def __init__(self): 36 | super(SBPLPlanner, self).__init__() 37 | 38 | try: 39 | self.planner = openravepy.RaveCreatePlanner(self.env, 'SBPL') 40 | except openravepy.openrave_exception: 41 | raise UnsupportedPlanningError('Unable to create SBPL module') 42 | 43 | def setupEnv(self, env): 44 | self.env = env 45 | try: 46 | self.problem = openravepy.RaveCreateProblem(self.env, 'SBPL') 47 | except openravepy.openrave_exception: 48 | raise UnsupportedPlanningError('Unable to create SBPL module.') 49 | 50 | def __str__(self): 51 | return 'SBPL' 52 | 53 | def SetPlannerParameters(self, params_yaml): 54 | self.planner_params = params_yaml 55 | 56 | @ClonedPlanningMethod 57 | def PlanToBasePose(self, robot, goal_pose, timelimit=60.0, return_first=False, **kw_args): 58 | """ 59 | Plan to a base pose using SBPL 60 | @param robot 61 | @param goal_pose desired base pose 62 | @param timelimit timeout in seconds 63 | @param return_first return the first path found (if true, the planner will run until a path is found, ignoring the time limit) 64 | """ 65 | params = openravepy.Planner.PlannerParameters() 66 | 67 | from openravepy import DOFAffine 68 | robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis) 69 | params.SetRobotActiveJoints(robot) 70 | 71 | config_spec = openravepy.RaveGetAffineConfigurationSpecification(DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, robot) 72 | #params.SetConfigurationSpecification(self.env, config_spec) # This breaks 73 | 74 | goal_config = openravepy.RaveGetAffineDOFValuesFromTransform(goal_pose, 75 | DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis) 76 | 77 | params.SetGoalConfig(goal_config) 78 | 79 | # Setup default extra parameters 80 | extra_params = self.planner_params 81 | 82 | limits = robot.GetAffineTranslationLimits(); 83 | extents = [limits[0][0], limits[1][0], limits[0][1], limits[1][1]]; 84 | extra_params["extents"] = extents 85 | 86 | extra_params["timelimit"] = timelimit 87 | if return_first: 88 | extra_params["return_first"] = 1 89 | else: 90 | extra_params["return_first"] = 0 91 | 92 | extra_params["initial_eps"] = 1.0 93 | 94 | for key, value in kw_args.iteritems(): 95 | extra_params[key] = value 96 | 97 | params.SetExtraParameters(str(extra_params)) 98 | traj = openravepy.RaveCreateTrajectory(self.env, '') 99 | 100 | try: 101 | self.planner.InitPlan(robot, params) 102 | status = self.planner.PlanPath(traj, releasegil=True) 103 | 104 | except Exception as e: 105 | raise PlanningError('Planning failed with error: {0:s}'.format(e)) 106 | 107 | from openravepy import PlannerStatus 108 | if status not in [ PlannerStatus.HasSolution, PlannerStatus.InterruptedWithSolution ]: 109 | raise PlanningError('Planner returned with status {0:s}.'.format(str(status))) 110 | 111 | return traj 112 | 113 | -------------------------------------------------------------------------------- /src/prpy/dependency_manager.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import rospkg, os, sys, distutils.version 32 | 33 | def is_catkin(): 34 | distro = os.environ.get('ROS_DISTRO', '') 35 | if distro: 36 | # Clever hack courtesy of Chris Dellin. 37 | return ord(distro[0].lower()) >= ord('g') 38 | else: 39 | raise ValueError('Environment variable ROS_DISTRO is not set.') 40 | 41 | if is_catkin(): 42 | def export(): 43 | pass # do nothing 44 | else: 45 | def append_to_env(variable, new_path, separator=':'): 46 | paths = os.environ.get(variable, '') 47 | if paths: 48 | paths_list = paths.split(separator) 49 | else: 50 | paths_list = list() 51 | 52 | # Check if we're adding a duplicate entry. 53 | duplicate = False 54 | new_canonical_path = os.path.realpath(new_path) 55 | for path in paths_list: 56 | canonical_path = os.path.realpath(path) 57 | if canonical_path == new_canonical_path: 58 | duplicate = True 59 | break 60 | 61 | # Add the new path to the environmental variable. 62 | if not duplicate: 63 | paths_list.insert(0, new_path) 64 | paths = separator.join(paths_list) 65 | os.environ[variable] = paths 66 | 67 | return paths 68 | 69 | def export_paths(pkg, package_name): 70 | 71 | packages = list() 72 | 73 | # first get all the required packages 74 | try: 75 | manifest = pkg.get_manifest(package_name) 76 | required_packages = pkg.get_depends(package_name) 77 | packages.extend(required_packages) 78 | except rospkg.ResourceNotFound: 79 | return False, [package_name] 80 | 81 | # next get all the optional packages 82 | optional_packages = manifest.get_export('openrave', 'optional') 83 | packages.extend(optional_packages) 84 | 85 | # Process the OpenRAVE export tags. 86 | for plugin_path in manifest.get_export('openrave', 'plugins'): 87 | plugin_paths = append_to_env('OPENRAVE_PLUGINS', plugin_path) 88 | 89 | for data_path in manifest.get_export('openrave', 'data'): 90 | data_paths = append_to_env('OPENRAVE_DATA', data_path) 91 | 92 | for database_path in manifest.get_export('openrave', 'database'): 93 | database_paths = append_to_env('OPENRAVE_DATABASE', database_path) 94 | 95 | for python_path in manifest.get_export('python', 'path'): 96 | sys.path.append(python_path) 97 | 98 | # Add the appropriate directories to PYTHONPATH. 99 | # TODO: This is a hack. Should I parsing a Python export instead? 100 | sys.path.append(pkg.get_path(package_name) + '/src') 101 | 102 | # now dive into the subpackages 103 | packages = set(packages) 104 | missing_packages = list() 105 | for package in packages: 106 | success, missing = export_paths(pkg, package) 107 | missing_packages.extend(missing) 108 | if not success: 109 | # check if this package is required 110 | if package in required_packages: 111 | return False, missing_packages 112 | 113 | return True, missing_packages 114 | 115 | def export(package_name): 116 | pkg = rospkg.RosPack() 117 | manifest = pkg.get_manifest(package_name) 118 | 119 | # Required dependencies. 120 | success, missing_packages = export_paths(pkg, package_name) 121 | if not success: 122 | raise Exception('Unable to load required dependencies.') 123 | 124 | if missing_packages: 125 | missing_packages.sort() 126 | print 'Missing optional dependencies: %s' % ' '.join(missing_packages) 127 | return missing_packages 128 | -------------------------------------------------------------------------------- /tests/test_grasp_cloning.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import openravepy 3 | import unittest 4 | 5 | import os # environ, path 6 | import subprocess 7 | import sys # stderr 8 | 9 | import numpy 10 | 11 | from prpy.clone import Clone 12 | 13 | # Add the models included with OpenRAVE to the OPENRAVE_DATA path. 14 | # These may not be available if the user manually set the OPENRAVE_DATA 15 | # environmental variable, e.g. through openrave_catkin. 16 | try: 17 | share_path = \ 18 | subprocess.check_output(['openrave-config', '--share-dir']).strip() 19 | os.environ['OPENRAVE_DATA'] = os.path.join(share_path, 'data') 20 | except subprocess.CalledProcessError as e: 21 | print('error: Failed using "openrave-config" to find the default' 22 | ' OPENRAVE_DATA path. Loading assets may fail.', 23 | file=sys.stderr) 24 | 25 | # Initialize OpenRAVE. 26 | openravepy.RaveInitialize(True) 27 | openravepy.misc.InitOpenRAVELogging() 28 | openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Fatal) 29 | 30 | 31 | class Tests(unittest.TestCase): 32 | """ 33 | Various unit tests for grasping and cloning 34 | """ 35 | 36 | def setUp(self): 37 | self.env = openravepy.Environment() 38 | self.env.Load('wamtest1.env.xml') 39 | self.robot = self.env.GetRobot('BarrettWAM') 40 | self.manipulator = self.robot.GetManipulator('arm') 41 | 42 | self.robot.Enable(True) 43 | 44 | # Set all 7 DOF of the WAM arm to active 45 | with self.env: 46 | self.robot.SetActiveDOFs(self.manipulator.GetArmIndices()) 47 | self.robot.SetActiveManipulator(self.manipulator) 48 | self.active_dof_indices = self.robot.GetActiveDOFIndices() 49 | 50 | # Get the resolution (in radians) for the 7 joints 51 | # [0.0043, 0.0087, 0.0087, 0.0174, 0.0193, 0.0282, 0.0282] 52 | self.dof_resolutions = \ 53 | self.robot.GetDOFResolutions()[self.active_dof_indices] 54 | 55 | 56 | #add the box object we will grab 57 | self.to_grab_body = openravepy.RaveCreateKinBody(self.env, '') 58 | self.to_grab_body.SetName('box') 59 | self.to_grab_body.InitFromBoxes(numpy.array([[0., 0., 0., 0.1, 0.1, 0.1]]), False) 60 | self.env.Add(self.to_grab_body) 61 | 62 | self.to_grab_body.Enable(True) 63 | T = numpy.eye(4) 64 | T[2,3] = 20. #move far to not be in collision 65 | self.to_grab_body.SetTransform(T) 66 | 67 | 68 | def tearDown(self): 69 | self.env.Destroy() 70 | 71 | # clone the environment, and test to make sure everything is the same 72 | def check_cloned_attributes_collisions(self): 73 | with Clone(self.robot.GetEnv()) as cloned_env: 74 | cloned_robot = cloned_env.Cloned(self.robot) 75 | 76 | #self collision is identical 77 | self.assertEqual(self.robot.CheckSelfCollision(), cloned_robot.CheckSelfCollision()) 78 | 79 | #same number of grasped items 80 | self.assertEqual(len(self.robot.GetGrabbedInfo()), len(cloned_robot.GetGrabbedInfo()) ) 81 | 82 | #grabbed info is set properly 83 | for grab_info in self.robot.GetGrabbedInfo(): 84 | 85 | found_equal_info = False 86 | for cloned_grab_info in cloned_robot.GetGrabbedInfo(): 87 | this_info_equal = (grab_info._grabbedname == cloned_grab_info._grabbedname) 88 | this_info_equal = this_info_equal & (grab_info._robotlinkname == cloned_grab_info._robotlinkname) 89 | this_info_equal = this_info_equal & (grab_info._setRobotLinksToIgnore == cloned_grab_info._setRobotLinksToIgnore) 90 | this_info_equal = this_info_equal & (numpy.linalg.norm(grab_info._trelative - cloned_grab_info._trelative) < 1e-4) 91 | 92 | if this_info_equal: 93 | found_equal_info = True 94 | break 95 | 96 | #assert that there exists a grabbed info with equal properties 97 | self.assertTrue(found_equal_info) 98 | 99 | 100 | def test_cloning_start_config(self): 101 | self.check_cloned_attributes_collisions() 102 | 103 | def test_cloning_grasped_object_nocollision(self): 104 | self.assertFalse(self.env.CheckCollision(self.robot)) 105 | 106 | self.robot.Grab(self.to_grab_body) 107 | self.check_cloned_attributes_collisions() 108 | 109 | def test_cloning_grasped_object_collision(self): 110 | self.to_grab_body.SetTransform(self.manipulator.GetEndEffectorTransform()) 111 | self.assertTrue(self.env.CheckCollision(self.robot)) 112 | 113 | self.robot.Grab(self.to_grab_body) 114 | self.check_cloned_attributes_collisions() 115 | 116 | 117 | def test_cloning_grasped_object_nocollision_grablinks(self): 118 | self.assertFalse(self.env.CheckCollision(self.robot)) 119 | link = self.robot.GetLinks()[7] 120 | self.robot.Grab(self.to_grab_body, grablink=link, linkstoignore=[8, 9, 10]) 121 | self.check_cloned_attributes_collisions() 122 | 123 | def test_cloning_grasped_object_collision_grablinks(self): 124 | self.to_grab_body.SetTransform(self.manipulator.GetEndEffectorTransform()) 125 | self.assertTrue(self.env.CheckCollision(self.robot)) 126 | link = self.robot.GetLinks()[7] 127 | self.robot.Grab(self.to_grab_body, grablink=link, linkstoignore=[8, 9, 10]) 128 | self.check_cloned_attributes_collisions() 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | if __name__ == '__main__': 137 | unittest.main() 138 | -------------------------------------------------------------------------------- /src/prpy/viz.py: -------------------------------------------------------------------------------- 1 | import numpy, openravepy 2 | 3 | class RenderTrajectory: 4 | """ 5 | Context manager for rendering trajectories in the OpenRAVE viewer. This 6 | renders a trajectory as a smooth curve. The curve is are removed when the exit 7 | handler is called. 8 | @param robot robot executing the trajectory 9 | @param traj input trajectory 10 | @param num_samples number of samples to use for interpolation 11 | @param linewidth The width of the rendered line representing the trajecotry 12 | @param color interpolated line color 13 | @param render If true, perform the render 14 | """ 15 | def __init__(self, robot, traj, num_samples=20, linewidth=2, color=[1, 0, 0, 1], render=True): 16 | self.env = robot.GetEnv() 17 | self.robot = robot 18 | self.handles = list() 19 | self.render = render 20 | 21 | # Rendering options. 22 | self.num_samples = num_samples 23 | self.linewidth = linewidth 24 | self.color = numpy.array(color, dtype='float') 25 | 26 | # Clone and retime the trajectory. 27 | self.traj = openravepy.RaveCreateTrajectory(self.env, 'GenericTrajectory') 28 | self.traj.Clone(traj, 0) 29 | openravepy.planningutils.RetimeTrajectory(self.traj) 30 | 31 | def __enter__(self): 32 | 33 | if not self.render: 34 | return 35 | 36 | with self.env: 37 | with self.robot.CreateRobotStateSaver(): 38 | config_spec = self.traj.GetConfigurationSpecification() 39 | manipulators = self.robot.GetTrajectoryManipulators(self.traj) 40 | 41 | for manipulator in manipulators: 42 | arm_indices = manipulator.GetArmIndices() 43 | 44 | # Skip manipulators that don't have render_offset set. 45 | if hasattr(manipulator, "render_offset"): 46 | render_offset = manipulator.render_offset 47 | 48 | if render_offset is None: 49 | continue 50 | else: 51 | render_offset = [0., 0., 0., 1.] 52 | 53 | # Evenly interpolate joint values throughout the entire trajectory. 54 | interpolated_points = list() 55 | for t in numpy.linspace(0, self.traj.GetDuration(), self.num_samples): 56 | waypoint = self.traj.Sample(t) 57 | joint_values = config_spec.ExtractJointValues(waypoint, self.robot, arm_indices) 58 | manipulator.SetDOFValues(joint_values) 59 | hand_pose = manipulator.GetEndEffectorTransform() 60 | render_position = numpy.dot(hand_pose, render_offset) 61 | interpolated_points.append(render_position[0:3]) 62 | 63 | # Render a line through the interpolated points. 64 | interpolated_points = numpy.array(interpolated_points) 65 | if len(interpolated_points) > 0: 66 | handle = self.env.drawlinestrip(interpolated_points, self.linewidth, self.color) 67 | handle.SetShow(True) 68 | self.handles.append(handle) 69 | 70 | def __exit__(self, *exc_info): 71 | with self.env: 72 | del self.handles 73 | 74 | class RenderPoses(object): 75 | """ 76 | Render axis at a poses 77 | @param poses The poses to render 78 | @param env The OpenRAVE environment 79 | @param length The length of each axis 80 | @param render If false, this class does nothing 81 | """ 82 | def __init__(self, poses, env, length=0.2, render=True): 83 | self.env = env 84 | self.poses = poses 85 | self.length = length 86 | self.render = render 87 | 88 | def __enter__(self): 89 | if self.render: 90 | self.handles = [] 91 | for pose in self.poses: 92 | self.handles.append(openravepy.misc.DrawAxes(self.env, pose, dist=self.length)) 93 | 94 | def __exit__(self, exc_type, exc_value, traceback): 95 | self.handles = [] 96 | 97 | class RenderTSRList(RenderPoses): 98 | """ 99 | Render samples from a list of tsrs. 100 | @param tsr_list A list of TSRChain objects (typically output from a call to tsrlibrary) 101 | @param env The OpenRAVE environment 102 | @param num_samples The number of samples to render 103 | @param render If false, this class does nothing 104 | """ 105 | def __init__(self, tsr_list, env, num_samples=25, length=0.2, render=True): 106 | import random 107 | poses = [] 108 | for idx in range(num_samples): 109 | tsr_chain_idx = random.randint(0, len(tsr_list) - 1) 110 | tsr_chain = tsr_list[tsr_chain_idx] 111 | poses.append(tsr_chain.sample()) 112 | RenderPoses.__init__(self, poses, env, length=length, render=render) 113 | 114 | class RenderVector(object): 115 | ''' 116 | Render a vector in an openrave environment 117 | @param start_pt The start point of the vector 118 | @param direction The direction of the vector to render 119 | @param length The length of the rendered vector 120 | @param env The OpenRAVE environment 121 | @param render If false, this class does nothing 122 | ''' 123 | def __init__(self, start_pt, direction, length, env, render=True): 124 | self.env = env 125 | self.start_point = start_pt 126 | self.end_point = start_pt + numpy.array(direction)*length 127 | self.render = render 128 | 129 | def __enter__(self): 130 | if self.render: 131 | self.h = self.env.drawarrow(self.start_point, self.end_point) 132 | 133 | def __exit__(self, exc_type, exc_value, traceback): 134 | self.h = [] 135 | 136 | -------------------------------------------------------------------------------- /src/prpy/perception/vncc.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import vncc_msgs.srv 3 | import vncc_msgs.msg 4 | import math 5 | import tf 6 | import tf.transformations as transformations 7 | import numpy 8 | import os.path 9 | 10 | from base import PerceptionModule, PerceptionMethod 11 | 12 | 13 | class VnccModule(PerceptionModule): 14 | 15 | def __init__(self, kinbody_path, detection_frame, world_frame, 16 | service_namespace=None): 17 | """ 18 | This initializes a VNCC detector. 19 | 20 | @param kinbody_path The path to the folder where kinbodies are stored 21 | @param detection_frame The TF frame of the camera 22 | @param world_frame The desired world TF frame 23 | @param service_namespace The namespace for the VNCC service (default: /vncc) 24 | """ 25 | 26 | # Initialize a new ros node if one has not already been created 27 | try: 28 | rospy.init_node('vncc_detector', anonymous=True) 29 | except rospy.exceptions.ROSException: 30 | pass 31 | 32 | #For getting transforms in world frame 33 | if detection_frame is not None and world_frame is not None: 34 | self.listener = tf.TransformListener() 35 | else: 36 | self.listener = None 37 | 38 | if service_namespace is None: 39 | service_namespace='/vncc' 40 | 41 | self.detection_frame = detection_frame 42 | self.world_frame = world_frame 43 | self.service_namespace = service_namespace 44 | 45 | self.kinbody_path = kinbody_path 46 | 47 | # A map of known objects that can be queries 48 | self.kinbody_to_query_map = {'plastic_plate': 'plate', 49 | 'plastic_bowl': 'bowl'} 50 | 51 | @staticmethod 52 | def _MsgToPose(msg): 53 | """ 54 | Parse the ROS message to a 4x4 pose format 55 | @param msg The ros message containing a pose 56 | @return A 4x4 transformation matrix containing the pose 57 | as read from the message 58 | """ 59 | #Get translation and rotation (from Euler angles) 60 | pose = transformations.euler_matrix(msg.roll*0.0,msg.pitch*0.0,msg.yaw*0.0) 61 | 62 | pose[0,3] = msg.pt.x 63 | pose[1,3] = msg.pt.y 64 | pose[2,3] = msg.pt.z 65 | 66 | return pose 67 | 68 | def _LocalToWorld(self,pose): 69 | """ 70 | Transform a pose from local frame to world frame 71 | @param pose The 4x4 transformation matrix containing the pose to transform 72 | @return The 4x4 transformation matrix describing the pose in world frame 73 | """ 74 | #Get pose w.r.t world frame 75 | self.listener.waitForTransform(self.world_frame,self.detection_frame, 76 | rospy.Time(),rospy.Duration(10)) 77 | t, r = self.listener.lookupTransform(self.world_frame,self.detection_frame, 78 | rospy.Time(0)) 79 | 80 | #Get relative transform between frames 81 | offset_to_world = numpy.matrix(transformations.quaternion_matrix(r)) 82 | offset_to_world[0,3] = t[0] 83 | offset_to_world[1,3] = t[1] 84 | offset_to_world[2,3] = t[2] 85 | 86 | #Compose with pose to get pose in world frame 87 | result = numpy.array(numpy.dot(offset_to_world, pose)) 88 | 89 | return result 90 | 91 | 92 | def _GetDetection(self, obj_name): 93 | """ 94 | Calls the service to get a detection of a particular object. 95 | @param obj_name The name of the object to detect 96 | @return A 4x4 transformation matrix describing the pose of the object 97 | in world frame, None if the object is not detected 98 | """ 99 | #Call detection service for a particular object 100 | detect_vncc = rospy.ServiceProxy(self.service_namespace+'/get_vncc_detections', 101 | vncc_msgs.srv.GetDetections) 102 | 103 | detect_resp = detect_vncc(object_name=obj_name) 104 | 105 | if detect_resp.ok == False: 106 | return None 107 | 108 | #Assumes one instance of object 109 | result = self._MsgToPose(detect_resp.detections[0]) 110 | if (self.detection_frame is not None and self.world_frame is not None): 111 | result = self._LocalToWorld(result) 112 | result[:3,:3] = numpy.eye(3) 113 | return result 114 | 115 | @PerceptionMethod 116 | def DetectObject(self, robot, obj_name, **kw_args): 117 | """ 118 | Detect a single object 119 | @param robot The OpenRAVE robot 120 | @param obj_name The name of the object to detect 121 | @returns A kinbody placed in the detected location 122 | @throws PerceptionException if the object is not detected 123 | """ 124 | 125 | from prpy.perception.base import PerceptionException 126 | 127 | if obj_name not in self.kinbody_to_query_map: 128 | raise PerceptionException( 129 | 'The VNCC module cannot detect object {:s}'.format(obj_name)) 130 | 131 | query_name = self.kinbody_to_query_map[obj_name] 132 | obj_pose = self._GetDetection(query_name) 133 | if obj_pose is None: 134 | raise PerceptionException( 135 | 'Failed to detect object {:s}'.format(obj_name)) 136 | 137 | env = robot.GetEnv() 138 | if env.GetKinBody(obj_name) is None: 139 | from prpy.rave import add_object 140 | kinbody_file = '{:s}.kinbody.xml'.format(obj_name) 141 | new_body = add_object( 142 | env, 143 | obj_name, 144 | os.path.join(self.kinbody_path, kinbody_file)) 145 | 146 | body = env.GetKinBody(obj_name) 147 | body.SetTransform(obj_pose) 148 | return body 149 | -------------------------------------------------------------------------------- /src/prpy/simulation/servo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import atexit, logging, numpy, openravepy, threading, time 32 | 33 | class ServoSimulator(object): 34 | def __init__(self, manip, rate, watchdog_timeout): 35 | self.manip = manip 36 | 37 | # enabling threading and the following lines causes robot (and hence 38 | # the environment) to not be garbage collected. It seems that threading 39 | # somehow blocks self.robot from being garbage collected 40 | #self.robot = self.manip.GetRobot() 41 | #self.env = self.robot.GetEnv() 42 | 43 | self.indices = self.manip.GetArmIndices() 44 | self.num_dofs = len(self.indices) 45 | self.q_dot = numpy.zeros(self.num_dofs) 46 | 47 | self.running = False 48 | self.watchdog = time.time() 49 | self.watchdog_timeout = watchdog_timeout 50 | 51 | self.period = 1.0 / rate 52 | self.timer = None 53 | self.mutex = threading.Lock() 54 | self.event = threading.Event() 55 | 56 | self.Start() 57 | 58 | def Start(self): 59 | self.thread = threading.Thread(target=self.Step) 60 | self.thread.daemon = True 61 | self.thread.start() 62 | 63 | def Stop(self): 64 | self.event.set() 65 | self.thread.join() 66 | 67 | def SetVelocity(self, q_dot): 68 | with self.manip.GetRobot().GetEnv(): 69 | q_dot_limits = self.manip.GetRobot().GetDOFVelocityLimits(self.indices) 70 | 71 | if not (numpy.abs(q_dot) <= q_dot_limits).all(): 72 | raise openravepy.openrave_exception('Desired velocity exceeds limits.') 73 | 74 | with self.mutex: 75 | if (q_dot != numpy.zeros(self.num_dofs)).any(): 76 | self.q_dot = numpy.array(q_dot, dtype='float') 77 | self.watchdog = time.time() 78 | self.running = True 79 | else: 80 | self.running = False 81 | 82 | def Step(self): 83 | while True: 84 | start_time = time.time() 85 | with self.mutex: 86 | # Copy the velocity for thread safety. 87 | q_dot = self.q_dot.copy() 88 | running = self.running 89 | 90 | # Stop servoing when the watchdog times out. 91 | if running and start_time - self.watchdog > self.watchdog_timeout: 92 | self.q_dot = numpy.zeros(self.num_dofs) 93 | self.running = False 94 | logging.warning('Servo motion timed out in %.3f seconds.', start_time - self.watchdog) 95 | 96 | if running: 97 | env = self.manip.GetRobot().GetEnv() 98 | with env: 99 | try: 100 | q = self.manip.GetDOFValues() 101 | q += self.period * q_dot 102 | except openravepy.openrave_exception as e: 103 | # Otherwise, the those threads may access the OpenRAVE 104 | # environment after it has been destroyed. This can 105 | # occur if an exception is thrown elsewhere in the code 106 | # and can generate very confusing error messages. We'll 107 | # catch the exception here and cleanly exit. 108 | if e.GetCode() == openravepy.ErrorCode.NotInitialized: 109 | self.event.set() 110 | break 111 | else: 112 | raise 113 | 114 | # Check joint limits. 115 | sp = openravepy.Robot.SaveParameters 116 | with self.manip.GetRobot().CreateRobotStateSaver(sp.ActiveDOF | sp.ActiveManipulator): 117 | self.manip.SetActive() 118 | q_min, q_max = self.manip.GetRobot().GetActiveDOFLimits() 119 | 120 | if ((q_min <= q).all() and (q <= q_max).all()): 121 | self.manip.SetDOFValues(q) 122 | else: 123 | self.running = False 124 | logging.warning('Servo motion hit a joint limit.') 125 | if self.event.wait(max(self.period - (time.time()-start_time), 0.)): 126 | break 127 | 128 | -------------------------------------------------------------------------------- /src/prpy/planning/logged.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2016, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Chris Dellin 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | from __future__ import print_function 32 | from __future__ import unicode_literals 33 | from __future__ import absolute_import 34 | from __future__ import division 35 | import collections 36 | import math 37 | import time 38 | 39 | import openravepy 40 | import numpy 41 | import yaml 42 | 43 | import prpy.planning.base 44 | import prpy.serialization 45 | 46 | def serialize_or_errorstr(value): 47 | try: 48 | return prpy.serialization.serialize(value) 49 | except prpy.exceptions.UnsupportedTypeSerializationException as ex: 50 | return 'Error: {}'.format(str(ex)) 51 | 52 | class LoggedPlanner(prpy.planning.base.MetaPlanner): 53 | """Planner wrapper which serializes planning requets and responses. 54 | 55 | LoggedPlanner delegates all planning calls to the sub-planner 56 | passed to it on construction. It does not affect the input to 57 | or output from this planner. 58 | 59 | On every request, LoggedPlanner serializes the environment, 60 | planning method request arguments, and the returned trajectory. 61 | It creates a timestamped yaml file in the current directory: 62 | - log-20160101-010203.567890.yaml 63 | """ 64 | 65 | def __init__(self, planner, uri_only=True): 66 | super(LoggedPlanner, self).__init__() 67 | self.planner = planner 68 | self.uri_only = uri_only 69 | 70 | def __str__(self): 71 | return 'Logged({0:s})'.format(self.planner) 72 | 73 | def has_planning_method(self, method_name): 74 | return self.planner.has_planning_method(method_name) 75 | 76 | def get_planning_method_names(self): 77 | return self.planner.get_planning_method_names() 78 | 79 | def get_planners(self, method_name): 80 | return [self.planner] 81 | 82 | def plan(self, method, args, kw_args): 83 | 84 | # get log file name 85 | stamp = time.time() 86 | struct = time.localtime(stamp) 87 | fn = 'log-{}.{:06d}.yaml'.format( 88 | time.strftime('%Y%m%d-%H%M%S', struct), 89 | int(1.0e6*(stamp-math.floor(stamp)))) 90 | 91 | # retrieve enviroment 92 | if 'robot' in kw_args: 93 | env = kw_args['robot'].GetEnv() 94 | elif len(args) > 0: 95 | env = args[0].GetEnv() 96 | else: 97 | raise RuntimeError('could not retrieve env from planning request!') 98 | 99 | # serialize environment 100 | envdict = prpy.serialization.serialize_environment(env, uri_only=self.uri_only) 101 | 102 | # serialize planning request 103 | reqdict = {} 104 | reqdict['method'] = method # string 105 | reqdict['args'] = [] 106 | for arg in args: 107 | reqdict['args'].append(serialize_or_errorstr(arg)) 108 | reqdict['kw_args'] = {} 109 | for key,value in kw_args.items(): 110 | # if key serialization fails, it thros UnsupportedTypeSerializationException 111 | key = prpy.serialization.serialize(key) 112 | reqdict['kw_args'][key] = serialize_or_errorstr(value) 113 | 114 | # get ready to serialize result 115 | resdict = {} 116 | 117 | try: 118 | plan_fn = getattr(self.planner, method) 119 | traj = plan_fn(*args, **kw_args) 120 | resdict['ok'] = True 121 | resdict['traj_first'] = list(map(float,traj.GetWaypoint(0))) 122 | resdict['traj_last'] = list(map(float,traj.GetWaypoint(traj.GetNumWaypoints()-1))) 123 | resdict['traj'] = traj.serialize(0) 124 | return traj 125 | 126 | except prpy.planning.PlanningError as ex: 127 | resdict['ok'] = False 128 | resdict['exception'] = str(ex) 129 | raise 130 | 131 | except: 132 | resdict['ok'] = False 133 | resdict['exception'] = 'Unknown exception type: {}'.format(repr(sys.exc_info()[1])) 134 | raise 135 | 136 | finally: 137 | # create yaml dictionary to be serialized 138 | yamldict = {} 139 | yamldict['environment'] = envdict 140 | yamldict['request'] = reqdict 141 | yamldict['result'] = resdict 142 | fp = open(fn,'w') 143 | yaml.safe_dump(yamldict, fp) 144 | fp.close() 145 | 146 | 147 | -------------------------------------------------------------------------------- /src/prpy/tactile.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import numpy, openravepy 32 | 33 | class TactileArray(object): 34 | def __init__(self, offset, origins, normals): 35 | self.offset = numpy.array(offset, dtype='float') 36 | self.origins = numpy.array(origins, dtype='float') 37 | self.normals = numpy.array(normals, dtype='float') 38 | 39 | def __repr__(self): 40 | return 'TactileArray(offset={0:r}, origins=<{1:d}x{2:d} array>, normals=<{3:d}x{4:d} array>)'.format( 41 | self.offset, self.origins.shape[0], self.origins.shape[1], 42 | self.normals.shape[0], self.origins.shape[1]) 43 | 44 | def __len__(self): 45 | return self.origins.shape[0] 46 | 47 | def get_geometry(self, link_pose): 48 | offset = self.get_offset(link_pose) 49 | origins = numpy.dot(offset[0:3, 0:3], self.origins.T) + offset[0:3, 3].reshape((3, 1)) 50 | normals = numpy.dot(offset[0:3, 0:3], self.normals.T) 51 | return origins.T, normals.T 52 | 53 | def get_offset(self, link_pose): 54 | return numpy.dot(link_pose, self.offset) 55 | 56 | @classmethod 57 | def from_yaml(cls, array_yaml): 58 | offset_quaternion = numpy.array(array_yaml['offset']['orientation'], dtype='float') 59 | offset_pose = openravepy.matrixFromQuat(offset_quaternion) 60 | offset_pose[0:3, 3] = numpy.array(array_yaml['offset']['position'], dtype='float') 61 | return cls(offset_pose, array_yaml['origin'], array_yaml['normal']) 62 | 63 | class TactileSensor(object): 64 | def __init__(self): 65 | self.arrays = dict() 66 | 67 | def get_geometry(self, robot): 68 | all_origins = list() 69 | all_normals = list() 70 | 71 | for link_name, tactile_array in self.arrays.items(): 72 | link_pose = robot.GetLink(link_name).GetTransform() 73 | array_origins, array_normals = tactile_array.get_geometry(link_pose) 74 | all_origins.append(array_origins) 75 | all_normals.append(array_normals) 76 | 77 | return numpy.vstack(all_origins), numpy.vstack(all_normals) 78 | 79 | def render_values(self, robot, values, scale=1.0, color=None, linewidth=2): 80 | all_lines = list() 81 | 82 | if color is None: 83 | color = numpy.array([ 1., 0., 0., 1. ]) 84 | 85 | if isinstance(values, dict): 86 | all_values = list() 87 | for link_name in self.arrays.keys(): 88 | all_values.extend(values[link_name]) 89 | else: 90 | all_values = values 91 | 92 | num_cells = len(all_values) 93 | all_values = numpy.array(all_values, dtype='float') 94 | origins, normals = self.get_geometry(robot) 95 | lines = numpy.empty((2 * num_cells, 3)) 96 | lines[0::2, :] = origins 97 | lines[1::2, :] = origins + scale * all_values.reshape((num_cells, 1)) * normals 98 | return robot.GetEnv().drawlinelist(lines, linewidth, color) 99 | 100 | def render_cells(self, robot, origins=True, normals=True, spheres=True, color=None, 101 | size=0.0025, linewidth=2, length=0.01): 102 | if color is None: 103 | color = numpy.array([ 1., 1., 0., 1. ]) 104 | 105 | all_origins, all_normals = self.get_geometry(robot) 106 | handles = list() 107 | 108 | if normals: 109 | lines = numpy.empty((2 * all_origins.shape[0], 3)) 110 | lines[0::2, :] = all_origins 111 | lines[1::2, :] = all_origins + length * all_normals 112 | handle = robot.GetEnv().drawlinelist(lines, linewidth, color) 113 | handles.append(handle) 114 | 115 | if spheres: 116 | handle = robot.GetEnv().plot3(all_origins, size, color, True) 117 | handles.append(handle) 118 | 119 | if origins: 120 | for tactile_array in self.arrays.values(): 121 | handle = openravepy.misc.DrawAxes(robot.GetEnv(), tactile_array.offset, 122 | dist=0.02, linewidth=linewidth) 123 | handles.append(handle) 124 | 125 | return handles 126 | 127 | @classmethod 128 | def from_yaml(cls, path): 129 | # Load the tactile cell origins and normals from YAML. 130 | with open(path, 'rb') as stream: 131 | import yaml 132 | tactile_yaml = yaml.load(stream) 133 | 134 | # Create a TactileArray object for each entry in the file. 135 | sensor = cls() 136 | for link_name, array_yaml in tactile_yaml.items(): 137 | sensor.arrays[link_name] = TactileArray.from_yaml(array_yaml) 138 | 139 | return sensor 140 | -------------------------------------------------------------------------------- /tests/planning/methods/PlanToEndEffectorOffset.py: -------------------------------------------------------------------------------- 1 | import numpy 2 | from prpy.clone import CloneException 3 | from prpy.planning.base import PlanningError 4 | from prpy.planning.exceptions import ( 5 | CollisionPlanningError, 6 | SelfCollisionPlanningError, 7 | ConstraintViolationPlanningError, 8 | JointLimitError) 9 | 10 | 11 | class PlanToEndEffectorOffsetTest(object): 12 | # This configuration is feasible, but it's not possible to move up due to 13 | # collision with the environment. 14 | config_infeasible_env_movement = numpy.array([ 15 | 2.4393138295135239, 1.92465643247649250, -0.74016692563109798, 16 | 1.5874962800295755, 0.81768598344294929, -1.2573553146693555, 17 | 2.35727649844962 18 | ]) 19 | 20 | # This configuration is feasible, but it's not possible to move down due to 21 | # self-collision. 22 | config_infeasible_self_movement = numpy.array([ 23 | -0.0057795025512650264, -1.14744762192955350, 2.5500000000000003, 24 | 2.5469121802527628000, -0.88347846589538992, -1.73131950448453797, 25 | -2.6602830007072269 26 | ]) 27 | 28 | def setUp(self): 29 | self.direction = numpy.array([ 0., 0., 1. ]) 30 | self.distance = 0.1 31 | 32 | def test_PlanToEndEffectorOffset_SatisfiesConstraint(self): 33 | from numpy.testing import assert_allclose 34 | 35 | # Setup 36 | with self.env: 37 | cspec = self.robot.GetActiveConfigurationSpecification() 38 | self.robot.SetActiveDOFValues(self.config_feasible_start) 39 | start_ik = self.manipulator.GetEndEffectorTransform() 40 | 41 | # Test 42 | path = self.planner.PlanToEndEffectorOffset( 43 | self.robot, direction=self.direction, distance=self.distance) 44 | 45 | # Assert 46 | self.ValidatePath(path) 47 | 48 | distance = 0. 49 | first_waypoint = path.GetWaypoint(0) 50 | assert_allclose(first_waypoint, self.config_feasible_start) 51 | 52 | for iwaypoint in xrange(1, path.GetNumWaypoints()): 53 | with self.env: 54 | waypoint = path.GetWaypoint(iwaypoint, cspec) 55 | self.robot.SetActiveDOFValues(waypoint) 56 | actual_ik = self.manipulator.GetEndEffectorTransform() 57 | 58 | # Verify that the position is monotone. 59 | distance = numpy.linalg.norm(actual_ik[0:3, 3] - start_ik[0:3, 3]) 60 | expected_ik = start_ik.copy() 61 | expected_ik[0:3, 3] += distance * numpy.array([ 0., 0., 1. ]) 62 | 63 | self.assertTransformClose(actual_ik, expected_ik, 64 | linear_tol=0.005, angular_tol=0.2) 65 | 66 | self.assertAlmostEqual(distance, 0.1, delta=0.005) 67 | 68 | def test_PlanToEndEffectorOffset_StartInCollision_Throws(self): 69 | # Setup 70 | with self.env: 71 | self.robot.SetActiveDOFValues(self.config_feasible_goal) 72 | goal_ik = self.manipulator.GetEndEffectorTransform() 73 | 74 | self.robot.SetActiveDOFValues(self.config_env_collision) 75 | 76 | # Test/Assert 77 | with self.assertRaises(PlanningError): 78 | self.planner.PlanToEndEffectorOffset( 79 | self.robot, direction=self.direction, distance=self.distance) 80 | 81 | def test_PlanToEndEffectorOffset_StartInSelfCollision_Throws(self): 82 | # Setup 83 | with self.env: 84 | self.robot.SetActiveDOFValues(self.config_feasible_goal) 85 | goal_ik = self.manipulator.GetEndEffectorTransform() 86 | 87 | self.robot.SetActiveDOFValues(self.config_self_collision) 88 | 89 | # Test/Assert 90 | with self.assertRaises((PlanningError, CloneException)): 91 | self.planner.PlanToEndEffectorOffset( 92 | self.robot, direction=self.direction, distance=self.distance) 93 | 94 | def test_PlanToEndEffectorOffset_GoalInEnvCollision_Throws(self): 95 | # Setup 96 | with self.env: 97 | self.robot.SetActiveDOFValues(self.config_infeasible_env_movement) 98 | 99 | # Test/Assert 100 | with self.assertRaises(PlanningError): 101 | self.planner.PlanToEndEffectorOffset( 102 | self.robot, direction=self.direction, distance=self.distance) 103 | 104 | def test_PlanToEndEffectorOffset_GoalInSelfCollision_Throws(self): 105 | # Setup 106 | with self.env: 107 | self.robot.SetActiveDOFValues(self.config_infeasible_self_movement) 108 | 109 | # Test/Assert 110 | with self.assertRaises(PlanningError): 111 | self.planner.PlanToEndEffectorOffset( 112 | self.robot, direction=numpy.array([0., 0., -1.]), 113 | distance=0.1) 114 | 115 | class PlanToEndEffectorOffsetCollisionTest(object): 116 | """ Expects collision-specific error""" 117 | def setUp(self): 118 | self.direction = numpy.array([ 0., 0., 1. ]) 119 | self.distance = 0.1 120 | 121 | def test_PlanToEndEffectorOffset_StartInCollision_Throws_CollisionError(self): 122 | # Setup 123 | with self.env: 124 | self.robot.SetActiveDOFValues(self.config_feasible_goal) 125 | goal_ik = self.manipulator.GetEndEffectorTransform() 126 | self.robot.SetActiveDOFValues(self.config_env_collision) 127 | 128 | # Test/Assert 129 | with self.assertRaises((CollisionPlanningError, 130 | SelfCollisionPlanningError, 131 | JointLimitError, 132 | ConstraintViolationPlanningError 133 | )): 134 | self.planner.PlanToEndEffectorOffset( 135 | self.robot, direction=self.direction, distance=self.distance) 136 | 137 | def test_PlanToEndEffectorOffset_GoalInEnvCollision_Throws_CollisionError(self): 138 | # Setup 139 | with self.env: 140 | self.robot.SetActiveDOFValues( 141 | PlanToEndEffectorOffsetTest.config_infeasible_env_movement) 142 | 143 | # Test/Assert 144 | with self.assertRaises((CollisionPlanningError, 145 | SelfCollisionPlanningError, 146 | JointLimitError, 147 | ConstraintViolationPlanningError, 148 | )): 149 | self.planner.PlanToEndEffectorOffset( 150 | self.robot, direction=self.direction, distance=self.distance) 151 | -------------------------------------------------------------------------------- /src/prpy/planning/ik.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | import logging 31 | import numpy 32 | import openravepy 33 | from .. import ik_ranking 34 | from base import (Planner, 35 | PlanningError, 36 | LockedPlanningMethod) 37 | 38 | logger = logging.getLogger(__name__) 39 | 40 | 41 | class IKPlanner(Planner): 42 | def __init__(self, delegate_planner=None): 43 | super(IKPlanner, self).__init__() 44 | self.delegate_planner = delegate_planner 45 | 46 | def __str__(self): 47 | return 'IKPlanner' 48 | 49 | 50 | @LockedPlanningMethod 51 | def PlanToIK(self, robot, pose, **kwargs): 52 | """ 53 | Plan to a desired end effector pose with IKPlanner. 54 | 55 | An IK ranking function can optionally be specified to select a 56 | preferred IK solution from those available at the goal pose. 57 | 58 | @param robot the robot whose active manipulator will be used 59 | @param pose the desired manipulator end effector pose 60 | @return traj a trajectory from current configuration to specified pose 61 | """ 62 | return self._PlanToIK(robot, pose, **kwargs) 63 | 64 | @LockedPlanningMethod 65 | def PlanToEndEffectorPose(self, robot, pose, **kwargs): 66 | """ 67 | Plan to a desired end effector pose with IKPlanner. 68 | 69 | This function is internally implemented identically to PlanToIK(). 70 | 71 | @param robot the robot whose active manipulator will be used 72 | @param pose the desired manipulator end effector pose 73 | @return traj a trajectory from current configuration to specified pose 74 | """ 75 | return self._PlanToIK(robot, pose, **kwargs) 76 | 77 | 78 | def _PlanToIK(self, robot, goal_pose, ranker=None, num_attempts=1, **kw_args): 79 | from openravepy import (IkFilterOptions, 80 | IkParameterization, 81 | IkParameterizationType) 82 | 83 | manipulator = robot.GetActiveManipulator() 84 | 85 | if ranker is None: 86 | ranker = ik_ranking.NominalConfiguration(manipulator.GetArmDOFValues()) 87 | 88 | # Find an unordered list of IK solutions. 89 | with robot.GetEnv(): 90 | ik_param = IkParameterization( 91 | goal_pose, IkParameterizationType.Transform6D) 92 | ik_solutions = manipulator.FindIKSolutions( 93 | ik_param, IkFilterOptions.CheckEnvCollisions, 94 | ikreturn=False, releasegil=True 95 | ) 96 | 97 | if ik_solutions.shape[0] == 0: 98 | raise PlanningError( 99 | 'There is no IK solution at the goal pose.', deterministic=True) 100 | 101 | # Sort the IK solutions in ascending order by the costs returned by the 102 | # ranker. Lower cost solutions are better and infinite cost solutions 103 | # are assumed to be infeasible. 104 | scores = ranker(robot, ik_solutions) 105 | ranked_indices = numpy.argsort(scores) 106 | ranked_indices = ranked_indices[~numpy.isposinf(scores)] 107 | ranked_ik_solutions = ik_solutions[ranked_indices, :] 108 | 109 | if ranked_ik_solutions.shape[0] == 0: 110 | raise PlanningError('All IK solutions have infinite cost.', deterministic=True) 111 | 112 | # Sequentially plan to the solutions in descending order of cost. 113 | planner = self.delegate_planner or robot.planner 114 | p = openravepy.KinBody.SaveParameters 115 | 116 | all_deterministic=True 117 | with robot.CreateRobotStateSaver(p.ActiveDOF): 118 | robot.SetActiveDOFs(manipulator.GetArmIndices()) 119 | 120 | num_attempts = min(ranked_ik_solutions.shape[0], num_attempts) 121 | for i, ik_sol in enumerate(ranked_ik_solutions[0:num_attempts, :]): 122 | try: 123 | traj = planner.PlanToConfiguration(robot, ik_sol) 124 | logger.info('Planned to IK solution %d of %d.', 125 | i + 1, num_attempts) 126 | return traj 127 | except PlanningError as e: 128 | logger.warning( 129 | 'Planning to IK solution %d of %d failed: %s', 130 | i + 1, num_attempts, e) 131 | if e.deterministic is None: 132 | all_deterministic = False 133 | logger.warning( 134 | 'Planner %s raised a PlanningError without the' 135 | ' "deterministic" flag set. Assuming the result' 136 | ' is non-deterministic.', planner) 137 | elif not e.deterministic: 138 | all_deterministic = False 139 | 140 | raise PlanningError( 141 | 'Planning to the top {:d} of {:d} IK solutions failed.' 142 | .format(num_attempts, ranked_ik_solutions.shape[0]), deterministic=all_deterministic) 143 | -------------------------------------------------------------------------------- /src/prpy/planning/openrave.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Siddhartha Srinivasa 6 | # Michael Koval 7 | # Pras Velagapudi 8 | # 9 | # Redistribution and use in source and binary forms, with or without 10 | # modification, are permitted provided that the following conditions are met: 11 | # 12 | # - Redistributions of source code must retain the above copyright notice, this 13 | # list of conditions and the following disclaimer. 14 | # - Redistributions in binary form must reproduce the above copyright notice, 15 | # this list of conditions and the following disclaimer in the documentation 16 | # and/or other materials provided with the distribution. 17 | # - Neither the name of Carnegie Mellon University nor the names of its 18 | # contributors may be used to endorse or promote products derived from this 19 | # software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | import numpy 34 | import openravepy 35 | from base import (Planner, 36 | PlanningError, 37 | UnsupportedPlanningError, 38 | LockedPlanningMethod) 39 | 40 | 41 | class OpenRAVEPlanner(Planner): 42 | def __init__(self, algorithm='birrt'): 43 | super(OpenRAVEPlanner, self).__init__() 44 | 45 | self.setup = False 46 | self.algorithm = algorithm 47 | 48 | def __str__(self): 49 | return 'OpenRAVE {0:s}'.format(self.algorithm) 50 | 51 | @LockedPlanningMethod 52 | def PlanToConfiguration(self, robot, goal, **kw_args): 53 | """ 54 | Plan to a desired configuration with OpenRAVE. This will invoke the 55 | OpenRAVE planner specified in the OpenRAVEPlanner constructor. 56 | @param robot the robot whose active DOFs will be used 57 | @param goal the desired robot joint configuration 58 | @return traj a trajectory from current configuration to specified goal 59 | """ 60 | 61 | return self._Plan(robot, goal, **kw_args) 62 | 63 | def _Plan(self, robot, goals, maxiter=500, continue_planner=False, 64 | or_args=None, **kw_args): 65 | 66 | env = robot.GetEnv() 67 | 68 | planner = openravepy.RaveCreatePlanner(env, self.algorithm) 69 | if planner is None: 70 | raise UnsupportedPlanningError('Unable to create {:s} module.' 71 | .format(str(self))) 72 | 73 | # Get rid of default postprocessing 74 | extraParams = ('<_postprocessing planner="">' 75 | '<_nmaxiterations>0' 76 | '') 77 | # Maximum planner iterations 78 | extraParams += ('<_nmaxiterations>{:d}' 79 | .format(maxiter)) 80 | 81 | if or_args is not None: 82 | for key, value in or_args.iteritems(): 83 | extraParams += '<{k:s}>{v:s}'.format(k=str(key), 84 | v=str(value)) 85 | 86 | params = openravepy.Planner.PlannerParameters() 87 | params.SetRobotActiveJoints(robot) 88 | params.SetGoalConfig(goals) 89 | params.SetExtraParameters(extraParams) 90 | 91 | traj = openravepy.RaveCreateTrajectory(env, 'GenericTrajectory') 92 | 93 | try: 94 | env.Lock() 95 | 96 | with robot.CreateRobotStateSaver( 97 | openravepy.Robot.SaveParameters.LinkTransformation): 98 | # Plan. 99 | if (not continue_planner) or not self.setup: 100 | planner.InitPlan(robot, params) 101 | self.setup = True 102 | 103 | status = planner.PlanPath(traj, releasegil=True) 104 | from openravepy import PlannerStatus 105 | if status not in [PlannerStatus.HasSolution, 106 | PlannerStatus.InterruptedWithSolution]: 107 | raise PlanningError('Planner returned with status {:s}.' 108 | .format(str(status))) 109 | except Exception as e: 110 | raise PlanningError('Planning failed with error: {:s}'.format(e)) 111 | finally: 112 | env.Unlock() 113 | 114 | return traj 115 | 116 | 117 | class BiRRTPlanner(OpenRAVEPlanner): 118 | 119 | def __init__(self): 120 | OpenRAVEPlanner.__init__(self, algorithm='birrt') 121 | 122 | @LockedPlanningMethod 123 | def PlanToConfiguration(self, robot, goal, **kw_args): 124 | """ 125 | Plan to a desired configuration with OpenRAVE. This will invoke the 126 | OpenRAVE planner specified in the OpenRAVEPlanner constructor. 127 | @param robot the robot whose active DOFs will be used 128 | @param goal the desired robot joint configuration 129 | @return traj a trajectory from current configuration to specified goal 130 | """ 131 | return self._Plan(robot, goal, **kw_args) 132 | 133 | @LockedPlanningMethod 134 | def PlanToConfigurations(self, robot, goals, **kw_args): 135 | """ 136 | Plan to one of many configuration with OpenRAVE's BiRRT planner. 137 | @param robot the robot whose active DOFs will be used 138 | @param goals a list of desired robot joint configurations 139 | @return traj trajectory from current configuration to one of the goals 140 | """ 141 | if len(goals[0]) != len(robot.GetActiveDOFIndices()): 142 | raise ValueError('Goals must be same length as robot active DOFs.') 143 | 144 | # Serialize list of goals into a single 1D vector 145 | # (This will raise ValueError if the goals are not equal length.) 146 | goals = numpy.ravel(numpy.vstack(goals)) 147 | return self._Plan(robot, goals, **kw_args) 148 | -------------------------------------------------------------------------------- /src/prpy/planning/snap.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | import numpy 31 | import openravepy 32 | from openravepy import Robot 33 | from prpy.util import SetTrajectoryTags 34 | from prpy.planning.base import ( 35 | Planner, 36 | PlanningError, 37 | LockedPlanningMethod, 38 | Tags 39 | ) 40 | from ..collision import DefaultRobotCollisionCheckerFactory 41 | 42 | 43 | class SnapPlanner(Planner): 44 | """Planner that checks the straight-line trajectory to the goal. 45 | 46 | SnapPlanner is a utility planner class that collision checks the 47 | straight-line trajectory to the goal. If that trajectory is invalid, 48 | e.g. due to an environment or self collision, the planner 49 | immediately returns failure by raising a PlanningError. 50 | 51 | SnapPlanner is intended to be used only as a "short circuit" to 52 | speed-up planning between nearby configurations. This planner is 53 | most commonly used as the first item in a Sequence meta-planner to 54 | avoid calling a motion planner when the trivial solution is valid. 55 | """ 56 | def __init__(self, robot_checker_factory=None): 57 | super(SnapPlanner, self).__init__() 58 | 59 | if robot_checker_factory is None: 60 | robot_checker_factory = DefaultRobotCollisionCheckerFactory 61 | 62 | self.robot_checker_factory = robot_checker_factory 63 | 64 | def __str__(self): 65 | return 'SnapPlanner' 66 | 67 | @LockedPlanningMethod 68 | def PlanToConfiguration(self, robot, goal, **kw_args): 69 | """ 70 | Attempt to plan a straight line trajectory from the robot's 71 | current configuration to the goal configuration. This will 72 | fail if the straight-line path is not collision free. 73 | 74 | @param robot 75 | @param goal desired configuration 76 | @return traj 77 | """ 78 | return self._Snap(robot, goal, **kw_args) 79 | 80 | def _Snap(self, robot, goal, **kw_args): 81 | from prpy.util import CheckJointLimits 82 | from prpy.util import GetLinearCollisionCheckPts 83 | from prpy.planning.exceptions import CollisionPlanningError 84 | from prpy.planning.exceptions import SelfCollisionPlanningError 85 | 86 | # Create a two-point trajectory between the 87 | # current configuration and the goal. 88 | # (a straight line in joint space) 89 | env = robot.GetEnv() 90 | traj = openravepy.RaveCreateTrajectory(env, '') 91 | cspec = robot.GetActiveConfigurationSpecification('linear') 92 | active_indices = robot.GetActiveDOFIndices() 93 | 94 | # Check the start position is within joint limits, 95 | # this can throw a JointLimitError 96 | start = robot.GetActiveDOFValues() 97 | CheckJointLimits(robot, start, deterministic=True) 98 | 99 | # Add the start waypoint 100 | start_waypoint = numpy.zeros(cspec.GetDOF()) 101 | cspec.InsertJointValues(start_waypoint, start, robot, 102 | active_indices, False) 103 | traj.Init(cspec) 104 | traj.Insert(0, start_waypoint.ravel()) 105 | 106 | # Make the trajectory end at the goal configuration, as 107 | # long as it is not in collision and is not identical to 108 | # the start configuration. 109 | CheckJointLimits(robot, goal, deterministic=True) 110 | if not numpy.allclose(start, goal): 111 | goal_waypoint = numpy.zeros(cspec.GetDOF()) 112 | cspec.InsertJointValues(goal_waypoint, goal, robot, 113 | active_indices, False) 114 | traj.Insert(1, goal_waypoint.ravel()) 115 | 116 | # Get joint configurations to check 117 | # Note: this returns a python generator, and if the 118 | # trajectory only has one waypoint then only the 119 | # start configuration will be collisioned checked. 120 | # 121 | # Sampling function: 122 | # 'linear' 123 | # from prpy.util import SampleTimeGenerator 124 | # linear = SampleTimeGenerator 125 | # 'Van der Corput' 126 | from prpy.util import VanDerCorputSampleGenerator 127 | vdc = VanDerCorputSampleGenerator 128 | 129 | checks = GetLinearCollisionCheckPts(robot, traj, 130 | norm_order=2, 131 | sampling_func=vdc) 132 | 133 | with self.robot_checker_factory(robot) as robot_checker, \ 134 | robot.CreateRobotStateSaver(Robot.SaveParameters.LinkTransformation): 135 | # Run constraint checks at DOF resolution: 136 | for t, q in checks: 137 | # Set the joint positions 138 | # Note: the planner is using a cloned 'robot' object 139 | robot.SetActiveDOFValues(q) 140 | 141 | # Check collision (throws an exception on collision) 142 | robot_checker.VerifyCollisionFree() 143 | 144 | SetTrajectoryTags(traj, { 145 | Tags.SMOOTH: True, 146 | Tags.DETERMINISTIC_TRAJECTORY: True, 147 | Tags.DETERMINISTIC_ENDPOINT: True, 148 | }, append=True) 149 | 150 | return traj 151 | -------------------------------------------------------------------------------- /src/prpy/perception/apriltags.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2015, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Jennifer King 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import logging 32 | from base import PerceptionModule, PerceptionMethod 33 | 34 | logger = logging.getLogger(__name__) 35 | logger.setLevel(logging.INFO) 36 | 37 | class ApriltagsModule(PerceptionModule): 38 | 39 | def __init__(self, marker_topic, marker_data_path, kinbody_path, 40 | detection_frame, destination_frame,reference_link,): 41 | """ 42 | This initializes an April Tags detector. 43 | 44 | @param marker_topic The ROS topic to read markers from. Typically the output topic for April Tags 45 | @param marker_data_path The json file where the association between tag and object is stored 46 | @param kinbody_path The path to the folder where kinbodies are stored 47 | @param detection_frame The TF frame of the camera 48 | @param destination_frame The desired world TF frame 49 | """ 50 | 51 | super(ApriltagsModule, self).__init__() 52 | 53 | self.marker_topic = marker_topic 54 | self.marker_data_path = marker_data_path 55 | self.kinbody_path = kinbody_path 56 | self.detection_frame = detection_frame 57 | self.destination_frame = destination_frame 58 | self.reference_link = reference_link 59 | 60 | 61 | def __str__(self): 62 | return self.__class__.__name__ 63 | 64 | 65 | def _DetectObjects(self, env, marker_topic=None, marker_data_path=None, 66 | kinbody_path=None, detection_frame=None, 67 | destination_frame=None, reference_link=None,**kw_args): 68 | """ 69 | Use the apriltags service to detect objects and add them to the 70 | environment. Params are as in __init__. 71 | 72 | @param env: The current OpenRAVE environment 73 | @param marker_topic The ROS topic to read markers from. Typically the output topic for April Tags 74 | @param marker_data_path The json file where the association between tag and object is stored 75 | @param kinbody_path The path to the folder where kinbodies are stored 76 | @param detection_frame The TF frame of the camera 77 | @param destination_frame The desired world TF frame 78 | 79 | @return The list of kinbodies associated with the detected apriltags 80 | """ 81 | try: 82 | # Allow caller to override any of the initial parameters 83 | # loaded into the module 84 | if marker_topic is None: 85 | marker_topic = self.marker_topic 86 | 87 | if marker_data_path is None: 88 | marker_data_path = self.marker_data_path 89 | 90 | if kinbody_path is None: 91 | kinbody_path = self.kinbody_path 92 | 93 | if detection_frame is None: 94 | detection_frame = self.detection_frame 95 | 96 | if destination_frame is None: 97 | destination_frame = self.destination_frame 98 | 99 | if reference_link is None: 100 | reference_link = self.reference_link 101 | 102 | # TODO: Creating detector is not instant...might want 103 | # to just do this once in the constructor 104 | import kinbody_detector.kinbody_detector as kd 105 | detector = kd.KinBodyDetector(env, 106 | marker_data_path, 107 | kinbody_path, 108 | marker_topic, 109 | detection_frame, 110 | destination_frame, 111 | reference_link) 112 | 113 | logger.warn('Waiting to detect objects...') 114 | return detector.Update(**kw_args) 115 | except Exception, e: 116 | logger.error('Detecton failed update: %s' % str(e)) 117 | raise 118 | 119 | @PerceptionMethod 120 | def DetectObjects(self, robot, **kw_args): 121 | """ 122 | Overriden method for detection_frame 123 | """ 124 | 125 | added_kinbodies, updated_kinbodies = self._DetectObjects(robot.GetEnv(), **kw_args) 126 | return added_kinbodies + updated_kinbodies 127 | 128 | @PerceptionMethod 129 | def DetectObject(self, robot, object_name, **kw_args): 130 | """ 131 | Detects a single named object. 132 | """ 133 | added_bodies, updated_bodies = self._DetectObjects(robot.GetEnv(), **kw_args) 134 | 135 | return_obj = None 136 | for obj in added_bodies: 137 | if obj.GetName() != object_name: 138 | env = robot.GetEnv() 139 | env.Remove(obj) 140 | else: 141 | return_obj = obj 142 | for obj in updated_bodies: 143 | if obj.GetName() == object_name: 144 | return_obj = obj 145 | # TODO: Otherwise we need to put it back 146 | 147 | if return_obj is not None: 148 | return return_obj 149 | 150 | from prpy.perception.base import PerceptionException 151 | raise PerceptionException('Failed to detect object %s', object_name) 152 | -------------------------------------------------------------------------------- /src/prpy/perception/rock_module.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2015, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Jennifer King 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | import logging 31 | from base import PerceptionModule, PerceptionMethod 32 | 33 | logger = logging.getLogger(__name__) 34 | 35 | class RockModule(PerceptionModule): 36 | 37 | def __init__(self, kinbody_path, detection_frame, destination_frame): 38 | """ 39 | Initialize the rock module 40 | 41 | @param kinbody_path The path to the directory that has kinbodies of objects 42 | @param detection_frame The TF frame of the camera 43 | @param destination_frame The TF frame of the world 44 | """ 45 | super(RockModule, self).__init__() 46 | self.kinbody_path = kinbody_path 47 | self.detection_frame = detection_frame 48 | self.destination_frame = destination_frame 49 | 50 | def __str__(self): 51 | return self.__class__.__name__ 52 | 53 | def _GetDetector(self, env, kinbody_path, detection_frame, destination_frame): 54 | """ 55 | @param env The OpenRAVE environment 56 | @param kinbody_path The path to use to map kinbodies to poses 57 | (if None defaults to the path used to initialize the module) 58 | @param detection_frame The camera frame 59 | (if None defaults to the frame used to initialize the module) 60 | @param destination_frame The frame to place detected objects into 61 | (if None defaults to the frame used to intialize the module) 62 | """ 63 | 64 | if kinbody_path is None: 65 | kinbody_path = self.kinbody_path 66 | 67 | if detection_frame is None: 68 | detection_frame = self.detection_frame 69 | 70 | if destination_frame is None: 71 | destination_frame = self.destination_frame 72 | 73 | from rock import RockDetector 74 | rock_detector = RockDetector( 75 | env, 76 | kinbody_path, 77 | detection_frame=detection_frame, 78 | world_frame=destination_frame) 79 | return rock_detector 80 | 81 | @PerceptionMethod 82 | def DetectObject(self, robot, obj_name, 83 | kinbody_path=None, 84 | detection_frame=None, 85 | destination_frame=None): 86 | """ 87 | @param obj_name The name of the object to search for 88 | @param kinbody_path The path to use to map kinbodies to poses 89 | (if None defaults to the path used to initialize the module) 90 | @param detection_frame The camera frame 91 | (if None defaults to the frame used to initialize the module) 92 | @param destination_frame The frame to place detected objects into 93 | (if None defaults to the frame used to intialize the module) 94 | @return A list of detected kinbodies 95 | """ 96 | # Initial the detector 97 | rock_detector = self._GetDetector(robot.GetEnv(), 98 | kinbody_path, 99 | detection_frame, 100 | destination_frame) 101 | 102 | # Use the detector to find object poses 103 | poses = rock_detector.SearchTabletop(obj_name) 104 | 105 | # Use the poses to generate new kinbodies 106 | kinbodies = rock_detector.KinBodiesFromPoses(obj_names, poses) 107 | 108 | return kinbodies 109 | 110 | @PerceptionMethod 111 | def DetectObjectNearPose(self, robot, obj_name, pose, 112 | search_extents=None, 113 | kinbody_path=None, 114 | detection_frame=None, 115 | destination_frame=None): 116 | """ 117 | Searches near a given pose for any objects 118 | @param obj_name The name of the object to search for 119 | @param pose The pose to search near 120 | @param search_extents = [x,y,z]-extents of search area 121 | (in pose frame) 122 | @param kinbody_path The path to use to map kinbodies to poses 123 | (if None defaults to the path used to initialize the module) 124 | @param detection_frame The camera frame 125 | (if None defaults to the frame used to initialize the module) 126 | @param destination_frame The frame to place detected objects into 127 | (if None defaults to the frame used to intialize the module) 128 | @return A list of detected kinbodies 129 | """ 130 | if search_extents is None: 131 | search_extents=[-0.1, -0.1, 0.] 132 | 133 | # Initial the detector 134 | rock_detector = self._GetDetector(robot.GetEnv(), 135 | kinbody_path, 136 | detection_frame, 137 | destination_frame) 138 | 139 | # Use the detector to find object poses 140 | poses = rock_detector.SearchBBox( 141 | obj_name, pose, 142 | -0.5*numpy.array(search_extents), 143 | 0.5*numpy.array(search_extents), 144 | rock_detector.UprightOrientations()) 145 | 146 | # Use the poses to generate new kinbodies 147 | kinbodies = rock_detector.KinBodiesFromPoses(obj_names, poses) 148 | 149 | return kinbodies 150 | 151 | -------------------------------------------------------------------------------- /src/prpy/perception/block_detector.py: -------------------------------------------------------------------------------- 1 | import prpy,os,numpy,sys 2 | import tabletop_perception_tools.msg 3 | from tabletop_perception_tools.srv import * 4 | import rospy 5 | import tf 6 | import geometry_msgs.msg 7 | from tf.transformations import quaternion_matrix,euler_from_matrix,euler_matrix 8 | 9 | from base import PerceptionModule, PerceptionMethod 10 | 11 | import logging 12 | logger = logging.getLogger(__name__) 13 | 14 | table_z_offset = -0.02 15 | 16 | class BlockDetector(PerceptionModule): 17 | 18 | def __init__(self,point_cloud_topic,detection_frame,destination_frame): 19 | """ 20 | Initializes a Block Detector 21 | 22 | @param point_cloud_topic the name of the point-cloud to read from 23 | @param detection_frame the TF frame of the camera 24 | @param destination_frame the TF frame of block_in_world 25 | """ 26 | 27 | super(PerceptionModule, self).__init__() 28 | 29 | self.point_cloud_topic = point_cloud_topic 30 | self.detection_frame = detection_frame 31 | self.destination_frame = destination_frame 32 | self.listener = tf.TransformListener() 33 | 34 | def __str__(self): 35 | return 'BlockDetector' 36 | 37 | 38 | def find_blocks(self,service_name="tools_server/find_blocks", 39 | cloud_topic="/head/kinect2/qhd/points", 40 | segment_planes=True, 41 | num_planes=1, 42 | plane_distance=0.015, 43 | segment_depth=True, 44 | min_depth=0.5, 45 | max_depth=1.5, 46 | cluster_tolerance=0.005, 47 | min_cluster_size=50, 48 | max_cluster_size=300, 49 | segment_box=True, 50 | box_min = [-0.3, -0.05, 0.6], 51 | box_max = [0.3, 0.5, 1.5]): 52 | """ 53 | @param service_name: name of the ROS service for tabletop_perception_tools 54 | @param cloud_topic: name of the ROS topic for the colored PointCloud2 55 | @param segment_planes: discard the largest num_plane planes 56 | @param num_planes: number of planes to discard, if segment_planes is True 57 | @param plane_distance: minimum distance from largest plane for points to be accepted 58 | @param min_depth: minimum depth from the camera 59 | @param max_depth: minimum depth from the camera 60 | @param cluster_tolerance: maximum distance between any two points in a cluster 61 | @param min_cluster_size: minimum number of points in a cluster 62 | @param max_cluster_size: maximum number of points in a cluster 63 | @param segment_box: flag to discard points outside of (box_min, box_max) 64 | @param box_min: minimum coordinsates of search area in camera frame 65 | @param box_max: maximum coordinsates of search area in camera frame 66 | 67 | @return list of blocks found, if any 68 | """ 69 | 70 | logging.info("waiting for service...") 71 | rospy.wait_for_service(service_name); 72 | logging.info("Calling service...") 73 | try: 74 | box_min_pt = geometry_msgs.msg.Point(); 75 | box_min_pt.x = box_min[0]; 76 | box_min_pt.y = box_min[1]; 77 | box_min_pt.z = box_min[2]; 78 | box_max_pt = geometry_msgs.msg.Point(); 79 | box_max_pt.x = box_max[0]; 80 | box_max_pt.y = box_max[1]; 81 | box_max_pt.z = box_max[2]; 82 | 83 | service = rospy.ServiceProxy(service_name, FindBlocks); 84 | response = service(cloud_topic, segment_planes, num_planes, plane_distance, segment_depth, min_depth, max_depth, cluster_tolerance, min_cluster_size, max_cluster_size, segment_box, box_min_pt, box_max_pt); 85 | return response.blocks; 86 | except rospy.ServiceException, e: 87 | logging.error("Service call failed: %s", str(e)) 88 | return [] 89 | 90 | 91 | @PerceptionMethod 92 | def DetectBlocks(self, robot, table, blocks=None,timeout=10, **kw_args): 93 | """ 94 | Calls detector for blocks and places them on the table 95 | 96 | @param robot: The robot instance using the detector 97 | @param table: The kinbody for the table on which the blocks are placed 98 | @blocks: List of blocks currently in the environment; if present, redetection not done 99 | 100 | @return The list of blocks detected 101 | """ 102 | if blocks is not None and len(blocks) == 0: 103 | # Add all blocks 104 | 105 | env = robot.GetEnv() 106 | 107 | # Get the pr-ordata package path 108 | import prpy.util 109 | block_path = os.path.join('objects', 'block.kinbody.xml') 110 | 111 | detected_blocks = self.find_blocks(cloud_topic=self.point_cloud_topic) 112 | 113 | # Place blocks on the table 114 | from prpy.util import ComputeEnabledAABB 115 | with prpy.rave.Disabled(table, padding_only=True): 116 | table_aabb = ComputeEnabledAABB(table) 117 | z = table_aabb.pos()[2] + table_aabb.extents()[2] + table_z_offset #OFFSET SET AT TOP 118 | 119 | for b in detected_blocks: 120 | 121 | block = env.ReadKinBodyXMLFile(block_path) 122 | 123 | for link in block.GetLinks(): 124 | for geometry in link.GetGeometries(): 125 | geometry.SetDiffuseColor(numpy.array([b.avg_color.r,b.avg_color.g,b.avg_color.b,b.avg_color.a])) 126 | 127 | block_pose = numpy.array(quaternion_matrix([ 128 | b.pose.orientation.x, 129 | b.pose.orientation.y, 130 | b.pose.orientation.z, 131 | b.pose.orientation.w])) 132 | block_pose[0,3] = b.pose.position.x 133 | block_pose[1,3] = b.pose.position.y 134 | block_pose[2,3] = b.pose.position.z 135 | 136 | self.listener.waitForTransform( 137 | self.detection_frame, 138 | self.destination_frame, 139 | rospy.Time(0), 140 | rospy.Duration(timeout)) 141 | 142 | frame_trans, frame_rot = self.listener.lookupTransform( 143 | self.destination_frame, 144 | self.detection_frame, 145 | rospy.Time(0)) 146 | 147 | frame_offset = numpy.matrix(quaternion_matrix(frame_rot)) 148 | frame_offset[0,3] = frame_trans[0] 149 | frame_offset[1,3] = frame_trans[1] 150 | frame_offset[2,3] = frame_trans[2] 151 | 152 | block_in_world = numpy.array(numpy.dot(frame_offset,block_pose)) 153 | 154 | 155 | #Snap block to table 156 | 157 | block_in_world[2,3] = z 158 | 159 | #To snap blocks to upright on table 160 | 161 | block_matrix = block_in_world[0:3,0:3] 162 | ax, ay, az = euler_from_matrix(block_matrix) 163 | ax = 0 164 | ay = 0 165 | block_in_world_corrected = euler_matrix(ax,ay,az) 166 | block_in_world[0:3,0:3] = block_in_world_corrected[0:3,0:3] 167 | block.SetTransform(block_in_world) 168 | 169 | #Set block name 170 | block.SetName('block') 171 | env.Add(block,anonymous=True) 172 | blocks.append(block) 173 | 174 | return blocks 175 | -------------------------------------------------------------------------------- /src/prpy/perception/simtrack.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy 3 | import os.path 4 | 5 | from base import PerceptionModule, PerceptionMethod 6 | 7 | 8 | class SimtrackModule(PerceptionModule): 9 | 10 | def __init__(self, kinbody_path, detection_frame, world_frame, 11 | service_namespace=None): 12 | """ 13 | This initializes a simtrack detector. 14 | 15 | @param kinbody_path The path to the folder where kinbodies are stored 16 | @param detection_frame The TF frame of the camera 17 | @param world_frame The desired world TF frame 18 | @param service_namespace The namespace for the simtrack service (default: /simtrack) 19 | """ 20 | import rospy 21 | import tf 22 | import tf.transformations as transformations 23 | # Initialize a new ros node if one has not already been created 24 | try: 25 | rospy.init_node('simtrack_detector', anonymous=True) 26 | except rospy.exceptions.ROSException: 27 | pass 28 | 29 | #For getting transforms in world frame 30 | if detection_frame is not None and world_frame is not None: 31 | self.listener = tf.TransformListener() 32 | else: 33 | self.listener = None 34 | 35 | if service_namespace is None: 36 | service_namespace='/simtrack' 37 | 38 | self.detection_frame = detection_frame 39 | self.world_frame = world_frame 40 | self.service_namespace = service_namespace 41 | 42 | self.kinbody_path = kinbody_path 43 | 44 | # A map of known objects that can be queries 45 | self.kinbody_to_query_map = {'fuze_bottle': 'fuze_bottle_visual', 46 | 'pop_tarts': 'pop_tarts_visual'} 47 | self.query_to_kinbody_map = {'fuze_bottle_visual': 'fuze_bottle', 48 | 'pop_tarts_visual': 'pop_tarts'} 49 | 50 | @staticmethod 51 | def _MsgToPose(msg): 52 | """ 53 | Parse the ROS message to a 4x4 pose format 54 | @param msg The ros message containing a pose 55 | @return A 4x4 transformation matrix containing the pose 56 | as read from the message 57 | """ 58 | import tf.transformations as transformations 59 | #Get translation and rotation (from Euler angles) 60 | pose = transformations.quaternion_matrix(numpy.array([msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w])) 61 | 62 | pose[0,3] = msg.pose.position.x 63 | pose[1,3] = msg.pose.position.y 64 | pose[2,3] = msg.pose.position.z 65 | 66 | return pose 67 | 68 | def _LocalToWorld(self,pose): 69 | """ 70 | Transform a pose from local frame to world frame 71 | @param pose The 4x4 transformation matrix containing the pose to transform 72 | @return The 4x4 transformation matrix describing the pose in world frame 73 | """ 74 | import rospy 75 | #Get pose w.r.t world frame 76 | self.listener.waitForTransform(self.world_frame,self.detection_frame, 77 | rospy.Time(),rospy.Duration(10)) 78 | t, r = self.listener.lookupTransform(self.world_frame,self.detection_frame, 79 | rospy.Time(0)) 80 | 81 | #Get relative transform between frames 82 | offset_to_world = numpy.matrix(transformations.quaternion_matrix(r)) 83 | offset_to_world[0,3] = t[0] 84 | offset_to_world[1,3] = t[1] 85 | offset_to_world[2,3] = t[2] 86 | 87 | #Compose with pose to get pose in world frame 88 | result = numpy.array(numpy.dot(offset_to_world, pose)) 89 | 90 | return result 91 | 92 | 93 | def _GetDetections(self, obj_names): 94 | import simtrack_msgs.srv 95 | """ 96 | Calls the service to get a detection of a particular object. 97 | @param obj_name The name of the object to detect 98 | @return A 4x4 transformation matrix describing the pose of the object 99 | in world frame, None if the object is not detected 100 | """ 101 | #Call detection service for a particular object 102 | detect_simtrack = rospy.ServiceProxy(self.service_namespace+'/detect_objects', 103 | simtrack_msgs.srv.DetectObjects) 104 | 105 | detect_resp = detect_simtrack(obj_names, 5.0) 106 | 107 | 108 | detections = [] 109 | 110 | for i in xrange(0, len(detect_resp.detected_models)) : 111 | obj_name = detect_resp.detected_models[i]; 112 | obj_pose = detect_resp.detected_poses[i]; 113 | obj_pose_tf = self._MsgToPose(obj_pose); 114 | if (self.detection_frame is not None and self.world_frame is not None): 115 | obj_pose_tf = self._LocalToWorld(obj_pose_tf) 116 | detections.append((obj_name, obj_pose_tf)); 117 | 118 | return detections 119 | 120 | @PerceptionMethod 121 | def DetectObject(self, robot, obj_name, **kw_args): 122 | """ 123 | Detect a single object 124 | @param robot The OpenRAVE robot 125 | @param obj_name The name of the object to detect 126 | @returns A kinbody placed in the detected location 127 | @throws PerceptionException if the object is not detected 128 | """ 129 | 130 | from prpy.perception.base import PerceptionException 131 | 132 | if obj_name not in self.kinbody_to_query_map: 133 | raise PerceptionException('The simtrack module cannot detect object %s', obj_name) 134 | 135 | query_name = self.kinbody_to_query_map[obj_name] 136 | obj_poses = self._GetDetections(query_name) 137 | if len(obj_poses is 0): 138 | raise PerceptionException('Failed to detect object %s', obj_name) 139 | 140 | obj_pose = None 141 | 142 | for (name, pose) in obj_poses: 143 | if (name is query_name): 144 | obj_pose = pose 145 | break; 146 | 147 | if (obj_pose is None): 148 | raise PerceptionException('Failed to detect object %s', obj_name) 149 | 150 | env = robot.GetEnv() 151 | if env.GetKinBody(obj_name) is None: 152 | from prpy.rave import add_object 153 | kinbody_file = '%s.kinbody.xml' % obj_name 154 | new_body = add_object( 155 | env, 156 | obj_name, 157 | os.path.join(self.kinbody_path, kinbody_file)) 158 | 159 | body = env.GetKinBody(obj_name) 160 | body.SetTransform(obj_pose) 161 | return body 162 | 163 | @PerceptionMethod 164 | def DetectObjects(self, robot, **kw_args): 165 | """ 166 | Overriden method for detection_frame 167 | """ 168 | from prpy.perception.base import PerceptionException 169 | env = robot.GetEnv() 170 | # Detecting empty list will detect all possible objects 171 | detections = self._GetDetections([]) 172 | 173 | for (obj_name, obj_pose) in detections: 174 | if (obj_name not in self.query_to_kinbody_map): 175 | continue 176 | 177 | kinbody_name = self.query_to_kinbody_map[obj_name] 178 | 179 | if env.GetKinBody(kinbody_name) is None: 180 | from prpy.rave import add_object 181 | kinbody_file = '%s.kinbody.xml' % kinbody_name 182 | new_body = add_object( 183 | env, 184 | kinbody_name, 185 | os.path.join(self.kinbody_path, kinbody_file)) 186 | print kinbody_name 187 | body = env.GetKinBody(kinbody_name) 188 | body.SetTransform(obj_pose) 189 | 190 | 191 | -------------------------------------------------------------------------------- /src/prpy/base/mobilebase.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013, Carnegie Mellon University 4 | # All rights reserved. 5 | # Authors: Michael Koval 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # - Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # - Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # - Neither the name of Carnegie Mellon University nor the names of its 16 | # contributors may be used to endorse or promote products derived from this 17 | # software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import copy, functools, numpy, openravepy 32 | from .. import bind 33 | from prpy.clone import Clone 34 | 35 | def create_affine_trajectory(robot, poses): 36 | doft = openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis 37 | cspec = openravepy.RaveGetAffineConfigurationSpecification(doft, robot) 38 | traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), 'GenericTrajectory') 39 | traj.Init(cspec) 40 | 41 | for iwaypoint, pose in enumerate(poses): 42 | waypoint = openravepy.RaveGetAffineDOFValuesFromTransform(pose, doft) 43 | traj.Insert(iwaypoint, waypoint) 44 | 45 | return traj 46 | 47 | 48 | class MobileBase(object): 49 | def __init__(self, sim, robot): 50 | self.simulated = sim 51 | self.robot = robot 52 | 53 | def __dir__(self): 54 | # Add planning methods to the tab-completion list. 55 | method_names = set(self.__dict__.keys()) 56 | method_names.update(self.robot.base_planner.get_planning_method_names()) 57 | return list(method_names) 58 | 59 | def __getattr__(self, name): 60 | delegate_method = getattr(self.robot.base_planner, name) 61 | 62 | # Resolve planner calls through the robot.planner field. 63 | if self.robot.base_planner.has_planning_method(name): 64 | @functools.wraps(delegate_method) 65 | def wrapper_method(*args, **kw_args): 66 | return self._BasePlanWrapper(delegate_method, args, kw_args) 67 | 68 | return wrapper_method 69 | 70 | raise AttributeError('{0:s} is missing method "{1:s}".'.format(repr(self), name)) 71 | 72 | def CloneBindings(self, parent): 73 | MobileBase.__init__(self, True, None) 74 | 75 | def Forward(self, meters, execute=True, direction=None, **kw_args): 76 | """ 77 | Drives the robot forward the desired distance 78 | Note: Only implemented in simulation. Derived robots should implement this method. 79 | @param meters the distance to drive the robot 80 | @param direction forward direction of motion 81 | @param timout duration to wait for execution 82 | """ 83 | if direction is None: 84 | direction = numpy.array([ 1., 0., 0. ]) 85 | if abs(numpy.linalg.norm(direction) - 1.0) > 1e-3: 86 | raise ValueError('Direction must be a unit vector.') 87 | 88 | with self.robot.GetEnv(): 89 | start_pose = self.robot.GetTransform() 90 | offset_pose = numpy.eye(4) 91 | offset_pose[0:3, 3] = meters * direction 92 | goal_pose = numpy.dot(start_pose, offset_pose) 93 | 94 | path = create_affine_trajectory(self.robot, [ start_pose, goal_pose ]) 95 | if execute: 96 | return self.robot.ExecutePath(path, **kw_args) 97 | else: 98 | return path 99 | 100 | def Rotate(self, angle_rad, execute=True, **kw_args): 101 | """ 102 | Rotates the robot the desired distance 103 | @param angle_rad the number of radians to rotate 104 | @param timeout duration to wait for execution 105 | """ 106 | with self.robot.GetEnv(): 107 | start_pose = self.robot.GetTransform() 108 | 109 | relative_pose = openravepy.matrixFromAxisAngle([ 0., 0., angle_rad ]) 110 | goal_pose = numpy.dot(start_pose, relative_pose) 111 | 112 | path = create_affine_trajectory(self.robot, [ start_pose, goal_pose ]) 113 | if execute: 114 | return self.robot.ExecutePath(path, **kw_args) 115 | else: 116 | return path 117 | 118 | def DriveStraightUntilForce(self, direction, velocity=0.1, force_threshold=3.0, 119 | max_distance=None, timeout=None, left_arm=True, right_arm=True): 120 | """ 121 | Drive the base in a direction until a force/torque sensor feels a force. The 122 | base first turns to face the desired direction, then drives forward at the 123 | specified velocity. The action terminates when max_distance is reached, the 124 | timeout is exceeded, or if a force is felt. The maximum distance and timeout 125 | can be disabled by setting the corresponding parameters to None. 126 | @param direction forward direction of motion in the world frame 127 | @param velocity desired forward velocity 128 | @param force_threshold threshold force in Newtons 129 | @param max_distance maximum distance in meters 130 | @param timeout maximum duration in seconds 131 | @param left_arm flag to use the left force/torque sensor 132 | @param right_arm flag to use the right force/torque sensor 133 | @return felt_force flag indicating whether the action felt a force 134 | """ 135 | if self.simulated: 136 | raise NotImplementedError('DriveStraightUntilForce does not work in simulation') 137 | else: 138 | raise NotImplementedError('DriveStraightUntilForce is not implemented') 139 | 140 | def _BasePlanWrapper(self, planning_method, args, kw_args): 141 | 142 | robot = self.robot 143 | with Clone(robot.GetEnv()) as cloned_env: 144 | cloned_robot = cloned_env.Cloned(robot) 145 | cloned_robot.SetActiveDOFs( 146 | [], 147 | affine=(openravepy.DOFAffine.X | 148 | openravepy.DOFAffine.Y | 149 | openravepy.DOFAffine.RotationAxis) 150 | ) 151 | cloned_traj = planning_method(cloned_robot, *args, **kw_args) 152 | 153 | config_spec = cloned_robot.GetActiveConfigurationSpecification() 154 | openravepy.planningutils.ConvertTrajectorySpecification( 155 | cloned_traj, config_spec 156 | ) 157 | 158 | # Copy the trajectory back to the original environment. 159 | from ..util import CopyTrajectory 160 | traj = CopyTrajectory(cloned_traj, env=robot.GetEnv()) 161 | 162 | # Optionally execute the trajectory. 163 | if 'execute' not in kw_args or kw_args['execute']: 164 | return self.robot.ExecutePath(traj, **kw_args) 165 | else: 166 | return traj 167 | 168 | -------------------------------------------------------------------------------- /tests/planning/test_CHOMP.py: -------------------------------------------------------------------------------- 1 | from methods.PlanToConfiguration import ( 2 | PlanToConfigurationCompleteTest, 3 | PlanToConfigurationStraightLineTest, 4 | PlanToConfigurationTest, 5 | PlanToConfigurationTestCollisionTest, 6 | ) 7 | from prpy.planning.chomp import CHOMPPlanner 8 | from planning_helpers import BasePlannerTest 9 | from unittest import TestCase 10 | 11 | 12 | class CHOMPModuleMock(object): 13 | def __init__(self, env): 14 | self.env = env 15 | self.sequence = 0 16 | self.computedistancefield_args = [] 17 | self.removefield_args = [] 18 | 19 | def GetEnv(self): 20 | return self.env 21 | 22 | def computedistancefield(self, kinbody=None, cube_extent=None, 23 | aabb_padding=None, cache_filename=None, 24 | releasegil=False, **kw_args): 25 | self.computedistancefield_args.append({ 26 | '__sequence__': self.sequence, 27 | 'kinbody': kinbody, 28 | 'cache_filename': cache_filename, 29 | }) 30 | self.sequence += 1 31 | 32 | def removefield(self, kinbody=None, releasegil=False): 33 | self.removefield_args.append({ 34 | '__sequence__': self.sequence, 35 | 'kinbody': kinbody, 36 | }) 37 | self.sequence += 1 38 | 39 | 40 | class DistanceFieldManagerTest(TestCase): 41 | def setUp(self): 42 | from prpy.planning.chomp import DistanceFieldManager 43 | from openravepy import Environment 44 | 45 | self.env = Environment() 46 | self.env.Load('data/wamtest2.env.xml') 47 | self.robot = self.env.GetRobot('BarrettWAM') 48 | self.body = self.env.GetKinBody('mug-table') 49 | self.bodies = set(self.env.GetBodies()) 50 | 51 | self.module = CHOMPModuleMock(self.env) 52 | self.manager = DistanceFieldManager(self.module) 53 | 54 | def test_GetGeometricState_ChangeEnabledStatusChangesState(self): 55 | with self.env: 56 | link = self.robot.GetLinks()[0] 57 | 58 | link.Enable(True) 59 | state_before = self.manager.get_geometric_state(self.robot) 60 | 61 | link.Enable(False) 62 | state_after = self.manager.get_geometric_state(self.robot) 63 | 64 | self.assertNotEquals(state_after, state_before) 65 | 66 | def test_GetGeometricState_ChangeInDOFValuesChangesState(self): 67 | with self.env: 68 | lower_limits, upper_limits = self.robot.GetDOFLimits() 69 | 70 | self.robot.SetDOFValues(lower_limits) 71 | state_before = self.manager.get_geometric_state(self.robot) 72 | 73 | self.robot.SetDOFValues(upper_limits) 74 | state_after = self.manager.get_geometric_state(self.robot) 75 | 76 | self.assertNotEquals(state_after, state_before) 77 | 78 | def test_GetGeometricState_ChangeInKinematicsGeometryHashChangesState(self): 79 | self.robot.GetKinematicsGeometryHash = lambda: 'mock_before' 80 | state_before = self.manager.get_geometric_state(self.robot) 81 | 82 | self.robot.GetKinematicsGeometryHash = lambda: 'mock_after' 83 | state_after = self.manager.get_geometric_state(self.robot) 84 | 85 | self.assertNotEquals(state_after, state_before) 86 | 87 | def test_GetCachePath_DifferentStatesProduceDifferentPaths(self): 88 | self.robot.GetKinematicsGeometryHash = lambda: 'mock_before' 89 | state_before = self.manager.get_geometric_state(self.robot) 90 | path_before = self.manager.get_cache_path(state_before) 91 | 92 | self.robot.GetKinematicsGeometryHash = lambda: 'mock_after' 93 | state_after = self.manager.get_geometric_state(self.robot) 94 | path_after = self.manager.get_cache_path(state_after) 95 | 96 | self.assertNotEquals(path_after, path_before) 97 | 98 | def test_Sync_InitiallyCreatesAllDistanceFields(self): 99 | self.manager.sync(self.robot) 100 | 101 | computed_bodies = [ args['kinbody'] for args in self.module.computedistancefield_args ] 102 | self.assertItemsEqual(self.bodies, computed_bodies) 103 | self.assertEqual(len(self.module.removefield_args), 0) 104 | 105 | def test_Sync_SyncTwiceDoesNothing(self): 106 | self.manager.sync(self.robot) 107 | del self.module.computedistancefield_args[:] 108 | del self.module.removefield_args[:] 109 | 110 | self.manager.sync(self.robot) 111 | 112 | self.assertEqual(len(self.module.computedistancefield_args), 0) 113 | self.assertEqual(len(self.module.removefield_args), 0) 114 | 115 | def test_Sync_IgnoresActiveRobotLinks(self): 116 | self.is_processed = False 117 | 118 | def computedistancefield(kinbody=None, cube_extent=None, 119 | aabb_padding=None, cache_filename=None, 120 | releasegil=False, **kw_args): 121 | if kinbody.GetName() == self.robot.GetName(): 122 | self.assertTrue(self.robot.GetLink('segway').IsEnabled()) 123 | self.assertTrue(self.robot.GetLink('wam0').IsEnabled()) 124 | self.assertTrue(self.robot.GetLink('wam1').IsEnabled()) 125 | self.assertTrue(self.robot.GetLink('wam2').IsEnabled()) 126 | self.assertTrue(self.robot.GetLink('wam3').IsEnabled()) 127 | self.assertFalse(self.robot.GetLink('wam4').IsEnabled()) 128 | self.assertFalse(self.robot.GetLink('wam5').IsEnabled()) 129 | self.assertFalse(self.robot.GetLink('wam6').IsEnabled()) 130 | self.assertFalse(self.robot.GetLink('wam7').IsEnabled()) 131 | self.assertFalse(self.robot.GetLink('handbase').IsEnabled()) 132 | self.assertFalse(self.robot.GetLink('Finger0-0').IsEnabled()) 133 | self.assertFalse(self.robot.GetLink('Finger0-1').IsEnabled()) 134 | self.assertFalse(self.robot.GetLink('Finger0-2').IsEnabled()) 135 | self.assertFalse(self.robot.GetLink('Finger1-0').IsEnabled()) 136 | self.assertFalse(self.robot.GetLink('Finger1-1').IsEnabled()) 137 | self.assertFalse(self.robot.GetLink('Finger1-2').IsEnabled()) 138 | self.assertFalse(self.robot.GetLink('Finger2-1').IsEnabled()) 139 | self.assertFalse(self.robot.GetLink('Finger2-2').IsEnabled()) 140 | self.is_processed = True 141 | 142 | dof_index = self.robot.GetJoint('Elbow').GetDOFIndex() 143 | self.robot.SetActiveDOFs([ dof_index ]) 144 | self.robot.Enable(True) 145 | 146 | self.module.computedistancefield = computedistancefield 147 | self.manager.sync(self.robot) 148 | 149 | self.assertTrue(self.is_processed) 150 | 151 | def test_Sync_RecomputesDistanceFieldIfStateChanges(self): 152 | self.manager.sync(self.robot) 153 | del self.module.computedistancefield_args[:] 154 | del self.module.removefield_args[:] 155 | 156 | # Change the geometry to invalidate the key. 157 | link = self.robot.GetLink('segway') 158 | link.SetGroupGeometries('test', []) 159 | link.SetGeometriesFromGroup('test') 160 | 161 | self.manager.sync(self.robot) 162 | 163 | self.assertEqual(len(self.module.computedistancefield_args), 1) 164 | self.assertEqual(self.module.computedistancefield_args[0]['kinbody'], self.robot) 165 | 166 | self.assertEqual(len(self.module.removefield_args), 1) 167 | self.assertEqual(self.module.removefield_args[0]['kinbody'], self.robot) 168 | self.assertLess(self.module.removefield_args[0]['__sequence__'], 169 | self.module.computedistancefield_args[0]['__sequence__']) 170 | 171 | 172 | class CHOMPPlannerTest(BasePlannerTest, 173 | PlanToConfigurationTest, 174 | PlanToConfigurationTestCollisionTest, 175 | PlanToConfigurationStraightLineTest, 176 | TestCase): 177 | planner_factory = CHOMPPlanner 178 | 179 | def setUp(self): 180 | super(CHOMPPlannerTest, self).setUp() 181 | --------------------------------------------------------------------------------