The main goal was to extend MoveIt's functionallity for path planning with task space path constraints. Specifically, extend the current OMPL interface with OMPL's constrained planning capabilities.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import numpy as np | |
from scipy.spatial.transform import Rotation | |
from acrobotics.path.util import create_grid | |
from pyquaternion import Quaternion | |
def rotvec_to_mat(v): | |
return Rotation.from_rotvec(v).as_dcm() | |
def mat_to_rotvec(mat): |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
$ roslaunch moveit_setup_assistant setup_assistant.launch | |
... logging to /home/jeroen/.ros/log/2750490c-02e9-11eb-ae7b-f834415a491a/roslaunch-jeroen-5580-17241.log | |
Checking log directory for disk usage. This may take a while. | |
Press Ctrl-C to interrupt | |
Done checking log file disk usage. Usage is <1GB. | |
started roslaunch server http://jeroen-5580:44185/ | |
SUMMARY | |
======== |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// debug code for MoveIt ompl interface ConstrainedPlanningStateSpace::getValueAddressAtIndex method | |
// (jeroendm) left in the debug code, because it is not unlikely that issues will arise here when adding new things in | |
// the future. Use dynamic casting to make our debugging live easier. | |
// -------------------------------------------------------------------------- | |
// auto casted_state = dynamic_cast<ompl_interface::ConstrainedPlanningStateSpace::StateType*>(ompl_state); | |
// if (casted_state) | |
// { | |
// return casted_state->values + index; | |
// } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
In file included from /home/jeroen/ros/moveit_ws/src/moveit/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp:37: | |
/home/jeroen/ros/moveit_ws/src/moveit/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h:64:8: warning: 'ompl_interface::StateValidityChecker::isValid' hides overloaded virtual function [-Woverloaded-virtual] | |
bool isValid(const ompl::base::State* state, bool verbose) const; | |
^ | |
/home/jeroen/ros/moveit_ws/devel/include/ompl-1.5/ompl/base/StateValidityChecker.h:128:26: note: hidden overloaded virtual function 'ompl::base::StateValidityChecker::isValid' declared here: different number of parameters (4 vs 2) | |
virtual bool isValid(const State *state, double &dist, State *validState, bool &validStateAvailable) const | |
^ | |
In file included from /home/jeroen/ros/moveit_ws/src/moveit/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp:37: | |
/home/jeroen/ros/moveit_ws/src/moveit/ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
$ valgrind --leak-check=full ./devel/lib/moveit_planners_ompl/test_state_validity_checker | |
==8746== Memcheck, a memory error detector | |
==8746== Copyright (C) 2002-2017, and GNU GPL'd, by Julian Seward et al. | |
==8746== Using Valgrind-3.13.0 and LibVEX; rerun with -h for copyright info | |
==8746== Command: ./devel/lib/moveit_planners_ompl/test_state_validity_checker | |
==8746== | |
[==========] Running 3 tests from 1 test case. | |
[----------] Global test environment set-up. | |
[----------] 3 tests from PandaValidityCheckerTests | |
[ RUN ] PandaValidityCheckerTests.createStateValidityChecker |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
~rosrun moveit_ros_planning moveit_kinematics_speed_and_validity_evaluator manipulator | |
ros.moveit_core.robot_model: Loading robot model 'kuka_kr5_arc'... | |
ros.moveit_core.robot_model: No root/virtual joint specified in SRDF. Assuming fixed joint | |
ros.moveit_opw_kinematics_plugin.opw: MoveItOPWKinematicsPlugin initializing | |
ros.moveit_opw_kinematics_plugin.opw: Dimension planning group 'manipulator': 6. Active Joints Models: 6. Mimic Joint Models: 0 | |
ros.moveit_opw_kinematics_plugin: Getting kinematic parameters from parameter server. | |
ros.moveit_opw_kinematics_plugin: Loaded parameters for ik solver: | |
Distances: [0.18 -0.12 0 0.4 0.6 0.62 0.115] | |
Offsets = [0 -1.5708 0 0 0 0 ] | |
Sign_corrections = [-1 1 1 -1 1 -1 ] |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Invocation failed without any known error condition, printing all lines to debug known error detection: | |
1 'Reading package lists...' | |
2 'Building dependency tree...' | |
3 'Reading state information...' | |
4 'Starting pkgProblemResolver with broken count: 5' | |
5 'Starting 2 pkgProblemResolver with broken count: 5' | |
6 'Investigating (0) ros-melodic-catkin:amd64 < none -> 0.7.23-1bionic.20200303.045725 @un puN Ib >' | |
7 'Broken ros-melodic-catkin:amd64 Depends on python-catkin-pkg:amd64 < none | 0.4.19-100 @un uH > (> 0.4.3)' | |
8 ' Considering python-catkin-pkg:amd64 1 as a solution to ros-melodic-catkin:amd64 10033' | |
9 ' Re-Instated tzdata:amd64' |
jeroendm@laptop-jeroen:~/ros/ws_descartes$ catkin build
--------------------------------------------------------------------
Profile: default
Extending: [env] /opt/ros/kinetic
Workspace: /home/jeroendm/ros/ws_descartes
--------------------------------------------------------------------
Source Space: [exists] /home/jeroendm/ros/ws_descartes/src
Log Space: [missing] /home/jeroendm/ros/ws_descartes/logs
Build Space: [exists] /home/jeroendm/ros/ws_descartes/build
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
~catkin build | |
--------------------------------------------------------------- | |
Profile: default | |
Extending: [explicit] /opt/ros/kinetic | |
Workspace: /home/jeroen/ros/catkin_ws | |
--------------------------------------------------------------- | |
Source Space: [exists] /home/jeroen/ros/catkin_ws/src | |
Log Space: [missing] /home/jeroen/ros/catkin_ws/logs | |
Build Space: [exists] /home/jeroen/ros/catkin_ws/build | |
Devel Space: [exists] /home/jeroen/ros/catkin_ws/devel |
NewerOlder