Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

CHOMP planner and CollisionDistanceField #155

Merged
merged 21 commits into from Sep 22, 2016

Conversation

Projects
None yet
3 participants
@ksatyaki
Copy link
Contributor

commented Aug 25, 2016

Pull request to add chomp and collision_distance_field from ksatyaki/moveit_planners and ksatyaki/collision_distance_field.

Brief list of changes made (mostly by @jrgnicho and myself)

  • collision_distance_field has been catkinized and ported to the latest moveit API.
  • Constructor Version 1 in CollisionRobotDistanceField has been fixed.
  • CHOMP motion planner and the moveit interface have been updated to work with the latest moveit API.
  • Compiles fine. Planning with chomp reports "dirty link transforms". Use chomp_test to test chomp.
@jrgnicho

This comment has been minimized.

Copy link
Contributor

commented Aug 25, 2016

The "dirty links transform" warning indicates that the pose of the link hasn't been updated accordingly. Calling the Link::update() method should resolve this.

@ksatyaki

This comment has been minimized.

Copy link
Contributor Author

commented Aug 25, 2016

@jrgnicho I don't know what exactly is causing the segfault. Backtrace reports that it is happening in RevoluteJointModel::isContinuous() method. It's funny.

Update

I have now fixed the segfault. But now I get
"Variable 'link_6-tool0' is not known to model 'test_kr210'"

Hence, planning fails.

Also, where exactly is the RobotState::update() method supposed to be called?

size_x_ = other.size_x_;
size_y_ = other.size_y_;
size_z_ = other.size_z_;
ROS_INFO_STREAM("Version 4");

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

is this necessary/relevant?

//ROS_WARN_STREAM("No current dfce");
if(!distance_field_cache_entry_)
{
ROS_DEBUG_STREAM("No current dfce");

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

remove acronym in favor of full name


<maintainer email="robot.moveit@gmail.com">Sachin Chitta</maintainer>
<maintainer email="isucan@gmail.com">Ioan Sucan</maintainer>
<maintainer email="acorn.pooley@sri.com">Acorn Pooley</maintainer>

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

I know Acorn and Ioan are def. not maintainers of this package anymore

<package>
<name>moveit_experimental</name>
<version>0.6.15</version>
<description>Core libraries used by MoveIt!</description>

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

Bad description

@@ -0,0 +1,90 @@
<package>
<name>moveit_experimental</name>
<version>0.6.15</version>

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

in indigo should be 0.7.2 to match rest of this repo

<build_depend>moveit_core</build_depend>

<test_depend>angles</test_depend>
<test_depend>tf_conversions</test_depend>

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

test_depends should be grouped together (see above) and preferrably after the run_depend

<export>
<moveit_core
plugin="${prefix}/collision_distance_field/collision_detector_hybrid_description.xml" />
</export>

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

better indentation formatting please

@@ -0,0 +1,34 @@
cmake_minimum_required(VERSION 2.8.3 FATAL_ERROR)

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

FATAL_ERROR is unusual to me - why have it here?

/* Author: E. Gil Jones */

#ifndef _CHOMP_INTERFACE_ROS_H_
#define _CHOMP_INTERFACE_ROS_H_

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

should be CHOMP_INTERFACE_CHOMP_INTERFACE_H


namespace chomp_interface
{
/** @class CHOMPInterfaceROS */

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

bad comment

*/

#ifndef CHOMP_PLANNING_CONTEXT_H_
#define CHOMP_PLANNING_CONTEXT_H_

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

rename guard per style guidelines

* chomp_planning_context.h
*
* Created on: 27-Jul-2016
* Author: ace

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

who is ace? this should have a license, hopefully BSDv3

This comment has been minimized.

Copy link
@ksatyaki

ksatyaki Aug 26, 2016

Author Contributor

Sorry for these errors. Correcting them now. I was very bent on getting CHOMP to work that I really didn't "clean out" the packages I imported.
Who should I attribute the copyright to? Myself or ?

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

They should be attributed to ace, but if you put a lot of refactoring work into it I think you can add your name also


class ChompPlanningContext: public planning_interface::PlanningContext {

typedef boost::shared_ptr <ChompPlanningContext> Ptr;

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

Ptr is very ambiguous... it is usually ChompPlanningContextPtr

This comment has been minimized.

Copy link
@ksatyaki

ksatyaki Aug 26, 2016

Author Contributor

I used the convention from ROS messages. Usually for every message type you have two Ptr typedefs.

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

but this is not a ROS message, rather a class. see "MoveIt-Specific" under style guidelines

void initialize();

private:
boost::shared_ptr<CHOMPInterface> chomp_interface_;

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

should use typedef, above

@@ -0,0 +1,51 @@
/*

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

why didn't this file previously exist?

This comment has been minimized.

Copy link
@ksatyaki

ksatyaki Aug 26, 2016

Author Contributor

Because CHOMP used the very old and now deprecated Planner interface from moveit.


private:
boost::shared_ptr<CHOMPInterface> chomp_interface_;
moveit::core::RobotModelConstPtr kmodel_;

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

moveit has since renamed this var to robot_model_ instead of kmodel_ as a standard, could you improve this?

<name>chomp_interface</name>
<description>The chomp_interface package</description>

<version>0.7.0</version>

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

version needs to match moveit_core

loadParams();
}

void CHOMPInterface::loadParams() {

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

new line for bracket

@@ -0,0 +1,78 @@
/*********************************************************************

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

i see there is still a chomp_interface_ros.cpp but I think those files should have been deleted when this file, and others, were created. isn't this duplicate code?

This comment has been minimized.

Copy link
@ksatyaki

ksatyaki Aug 26, 2016

Author Contributor

Yes. It's bad merging. I shall delete them.

@@ -0,0 +1,88 @@
/*
* chomp_planning_context.cpp
*

This comment has been minimized.

Copy link
@davetcoleman

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Sep 12, 2016

Member

ping - can you add a license that matches the other files?

* Author: ace
*/

#include "chomp_interface/chomp_planning_context.h"

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

brackets instead of quotes

INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

remove comments for cleaner CMake

<name>chomp_motion_planner</name>
<description>chomp_motion_planner</description>

<version>0.7.0</version>

This comment has been minimized.

Copy link
@davetcoleman

<version>0.7.0</version>
<author email="gjones@willowgarage.com">Gil Jones</author>
<maintainer email="isucan@willowgarage.com">Ioan Sucan</maintainer>

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 26, 2016

Member

need different maintainer

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Aug 26, 2016

@ksatyaki I realize most my comments don't apply to code you wrote, but to a previous author. Still, I think it would be nice to cleanup the code base a bit. I'd even advice running clang-format in moveit_experimental and chomp to auto fix all style mistakes

I'm glad to see you mention a test robot. But can we change your test to use one from moveit_resources?

Do you mind documenting how to use STOMP? It can be a short overview in a new page on moveit_tutorials just based on your personal notes. This will allow me to test your changes at runtime rather than just reviewing this PR.

Is there a way to easily see a visualization of the body sphere markers? I see it mentioned throughout the code

Eventually we want to move the distance field out of experimental, but we'll first want to have multiple users testing and using this code before it can "graduate".

@ksatyaki

This comment has been minimized.

Copy link
Contributor Author

commented Aug 26, 2016

@ksatyaki I realize most my comments don't apply to code you wrote, but to a previous author. Still, I think it would be nice to cleanup the code base a bit. I'd even advice running clang-format in moveit_experimental and chomp to auto fix all style mistakes

I have pushed changes after running clang-format.

I'm glad to see you mention a test robot. But can we change your test to use one from moveit_resources?

Sorry, I am not aware of this test robot... Maybe it was someone before me. I will try to learn more about this.

Do you mind documenting how to use STOMP? It can be a short overview in a new page on moveit_tutorials just based on your personal notes. This will allow me to test your changes at runtime rather than just reviewing this PR.

You mean CHOMP? I can create a page for this.

Is there a way to easily see a visualization of the body sphere markers? I see it mentioned throughout the code.

I can check this out. I am actually relatively new to moveit.

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Aug 26, 2016

Thanks for the quick response!

For the test robot, use: https://github.com/ros-planning/moveit_resources. If the two included robots aren't enough, you can add a new robot to that repo

CHOMP, yes :)

I can check this out. I am actually relatively new to moveit.

Don't worry about it then, thanks for the effort you're putting into this

@jrgnicho I'm hoping you can help me test this PR

@ksatyaki

This comment has been minimized.

Copy link
Contributor Author

commented Aug 29, 2016

I get the error,

[ERROR] [1472464558.330767910]: Variable 'joint_6-tool0' is not known to model 'fanuc'
Exception thrown.
[ERROR] [1472464558.331403186]: Exception caught: 'Variable 'joint_6-tool0' is not known to model 'fanuc''

When using the fanuc test robot from the moveit_resources package.

I am predicting that this is possibly because it is adding a fixed joint to the trajectory. This could be due to the API differences. One of the things I noticed was that the old didn't have the types RevoluteJointModel, PrismaticJointModel, etc.

Update

Ok, I had to change all the getJointModels() to getActiveJointModels() because in the OLD API getJointModels() returned only the active joint models. I think I'm pretty close to getting CHOMP to work.

@ksatyaki

This comment has been minimized.

Copy link
Contributor Author

commented Sep 12, 2016

Now the build is passing. :)

@jrgnicho

This comment has been minimized.

Copy link
Contributor

commented Sep 12, 2016

@ksatyaki thank you. @davetcoleman I'm OK with merging this PR.

@ksatyaki

This comment has been minimized.

Copy link
Contributor Author

commented Sep 12, 2016

Thanks!

On 12 September 2016 at 16:22, Jorge Nicho notifications@github.com wrote:

@ksatyaki https://github.com/ksatyaki thank you. @davetcoleman
https://github.com/davetcoleman I'm OK with merging this PR.


You are receiving this because you were mentioned.
Reply to this email directly, view it on GitHub
#155 (comment),
or mute the thread
https://github.com/notifications/unsubscribe-auth/AGuXcAe7gUXxHJinA5YsDRCBM9oIBQbiks5qpWAkgaJpZM4Js5U5
.

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Sep 12, 2016

I'm not a fan of simply disabling the tests - do you think you could instead look into fixing the tests so that future maintenance of this code is easier? Or will you have time in the future?

@ksatyaki

This comment has been minimized.

Copy link
Contributor Author

commented Sep 12, 2016

@ksatyaki

This comment has been minimized.

Copy link
Contributor Author

commented Sep 13, 2016

The tests are failing, is that normal or is something missing?

kstate.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
kstate.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
kstate.updateStateWithLinkAt("base_link", Eigen::Affine3d::Identity());
kstate.updateStateWithLinkAt("base_bellow_link", offset);

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Sep 13, 2016

Member

bad indentation

kstate.updateStateWithLinkAt("base_bellow_link", offset);

kstate.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Affine3d::Identity());
kstate.updateStateWithLinkAt("l_gripper_palm_link", offset);

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Sep 13, 2016

Member

indentation

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Sep 13, 2016

The moveit-indigo tests currently all pass, except for changes in this PR. Looking at the Travis logs the following test fails:

DistanceFieldCollisionDetectionTester.AttachedBodyTester

*** Error in `/root/ws_moveit/devel/.private/moveit_experimental/lib/moveit_experimental/test_collision_distance_field': free(): invalid pointer: 0x00007fff0856c130 ***

Aborted (core dumped)

Cannot find results, writing failure results to '/root/ws_moveit/build/moveit_experimental/test_results/moveit_experimental/MISSING-gtest-test_collision_distance_field.xml'

I would run these tests locally on you computer (see Running Tests Locally) and see if you can fix the invalid pointer

@ksatyaki

This comment has been minimized.

Copy link
Contributor Author

commented Sep 21, 2016

How do I run these tests within gdb? The segfault is super hard to track otherwise.

One of the 6 tests fails. It's the AttachedBodyTester that fails. I don't know if I can help fix this though. :(

Let me know if you can...

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Sep 21, 2016

@ksatyaki if you tried but couldn't get the test to pass, you can just disable it for now and we move to get this PR in so it isn't further delayed. I know this is a big PR for someone else's code and I appreciate your effort. We just need Travis to pass now.

@ksatyaki

This comment has been minimized.

Copy link
Contributor Author

commented Sep 21, 2016

Okay, it was only one assertion that failed. So I just commented that one out. All other tests pass. So I have committed that. Thanks.

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Sep 21, 2016

Great! When I merge I will squash this into one commit, unless you want to do that yourself. Afterwards can you also open PRs for the Jade and Kinetic branches, since I'm suspecting there will need to be small fixes there also?

@davetcoleman davetcoleman merged commit 32315dd into ros-planning:indigo-devel Sep 22, 2016

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details

@davetcoleman davetcoleman referenced this pull request Sep 22, 2016

Closed

CHOMP planner documentation #237

2 of 2 tasks complete

davetcoleman added a commit to davetcoleman/moveit that referenced this pull request Sep 25, 2016

Fix CHOMP planner and CollisionDistanceField (ros-planning#155)
* Copy collision_distance_field package
* Resurrect chomp
* remove some old Makefiles and manifests
* Correct various errors
* Code formatting, author, description, version, etc
* Add definitions for c++11. Nested templates problem.
* Add name to planner plugin.
* Change getJointModels to getActiveJointModels.
* Call robot_state::RobotState::update in setRobotStateFromPoint.
* Create README.md
* Improve package.xml, CMake config and other changes suggested by jrgnicho.
* Remove some commented code, add scaling factors to computeTimeStampes
* Add install targets in moveit_experimental and chomp
* Add install target for headers in chomp pkgs.
* Remove unnecessary debugging ROS_INFO.
* Port collision_distance_field test to indigo.
* Remove one assertion that makes collision_distance_field test to fail.

davetcoleman added a commit to davetcoleman/moveit that referenced this pull request Sep 25, 2016

Fix CHOMP planner and CollisionDistanceField (ros-planning#155)
* Copy collision_distance_field package
* Resurrect chomp
* remove some old Makefiles and manifests
* Correct various errors
* Code formatting, author, description, version, etc
* Add definitions for c++11. Nested templates problem.
* Add name to planner plugin.
* Change getJointModels to getActiveJointModels.
* Call robot_state::RobotState::update in setRobotStateFromPoint.
* Create README.md
* Improve package.xml, CMake config and other changes suggested by jrgnicho.
* Remove some commented code, add scaling factors to computeTimeStampes
* Add install targets in moveit_experimental and chomp
* Add install target for headers in chomp pkgs.
* Remove unnecessary debugging ROS_INFO.
* Port collision_distance_field test to indigo.
* Remove one assertion that makes collision_distance_field test to fail.
@davetcoleman

This comment has been minimized.

Copy link
Member

commented Sep 25, 2016

I've just opened the PRs for J/K

davetcoleman added a commit that referenced this pull request Sep 25, 2016

Fix CHOMP planner and CollisionDistanceField (#155) (#248)
* Copy collision_distance_field package
* Resurrect chomp
* remove some old Makefiles and manifests
* Correct various errors
* Code formatting, author, description, version, etc
* Add definitions for c++11. Nested templates problem.
* Add name to planner plugin.
* Change getJointModels to getActiveJointModels.
* Call robot_state::RobotState::update in setRobotStateFromPoint.
* Create README.md
* Improve package.xml, CMake config and other changes suggested by jrgnicho.
* Remove some commented code, add scaling factors to computeTimeStampes
* Add install targets in moveit_experimental and chomp
* Add install target for headers in chomp pkgs.
* Remove unnecessary debugging ROS_INFO.
* Port collision_distance_field test to indigo.
* Remove one assertion that makes collision_distance_field test to fail.

davetcoleman added a commit that referenced this pull request Sep 26, 2016

Fix CHOMP planner and CollisionDistanceField (#155)
* Copy collision_distance_field package
* Resurrect chomp
* remove some old Makefiles and manifests
* Correct various errors
* Code formatting, author, description, version, etc
* Add definitions for c++11. Nested templates problem.
* Add name to planner plugin.
* Change getJointModels to getActiveJointModels.
* Call robot_state::RobotState::update in setRobotStateFromPoint.
* Create README.md
* Improve package.xml, CMake config and other changes suggested by jrgnicho.
* Remove some commented code, add scaling factors to computeTimeStampes
* Add install targets in moveit_experimental and chomp
* Add install target for headers in chomp pkgs.
* Remove unnecessary debugging ROS_INFO.
* Port collision_distance_field test to indigo.
* Remove one assertion that makes collision_distance_field test to fail.

davetcoleman added a commit to davetcoleman/moveit that referenced this pull request Sep 27, 2016

Fix CHOMP planner and CollisionDistanceField (ros-planning#155)
* Copy collision_distance_field package
* Resurrect chomp
* remove some old Makefiles and manifests
* Correct various errors
* Code formatting, author, description, version, etc
* Add definitions for c++11. Nested templates problem.
* Add name to planner plugin.
* Change getJointModels to getActiveJointModels.
* Call robot_state::RobotState::update in setRobotStateFromPoint.
* Create README.md
* Improve package.xml, CMake config and other changes suggested by jrgnicho.
* Remove some commented code, add scaling factors to computeTimeStampes
* Add install targets in moveit_experimental and chomp
* Add install target for headers in chomp pkgs.
* Remove unnecessary debugging ROS_INFO.
* Port collision_distance_field test to indigo.
* Remove one assertion that makes collision_distance_field test to fail.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.