moveit / moveit_core

THIS REPO HAS MOVED TO https://github.com/ros-planning/moveit

The MoveIt Motion Planning Framework for ROS. For the ROS 2 repository see MoveIt 2. For the commercially supported version see MoveIt Pro.

Easy-to-use open source robotics manipulation platform for developing commercial applications, prototyping designs, and benchmarking algorithms.

• Overview of MoveIt
• Installation Instructions
• Documentation
• Get Involved

Branches Policy

• We develop latest features on master.
• The *-devel branches correspond to released and stable versions of MoveIt for specific distributions of ROS. noetic-devel is synced to master currently.
• Bug fixes occasionally get backported to these released versions of MoveIt.
• To facilitate compile-time switching, the patch version of MOVEIT_VERSION of a development branch will be incremented by 1 w.r.t. the package.xml's version number.

MoveIt Status

Continuous Integration

service	Melodic	Master
GitHub
CodeCov
build farm

Licenses

ROS Buildfarm

MoveIt Package	Melodic Source	Melodic Debian	Noetic Source	Noetic Debian
moveit
moveit_core
moveit_kinematics
moveit_planners
moveit_planners_ompl
moveit_planners_chomp
chomp_motion_planner
moveit_chomp_optimizer_adapter
pilz_industrial_motion_planner
pilz_industrial_motion_planner_testutils
moveit_plugins
moveit_fake_controller_manager
moveit_simple_controller_manager
moveit_ros_control_interface
moveit_ros
moveit_ros_planning
moveit_ros_move_group
moveit_ros_planning_interface
moveit_ros_benchmarks
moveit_ros_perception
moveit_ros_occupancy_map_monitor
moveit_ros_manipulation
moveit_ros_robot_interaction
moveit_ros_visualization
moveit_ros_warehouse
moveit_servo
moveit_runtime
moveit_commander
moveit_setup_assistant
moveit_msgs
geometric_shapes
srdfdom
moveit_visual_tools
moveit_tutorials

Stargazers over time

moveit_core's Issues

Kinematics Tool GUI - Reduce Orientation Ambiguity

It took me about 5 minutes of trial and error to realize that the orientation R,P, Y are set in degrees and not radians. At the very least, a note should be made to express the units here.

Ideally, there would be a selection box to allow a user to input orientation in their preferred method: degrees, radians, or quaternion.

rework CollisionRequest API

Ideally split into CollisionRequest and DistanceRequest.

For distance, also return closest points pair;
in the distance request, perhaps a max distance of interest would be useful.

attn @acornacorn

workspace parameter does not influences planning for rotational joints

Currently the workspace parameter only influences planning for planar & floating joints. So, the workspace definition does not affect the position of links that are moved by other types of joints (like in the case you display). Unfortunately, this is a missing feature.

https://groups.google.com/forum/?hl=ja#!topic/moveit-users/_11FJ4hVEJg

updating ros-groovy-moveit* does not pull new versions of ros-groovy-geometric-shapes

Pointed at the main ros repos I just ran the command 'sudo apt-get update ros-groovy-moveit*' and pulled the following packages:

Setting up ros-groovy-moveit-msgs (0.4.0-0precise-20130523-1152-+0000) ...
Setting up ros-groovy-moveit-core (0.4.0-0precise-20130523-1156-+0000) ...
Setting up ros-groovy-moveit-ros-perception (0.4.0-1precise-20130523-1223-+0000) ...
Setting up ros-groovy-moveit-ros-planning (0.4.0-1precise-20130523-1233-+0000) ...
Setting up ros-groovy-moveit-ros-move-group (0.4.0-1precise-20130523-1247-+0000) ...
Setting up ros-groovy-moveit-ros-manipulation (0.4.0-1precise-20130523-1258-+0000) ...
Setting up ros-groovy-moveit-ros-warehouse (0.4.0-1precise-20130523-1249-+0000) ...
Setting up ros-groovy-moveit-ros-planning-interface (0.4.0-1precise-20130523-1305-+0000) ...
Setting up ros-groovy-moveit-commander (0.4.0-0precise-20130523-1313-+0000) ...
Setting up ros-groovy-moveit-planners-ompl (0.3.11-0precise-20130523-1247-+0000) ...
Setting up ros-groovy-moveit-planners (0.3.11-0precise-20130523-1256-+0000) ...
Setting up ros-groovy-moveit-ros-benchmarks (0.4.0-1precise-20130523-1258-+0000) ...
Setting up ros-groovy-moveit-ros-robot-interaction (0.4.0-1precise-20130523-1249-+0000) ...
Setting up ros-groovy-moveit-ros-visualization (0.4.0-1precise-20130523-1312-+0000) ...
Setting up ros-groovy-moveit-ros-benchmarks-gui (0.4.0-1precise-20130523-1321-+0000) ...
Setting up ros-groovy-moveit-ros (0.4.0-1precise-20130523-1325-+0000) ...
Setting up ros-groovy-moveit-setup-assistant (0.3.9-0precise-20130523-1320-+0000) ...
Setting up ros-groovy-moveit-full (0.4.0-0precise-20130523-1327-+0000) ...
Setting up ros-groovy-moveit-pr2 (0.4.0-0precise-20130523-1317-+0000) ...
Setting up ros-groovy-moveit-full-pr2 (0.4.0-0precise-20130523-1329-+0000) ...
Setting up ros-groovy-moveit-ompl-planners-core (0.3.11-0precise-20130523-1256-+0000) ...
Setting up ros-groovy-moveit-ompl-planners-ros-plugin (0.3.11-0precise-20130523-1255-+0000) ...
Setting up ros-groovy-moveit-resources (0.3.1-0precise-20130520-2230-+0000) ...

I then independently needed to run 'sudo apt-get update ros-groovy-geometric-shapes' to get geometric_shapes -
Setting up ros-groovy-geometric-shapes (0.3.4-0precise-20130523-1122-+0000) ...

This is definitely an ongoing problem - I spent a while earlier this week debugging linking errors due to having an out-of-date geometric_shapes. I suspect dependencies withing the debs are not being set correctly.

add projections onto constraint manifolds to joint_state_group for the constraints in kinematic_constraints

use ids in AllowedCollisionMatrix as message

Possible to allow cancelling IK exploration in Kinematics Tool GUI?

The IK tool can take a long time to run. It could be convenient to add a cancellation button to the GUI to stop solving for new points in case the user made a mistake setting up the parameters (despite the Visualize Points button).

This way, the user wouldn't have to close and restart the GUI to restart.

Better control of collision object cache

As demonstrated to Ioan, updating a planning scene via planning scene diffs received over the wire can cause memory issues with large octomaps. This is due to the cache size and how often it decided to clear the cache of obsolete object references.

Suggest either (a) the ability to manually clear the cache, or (b) automatically clearing the cache when an octomap is removed (and replaced by a new one).

Distance to collision normal vector

When checking for collision using Collision Result, the distance to nearest collision is provided as information. However, the normal vector along that distance is not available. This would be valuable information in detecting collisions prior to their occurence.

Unwanted transform in setIK function of joint_state_group

Hi everyone, I'm encountering a little issue with the setIK() function provided by joint_state_group.
I have a ikfast based plugin implementing the kinematics_base interface. The base_frame for the plugin is the "/utorso" frame. I also have goal pose represented by a geometry_msgs/Pose already given in the "/utorso" frame. If I call setIK() on my joint group with this goal pose, I would expect that the the pose is forwarded to getPositionIk unmodified. Instead, the solver's base_frame is compared against the joint group parent's model frame. and a transformation is applied to the pose (see https://github.com/ros-planning/moveit_core/blob/groovy-devel/robot_state/src/joint_state_group.cpp#L354).
This results in a wrong pose being used to query IK in my case. Why aren't the solver base_frame and the goal pose frame_id compared against each other?
If I transform my pose to the "/pelvis" frame before and use this transformed pose as the goal for setIK(), the IK query seems to work correctly (but uses a tf query that should not be needed really).
Also, my ikfast based plugins seem to work fine for interactive marker based control with the demo.launch that gets generated by the setup assistant, suggesting that their frame setup is correct.

Cannot create Chain with branches

I'm trying to setup a robot composed of a chain where some of the links have more than one child (e.g, an arm with a camera attached to a link). It seems that moveit cannot create a Chain from it, I'm getting the error "Group is not a chain".

Hard coded database path

There is a WARN from rosconsole_bridge being generated when launch demo.launch that says:

 WARN ros.rosconsole_bridge.console_bridge: Manifest not found in folder '/home/isucan/constraints_approximation_database'. Not loading constraint approximations.

I did a grep through all of MoveIt! as well as my robot's repos and I found two location's with isucan hard-coded in. After removing them, this error still did not go away (the locations didn't seem like they would effect the output, and they didn't). But I'm not sure where the output is coming from.

The whole launch file output:
https://gist.github.com/davetcoleman/5905731

Help with compiling on Travis

I've started using Travis with my Baxter code and I'd like to have it able to compile MoveIt from source. I've tried may times but internal compiler errors always occur - from my research it seems gcc is perhaps running out of memory on the free Travis boxes. But I'm not sure. Any advice is welcome:

https://travis-ci.org/davetcoleman/moveit_core/builds/12080425

@jbohren you are the expert who got me into this, any tips? :)

Installation Failure on openSUSE 12.3

I'm trying to follow the instructions on http://moveit.ros.org/wiki/Installation to install MoveIt on a laptop running openSUSE 12.3. Because rosdep does not work on my system and there isn't a fix for it yet, I've been handling dependencies manually. Generally that hsa worked well, but I have hit a hard stop with the Flexible Collision Library (FCL). It seems that the latest edition of the library is no longer a ROS package but a standalone library, and I have it installed as such. However, when I run the catkin_make in my moveit directory according to those instructions, I get the following error when it tried to make moveit_core:

CMake Error at /opt/ros/groovy/share/catkin/cmake/catkinConfig.cmake:72 (find_package):
Could not find a package configuration file provided by "fcl" with any of
the following names:

fclConfig.cmake
fcl-config.cmake

Add the installation prefix of "fcl" to CMAKE_PREFIX_PATH or set "fcl_DIR"
to a directory containing one of the above files. If "fcl" provides a
separate development package or SDK, be sure it has been installed.
Call Stack (most recent call first):
moveit_core/CMakeLists.txt:10 (find_package)

That it is looking for package configuration files suggests to me that Moveit may still be configured to treat it as a package, and I don't know how to re-configure it to look for the shared object library instead. Is this indeed the issue that I think it is, and is there a fix? Thanks very much.

return points and normal of closest distance from collision detection

As far as I know, I can get the distance between world object and any part of pr2 link through moveit API of collision detection. And I am really interested in the coordinates of pair of points that makes that shortest distance happen. However, it seems moveit API is not able to return that information. Is it possible to do that?

Problem getting robot joint state

Every time I try to recover the joint states of my simulated arm using the method getVariableValues() I always get the same joint state position: 0.0, although the joints are moved.

I have loaded the model of the robot and populated the RobotState structure. I have also created an instance of MoveGroup object to be able to move it but it doesn't work.

The code that I'm executing is:

/* Load the robot model */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");

/* Get a shared pointer to the model */
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();

/* WORKING WITH THE KINEMATIC STATE /
/ Create a kinematic state - this represents the configuration for the robot represented by kinematic_model */
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));

/* Get the configuration for the joints robotnik arm_/
robot_state::JointStateGroup_ joint_state_group = kinematic_state->getJointStateGroup("robotnik_arm");

/* Get the joint states for the right arm*/
std::vector joint_values;
joint_state_group->getVariableValues(joint_values);

Thanks

set_joint_value_target() and plan() should check joint limits set in URDF

In MoveIt 0.4.0, it seems that the functions set_joint_value_target() and plan() allow one to specify target joint values outside the limits set in the URDF. The robot doesn't actually move beyond the limits but there is no warning that an attempt has been made to go outside the limits. Perhaps an attempt to exceed joint limits should return a "failure" or "warning" flag.

IK including virtual joint

Now, virtual joint can not be used in IK.
Then, virtual linear and rotational joints automatically could be added to KDL joint group.

Support multiple collision geometries for URDF models (e.g. Atlas model) in MoveIt!

This is related to robot_model ticket 18 which can serve as an overview on the issue:
ros/robot_model#18

It would be great to have compatibility for this in MoveIt! so we don't have to work on giving Atlas a single collision geom per link again.

Problems getting joint_states

Every time I try to recover the joint states of my simulated arm using the method getVariableValues() I always get the same joint state position: 0.0, although the joints are moved.

Thanks.

Ensure consistent convention for contact normals

I'm ticketing this to ensure it has been addressed before alpha release.
Previously, FCL was reporting normals that pointed from contact1 -> contact2 for certain types (primitives, I think), and from contact2 -> contact1 for Octomap.

We need to make sure there is a standard (for all collision detectors used through the MoveIt API) for which direction a reported normal points.

Configure IK Timeout in Kinematics Tool GUI

I heard that the default for a non-solution is 5 seconds per point.

If a user is not familiar with their arm, the initial guesses for Workspace size can be defined with many non-solution points.

Allowing the user to change this value would speed preliminary searches and allow much faster searching if the user knows their arm does not contain many degrees of freedom.

kinematics_model doesn't load end effector info properly from srdf

In planning_models::KinematicModel::buildGroupInfo, there's some logic to figure out the parent groups of each end effector group. It basically says that if the parent link of the end effector group belongs to exactly 1 group, then that is the parent group of that end effector group. It then sets attached_end_effector_group_name_ in the parent group, and end_effector_parent_ in the end effector group.

The problem is that the parent link of the pr2's right gripper, for instance, actually belongs to 3 groups: "right_arm", "arms", and "whole_body". So the function doesn't know what the parent group should be, and doesn't set anything.

What is the right way to correct this?

VariableBounds is uninitialized?

I noticed some VERY slow planning in some of my newly-created moveit packages. The debug logs showed some odd timing (note the 30-sec delay):

11:05:48 Solution found in 0.085398 seconds
11:05:48 manipulator: Returning successful solution with 33 states
11:05:48 Running 'Add Time Parameterization'
11:06:22 Motion planner reported a solution path with 33 states

I tracked down a config-related issue in my setup-assistant-generated joint_limits.yaml file. The auto-created file included the following settings for each joint:

has_acceleration_limits: true
max_acceleration: 0

When I set has_acceleration_limits: false, planning worked as expected.

I think the error is not in the Setup Assistant, but in jointBoundsFromURDF in robot_state.cpp. In that function, the VariableBounds structure is declared, but never initialized. Structures are not automatically initialized when declared, so their contents are undefined. My guess is that the VariableBounds.acceleration_bounded_ flag used in the Setup Assistant is being randomly initialized to true in this function, and causing the erroneous joint_limits.yaml file.

API for getting joint velocities from a KinematicState

It would be useful to be able to retrieve joint velocities for doing fancy-pants planning. At the simplest level, the velocities could be thrown into a map<string, double>. A more complex method would be to expand the actual JointState class to have velocities. (i.e. Does a simple map allow for multi-variable joints? Do we care?)

Bonus points for a convenience function that does forward time-integration of the state, using a supplied time period and using the velocity values of the state, e.g.
kinematic_state::integrate( start_state, delta_t, distination)

constraints cannot be defined for links that are fixed to the tip

see this discussion for details:
https://groups.google.com/forum/#!topic/moveit-users/gVWEXqcqHfY

add low level data structure for repersenting RobotTrajectory

We should change the way we represent MultiDOFJoints too.

Orocos IK Segfault when using Pick >1 Grasp

Pick server works when I send it 1 grasp at a time, but if I send multiple I get an error related to Orocos IK / Eigen / IKConstraintSampler

ERROR ros.moveit_ros_manipulation: Manipulation plan failed at stage 'reachable & valid pose filter' on thread 1
DEBUG ros.moveit_ros_planning: searchPositionIK2: Position request pose is 0.250475 0.000301798 0.169484 -0.000166572 0.70527 0.002466 0.708934
move_group_action_server: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:407: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::DenseCoeffsBase<Derived, 1>::Index) [with Derived = Eigen::Matrix<double, -0x00000000000000001, 1>, Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double, Eigen::DenseCoeffsBase<Derived, 1>::Index = long int]: Assertion `index >= 0 && index < size()' failed.
move_group_action_server: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:407: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::DenseCoeffsBase<Derived, 1>::Index) [with Derived = Eigen::Matrix<double, -0x00000000000000001, 1>, Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double, Eigen::DenseCoeffsBase<Derived, 1>::Index = long int]: Assertion `index >= 0 && index < size()' failed.

Program received signal SIGABRT, Aborted.
[Switching to Thread 0x7fffb57fa700 (LWP 29286)]
0x00007ffff4024425 in __GI_raise (sig=<optimized out>) at ../nptl/sysdeps/unix/sysv/linux/raise.c:64
64  ../nptl/sysdeps/unix/sysv/linux/raise.c: No such file or directory.

The backtrace:

(gdb) bt
#0  0x00007ffff4024425 in __GI_raise (sig=<optimized out>) at ../nptl/sysdeps/unix/sysv/linux/raise.c:64
#1  0x00007ffff4027b8b in __GI_abort () at abort.c:91
#2  0x00007ffff401d0ee in __assert_fail_base (fmt=<optimized out>, 
    assertion=0x7ffff2c89ff1 "index >= 0 && index < size()", 
    file=0x7ffff2c89650 "/usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h", line=<optimized out>, 
    function=<optimized out>) at assert.c:94
#3  0x00007ffff401d192 in __GI___assert_fail (assertion=0x7ffff2c89ff1 "index >= 0 && index < size()", 
    file=0x7ffff2c89650 "/usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h", line=407, 
    function=0x7ffff2c98840 "Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::DenseCoeffsBase<Derived, 1>::Index) [with Derived = Eigen::Matrix<double, -0x", '0' <repeats 16 times>, "1, 1>, Ei"...) at assert.c:103
#4  0x00007ffff2c5fcfe in KDL::JntArray::operator()(unsigned int, unsigned int) ()
   from /opt/ros/groovy/lib/liborocos-kdl.so.1.1
#5  0x00007ffff2c83d28 in KDL::SVD_HH::calculate(KDL::Jacobian const&, std::vector<KDL::JntArray, std::allocator<KDL::JntArray> >&, KDL::JntArray&, std::vector<KDL::JntArray, std::allocator<KDL::JntArray> >&, int) ()
   from /opt/ros/groovy/lib/liborocos-kdl.so.1.1
#6  0x00007ffff2c50c0d in KDL::ChainIkSolverVel_pinv::CartToJnt(KDL::JntArray const&, KDL::Twist const&, KDL::JntArray&) () from /opt/ros/groovy/lib/liborocos-kdl.so.1.1
#7  0x00007ffff2c7df59 in KDL::ChainIkSolverPos_NR_JL::CartToJnt(KDL::JntArray const&, KDL::Frame const&, KDL::JntArray&) () from /opt/ros/groovy/lib/liborocos-kdl.so.1.1
#8  0x00007fffcbdf51de in kdl_kinematics_plugin::KDLKinematicsPlugin::searchPositionIK(geometry_msgs::Pose_<std::allocator<void> > const&, std::vector<double, std::allocator<double> > const&, double, std::vector<double, std::allocator<double> >&, boost::function<void (geometry_msgs::Pose_<std::allocator<void> > const&, std::vector<double, std::allocator<double> > const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&)> const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&, std::vector<double, std::allocator<double> > const&) const ()
   from /home/dave/ros/moveit/devel/lib//libmoveit_kdl_kinematics_plugin.so
#9  0x00007fffcbdf5cae in kdl_kinematics_plugin::KDLKinematicsPlugin::searchPositionIK(geometry_msgs::Pose_<std::allocator<void> > const&, std::vector<double, std::allocator<double> > const&, double, std::vector<double, std::allocator<double> >&, boost::function<void (geometry_msgs::Pose_<std::allocator<void> > const&, std::vector<double, std::allocator<double> > const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&)> const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&) const () from /home/dave/ros/moveit/devel/lib//libmoveit_kdl_kinematics_plugin.so
#10 0x00007ffff09cb7d7 in constraint_samplers::IKConstraintSampler::callIK(geometry_msgs::Pose_<std::allocator<void> > const&, boost::function<void (geometry_msgs::Pose_<std::allocator<void> > const&, std::vector<double, std::allocator<double> > const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&)> const&, double, robot_state::JointStateGroup*, bool) () from /home/dave/ros/moveit/devel/lib/libmoveit_constraint_samplers.so
#11 0x00007ffff09cf12e in constraint_samplers::IKConstraintSampler::sampleHelper(robot_state::JointStateGroup*, robot_---Type <return> to continue, or q <return> to quit---
state::RobotState const&, unsigned int, bool) ()
   from /home/dave/ros/moveit/devel/lib/libmoveit_constraint_samplers.so
#12 0x00007ffff507a6e9 in pick_place::ReachableAndValidPoseFilter::evaluate(boost::shared_ptr<pick_place::ManipulationPlan> const&) const () from /home/dave/ros/moveit/devel/lib/libmoveit_pick_place_planner.so
#13 0x00007ffff50729be in pick_place::ManipulationPipeline::processingThread(unsigned int) ()
   from /home/dave/ros/moveit/devel/lib/libmoveit_pick_place_planner.so
#14 0x00007ffff6a72ce9 in thread_proxy () from /usr/lib/libboost_thread.so.1.46.1
#15 0x00007ffff6850e9a in start_thread (arg=0x7fffb57fa700) at pthread_create.c:308
#16 0x00007ffff40e1cbd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:112
#17 0x0000000000000000 in ?? ()

Segmentation faults in RobotState and in JointModelGroup

Hey,
i'm getting segfaults when objects are destroyed. I'm using ros groovy installed via .debs. I compiled the moveit libs with debug flags to get a proper bracktrace. The used sources are the same version as the libs from the installed packages. And i'm getting segfaults using the packaged versions as well as using the ones i compiled from source.

Program received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0xa74ffb40 (LWP 15423)]
0xb7bd3305 in robot_state::RobotState::clearAttachedBodies (this=0xcac4038) at <omitted>/groovy_workspace/src/moveit_core-0.4.7/robot_state/src/robot_state.cpp:478
478           attached_body_update_callback_(it->second, false);

#0  0xb7bd3305 in robot_state::RobotState::clearAttachedBodies (this=0xcac4038) at <omitted>/groovy_workspace/src/moveit_core-0.4.7/robot_state/src/robot_state.cpp:478
#1  0xb7bd19da in robot_state::RobotState::~RobotState (this=0xcac4038, __in_chrg=<optimized out>) at <omitted>/groovy_workspace/src/moveit_core-0.4.7/robot_state/src/robot_state.cpp:141
#2  0x0822e177 in boost::checked_delete<robot_state::RobotState> (x=0xcac4038) at /usr/include/boost/checked_delete.hpp:34
#3  0x08242eea in boost::detail::sp_counted_impl_p<robot_state::RobotState>::dispose (this=0xa8d08eb8) at /usr/include/boost/smart_ptr/detail/sp_counted_impl.hpp:78
#4  0x081fa9d4 in boost::detail::sp_counted_base::release (this=0xa8d08eb8) at /usr/include/boost/smart_ptr/detail/sp_counted_base_gcc_x86.hpp:145
#5  0x081faa46 in boost::detail::shared_count::~shared_count (this=0xa74fe950, __in_chrg=<optimized out>) at /usr/include/boost/smart_ptr/detail/shared_count.hpp:217
#6  0x081ff286 in boost::shared_ptr<robot_state::RobotState>::~shared_ptr (this=0xa74fe94c, __in_chrg=<optimized out>) at /usr/include/boost/smart_ptr/shared_ptr.hpp:168
#7  0x08225818 in ClosedFormSolver::~ClosedFormSolver (this=0xa74fe940, __in_chrg=<optimized out>) at <omitted>/groovy_workspace/src/ros_hollie_full_body_ik/src/ClosedFormSolver.h:20
#8  0x08221593 in InverseKinematics::calcInitialGuess (this=0xbfffed4c, ws_goal=...) at <omitted>/groovy_workspace/src/ros_hollie_full_body_ik/src/InverseKinematics.cpp:218
#9  0x081f8c12 in ProgramController::processFeedback (this=0xbfffed34, feedback=...) at <omitted>/groovy_workspace/src/ros_hollie_full_body_ik/src/ProgramController.cpp:53
#10 0x08214659 in boost::_mfi::mf1<void, ProgramController, boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> const&>::operator() (this=0xc314e60, p=0xbfffed34, a1=...)
    at /usr/include/boost/bind/mem_fn_template.hpp:165
#11 0x08213370 in boost::_bi::list2<boost::_bi::value<ProgramController*>, boost::arg<1> >::operator()<boost::_mfi::mf1<void, ProgramController, boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> const&>, boost::_bi::list1<boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> const&> > (this=0xc314e68, f=..., a=...) at /usr/include/boost/bind/bind.hpp:313
#12 0x08211e57 in boost::_bi::bind_t<void, boost::_mfi::mf1<void, ProgramController, boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<ProgramController*>, boost::arg<1> > >::operator()<boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> > (this=0xc314e60, a1=...) at /usr/include/boost/bind/bind_template.hpp:47
#13 0x08210211 in boost::detail::function::void_function_obj_invoker1<boost::_bi::bind_t<void, boost::_mfi::mf1<void, ProgramController, boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<ProgramController*>, boost::arg<1> > >, void, boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> const&>::invoke (function_obj_ptr=..., a0=...)
    at /usr/include/boost/function/function_template.hpp:153
#14 0xb7586c61 in interactive_markers::InteractiveMarkerServer::processFeedback(boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> const&) () from /opt/ros/groovy/lib/libinteractive_markers.so
#15 0xb75870e9 in boost::detail::function::void_function_obj_invoker1<boost::_bi::bind_t<void, boost::_mfi::mf1<void, interactive_markers::InteractiveMarkerServer, boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<interactive_markers::InteractiveMarkerServer*>, boost::arg<1> > >, void, boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> const&>::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> const&) () from /opt/ros/groovy/lib/libinteractive_markers.so
#16 0xb758e076 in boost::detail::function::void_function_obj_invoker1<boost::function<void (boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> const&)>, void, boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> >::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const>) ()
   from /opt/ros/groovy/lib/libinteractive_markers.so
#17 0xb759f3e3 in ros::SubscriptionCallbackHelperT<boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> const&, void>::call(ros::SubscriptionCallbackHelperCallParams&) ()
   from /opt/ros/groovy/lib/libinteractive_markers.so
#18 0xb7f8aba7 in ros::SubscriptionQueue::call() () from /opt/ros/groovy/lib/libroscpp.so
#19 0xb7f3a7e7 in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) () from /opt/ros/groovy/lib/libroscpp.so
#20 0xb7f3c2f4 in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/groovy/lib/libroscpp.so
#21 0xb7581fe7 in interactive_markers::InteractiveMarkerServer::spinThread() () from /opt/ros/groovy/lib/libinteractive_markers.so
#22 0xb758727b in boost::detail::thread_data<boost::_bi::bind_t<void, boost::_mfi::mf0<void, interactive_markers::InteractiveMarkerServer>, boost::_bi::list1<boost::_bi::value<interactive_markers::InteractiveMarkerServer*> > > >::run() ()
   from /opt/ros/groovy/lib/libinteractive_markers.so
#23 0xb726f48c in thread_proxy () from /usr/lib/libboost_thread.so.1.46.1
#24 0xb7e4fd4c in start_thread (arg=0xa74ffb40) at pthread_create.c:308
#25 0xb736bdde in clone () at ../sysdeps/unix/sysv/linux/i386/clone.S:130

Program received signal SIGSEGV, Segmentation fault.
0xb688b040 in boost::checked_delete<kinematics::KinematicsBase> (x=0x83abc48) at /usr/include/boost/checked_delete.hpp:34
34          delete x;

#0  0xb688b040 in boost::checked_delete<kinematics::KinematicsBase> (x=0x83abc48) at /usr/include/boost/checked_delete.hpp:34
#1  0xb6894e8e in boost::detail::sp_counted_impl_p<kinematics::KinematicsBase>::dispose (this=0x8cbcae0) at /usr/include/boost/smart_ptr/detail/sp_counted_impl.hpp:78
#2  0x081fa9d4 in boost::detail::sp_counted_base::release (this=0x8cbcae0) at /usr/include/boost/smart_ptr/detail/sp_counted_base_gcc_x86.hpp:145
#3  0x081faa46 in boost::detail::shared_count::~shared_count (this=0x83cf524, __in_chrg=<optimized out>) at /usr/include/boost/smart_ptr/detail/shared_count.hpp:217
#4  0xb7d6f036 in boost::shared_ptr<kinematics::KinematicsBase const>::~shared_ptr (this=0x83cf520, __in_chrg=<optimized out>) at /usr/include/boost/smart_ptr/shared_ptr.hpp:168
#5  0xb7d6d269 in robot_model::JointModelGroup::~JointModelGroup (this=0x83cf370, __in_chrg=<optimized out>) at <omitted>/groovy_workspace/src/moveit_core-0.4.7/robot_model/src/joint_model_group.cpp:214
#6  0xb7d7adba in robot_model::RobotModel::~RobotModel (this=0x83d0880, __in_chrg=<optimized out>) at <omitted>/groovy_workspace/src/moveit_core-0.4.7/robot_model/src/robot_model.cpp:60
#7  0xb7d7b1d7 in robot_model::RobotModel::~RobotModel (this=0x83d0880, __in_chrg=<optimized out>) at <omitted>/groovy_workspace/src/moveit_core-0.4.7/robot_model/src/robot_model.cpp:65
#8  0x08231f77 in boost::checked_delete<robot_model::RobotModel> (x=0x83d0880) at /usr/include/boost/checked_delete.hpp:34
#9  0x08242e8a in boost::detail::sp_counted_impl_p<robot_model::RobotModel>::dispose (this=0x90610f8) at /usr/include/boost/smart_ptr/detail/sp_counted_impl.hpp:78
#10 0x081fa9d4 in boost::detail::sp_counted_base::release (this=0x90610f8) at /usr/include/boost/smart_ptr/detail/sp_counted_base_gcc_x86.hpp:145
#11 0x081faa46 in boost::detail::shared_count::~shared_count (this=0xbfffedcc, __in_chrg=<optimized out>) at /usr/include/boost/smart_ptr/detail/shared_count.hpp:217
#12 0x081ff270 in boost::shared_ptr<robot_model::RobotModel const>::~shared_ptr (this=0xbfffedc8, __in_chrg=<optimized out>) at /usr/include/boost/smart_ptr/shared_ptr.hpp:168
#13 0x081ff50f in InverseKinematics::~InverseKinematics (this=0xbfffed4c, __in_chrg=<optimized out>) at <omitted>/groovy_workspace/src/ros_hollie_full_body_ik/src/InverseKinematics.h:26
#14 0x081ff871 in ProgramController::~ProgramController (this=0xbfffed34, __in_chrg=<optimized out>) at <omitted>/groovy_workspace/src/ros_hollie_full_body_ik/src/ProgramController.h:23
#15 0x081fa435 in main (argc=1, argv=0xbffff2a4) at <omitted>/groovy_workspace/src/ros_hollie_full_body_ik/src/ProgramController.cpp:185

generalize computeCartesianPath

in JointStateGroup, we need to change the API as follows:
instead of a direction of motion and a desired distance to be traveled, we need to take a target Pose (position + rotation).
To convert previous calls, rotation will be identity, position will be 0 + vector * distance

Then, once the target pose is specified, and a number of steps to split the distance from current state to that target pose, the intermediate poses are generated using interpolation in SE3, and IK si called as needed.

weight limit computation in kinematic_metrics

The weight limit appears to be computed incorrectly. A value that looks a lot like uninitialized values or division by 0 appears on screen instead.

sampling base poses

it seems that sampling base positions for goal regions never succeeds, although it seems to me it should.

Having trouble building against ros-groovy with catkin

Hi, I'm trying to build the latest moveit stack from source and have been having some difficulty. Basically I can build moveit_core fine, and then when I try to build moveit_ompl_planners_core it fails because it can't find the headers that should be exported by moveit_core. I feel like I have a decent understand of how catkin is set up but it seems like something is missing from the CMakeLists.txt to export the include dirs for other packages to use, as described in http://www.ros.org/wiki/catkin/CMakeLists.txt#catkin_package.28.29

But I'm not sure if that's relevant here since there are many packages

Should I be trying to build using rosmake/rosbuild instead? I tried that too and that didn't work because some of the packages didn't have Makefiles so I figured I should learn the catkin system but I'm not sure what the right way is.

I have tried adding
INCLUDE_DIRS 66 kinematics_base/include 67 kinematic_model/include 68 kinematic_state/include 69 collision_detection/include 70 collision_detection_fcl/include 71 kinematic_constraints/include 72 planning_scene/include 73 constraint_samplers/include 74 planning_interface/include 75 planning_request_adapter/include 76 trajectory_processing/include 77 distance_field/include 78 collision_distance_field/include 79 kinematics_metrics/include 80 dynamics_solver/include 81)

To the catkin_package call in CMakeLists.txt but then it seems like those includes that depend on srdfdom/includes cause errors when compiling moveit_ompl_planners_core.

Sorry if this is a lot of noise, if I'm completely off base with what I'm doing right now I guess the most helpful thing for me would be to know how those who are actively developing moveit are compiling it. I am trying to write a planner interface for moveit for some work we are doing at Berkeley and it seemed like I should be able to compile moveit first but maybe that's not necessary either.

getMinDistanceToBounds() does not work for PLANAR and FLOATING joints

Automatically creating fixed frame when not defined in srdf

Add differential IK to joint_state_group

probably something along the lines of bool setFromDiffIK(const Twist &twist, const double dT, const diffIKCallback &callback) where the diffIKCallback returns a vector of joint velocities that are to be applied in the nullspace. Then add a set of default callbacks: e.g. move towards reference joint position in nullspace or move away from joint limits, etc.

add handles to planning_interface base class

getLinkState() from robot_state issue

In the Rviz plugin, using the interactive markers, the end effector of Baxter is being rendered incorrectly with its links in the wrong position. I have also ran into this issue in some of my custom code. I have tracked it down to the getRobotMarkers() function in robot_state.cpp so I am positing this issue in moveit_core. Perhaps the issue is in the function getLinkState() - what is a link's state when no tf or joint_states are being published?

The two green fingers in the bottom left are the issue - they work normal in most other situations as pictured on the right.

Thanks!

rework KinematicState

make it efficient;

This will happen only after groovy release.

Adding closest feasible state

Here is the scenario: when using an interactive marker on a less than 6 dof arm, I'd like the closest valid state to be displayed. Right now, the IR is useless on a less than 6 dof as every state is invalid. I understand adding more joint (like the torso) would solve that problem.

I'm just wondering whether that feature is possible/useful, maybe through KDL. Your call.

IK parameter for weighting joints

I need a weight parameter for each joint.
Because joint near the root link such as joints in torso moved too much,
and that is not good for a biped humanoid robot.

Joint weights should be set at kinematics.yaml or PositionIKRequest.msg

IK perf for unreachable poses

The IK seems quite fast but slows down a lot when requested to calculate an unreachable pose.

Idea: a certain set of poses could be precalculated as impossible. For example for a chain with only revolute joints any end position farther than the sum of the link lengths is impossible. Would it make sense to store such info (e.g. a sphere radius per joint group) so that some requests can be trivially rejected? Such calculations could be done per group by the SetupAssistant and stored in the srdf.

Configure FK Computation in Kinematics Tool GUI in terms of Time or Samples

I loaded a simple 3 dof arm into the tool and allowed it to compute FK for around 1 second. This generated 600,000 points that immediately locked up my computer when RVIZ attempted to visualize that number.

It should be possible to define the stopping point of the computation in terms of either overall 'Allowed Time for Computation' or 'Maximum Output Samples' (default around 1000?). This should help control an unexpectedly taxing output for users with simpler arms.

split collision_detection

We need to make the collision checker and the world representation separate
attn: @acornacorn

kinematic_state has ~40 Mb of meshes checked into the test directory

The meshes should not be in the source repo.

make iterative time parameterization tool use KinematicTrajectory

transforms in moveit

transforms need to be separated out from robot_state
Transforms needs to probably exist as a base class that offers the current functionality;
Other classes inherit from Transforms and use the frames implicitly defined by a planning scene and a robot state. only base pointers to Transforms get passed around, but the planning scene instantiates the most derived class.

move getMinDistanceToBounds() to robot_state

[trajectory processing] glitches in time parameterisation

Quite often motion plans executed on my robot look like getting stuck at some points in time. The robot slows down so much, that it looks like it has aborted the execution.

Looking further into this, I got the impression that the time parameterisation is not working correctly.

Here is one example trajectory output from move_group:

Waypoint 0
  Name                 : waypoint_0
  Angles               : [   0.81  -0.00   0.21  -1.49   1.22 ]
  Nominal Rates        : [   0.01   0.02   0.11   0.08   0.06 ]

Waypoint 1
  Name                 : waypoint_1
  Angles               : [   0.82   0.02   0.31  -1.42   1.27 ]
  Nominal Rates        : [   0.01   0.02   0.11   0.08   0.06 ]

Waypoint 2
  Name                 : waypoint_2
  Angles               : [   0.83   0.03   0.41  -1.34   1.32 ]
  Nominal Rates        : [   0.01   0.02   0.11   0.08   0.06 ]

Waypoint 3
  Name                 : waypoint_3
  Angles               : [   0.84   0.05   0.51  -1.27   1.37 ]
  Nominal Rates        : [   0.11   0.02   0.14   0.07   0.01 ]

Waypoint 4
  Name                 : waypoint_4
  Angles               : [   0.84   0.07   0.61  -1.20   1.42 ]
  Nominal Rates        : [   0.24   0.02   0.16   0.05   0.08 ]

Waypoint 5
  Name                 : waypoint_5
  Angles               : [   0.79   0.08   0.65  -1.18   1.40 ]
  Nominal Rates        : [   0.24   0.02   0.16   0.05   0.08 ]

While for most way points the nominal rates fit the positions, there are some entries, which look odd to me.

For example way point 3: I don't think the nominal rates are correct for entry (joint) 1 and 5.

With the subsequent interpolator adjusting the trajectory to fit this input, the result is the extreme slowing down of the motion explained above.

Any idea what could cause this?

allow specifying passive joints in srdf