Merge pull request #2920 from rhaschke/forward-ports

A few forward ports from MoveIt1

moveit_planners/chomp/chomp_interface/test/chomp_moveit_panda.test

@@ -0,0 +1,9 @@
+<launch>
+
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch">
+    <arg name="pipeline" value="chomp" />
+  </include>
+
+  <test test-name="chomp_test_panda" pkg="moveit_planners_chomp" type="chomp_moveit_test_panda" time-limit="20.0"/>
+
+</launch>

moveit_planners/chomp/chomp_interface/test/chomp_moveit_rrbot.test

@@ -0,0 +1,7 @@
+<launch>
+
+  <include file="$(find moveit_planners_chomp)/test/rrbot_move_group.launch"/>
+
+  <test test-name="chomp_test_rrbot" pkg="moveit_planners_chomp" type="chomp_moveit_test_rrbot" time-limit="80.0"/>
+
+</launch>

moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp

@@ -0,0 +1,80 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, Sherbrooke University
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/// \author Captain Yoshi
+
+#include <ros/ros.h>
+#include <gtest/gtest.h>
+
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+class CHOMPMoveitTest : public ::testing::Test
+{
+public:
+  moveit::planning_interface::MoveGroupInterface move_group_;
+  moveit::planning_interface::MoveGroupInterface::Plan my_plan_;
+
+public:
+  CHOMPMoveitTest() : move_group_("hand")
+  {
+  }
+};
+
+// TEST CASES
+
+// https://github.com/moveit/moveit/issues/2542
+TEST_F(CHOMPMoveitTest, jointSpaceGoodGoal)
+{
+  move_group_.setStartState(*(move_group_.getCurrentState()));
+  move_group_.setJointValueTarget(std::vector<double>({ 0.0, 0.0 }));
+  move_group_.setPlanningTime(5.0);
+
+  moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_);
+  EXPECT_GT(my_plan_.trajectory_.joint_trajectory.points.size(), 0u);
+  EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::SUCCESS);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "chomp_moveit_test_panda");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp → moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp

@@ -107,7 +107,7 @@ TEST_F(CHOMPMoveitTest, collisionAtEndOfPath)
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);
-  ros::init(argc, argv, "chomp_moveit_test");
+  ros::init(argc, argv, "chomp_moveit_test_rrbot");
 
   ros::AsyncSpinner spinner(1);
   spinner.start();

moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp

@@ -205,26 +205,15 @@ void ChompOptimizer::initialize()
   {
     if (fixed_link_resolution_map.find(link->getParentJointModel()->getName()) == fixed_link_resolution_map.end())
     {
-      const moveit::core::JointModel* parent_model = nullptr;
-      bool found_root = false;
-
-      while (!found_root)
+      const moveit::core::JointModel* parent_model = link->getParentJointModel();
+      while (true)  // traverse up the tree until we find a joint we know about in joint_names_
       {
-        if (parent_model == nullptr)
-        {
-          parent_model = link->getParentJointModel();
-        }
-        else
-        {
-          parent_model = parent_model->getParentLinkModel()->getParentJointModel();
-          for (const std::string& joint_name : joint_names_)
-          {
-            if (parent_model->getName() == joint_name)
-            {
-              found_root = true;
-            }
-          }
-        }
+        if (parent_model->getParentLinkModel() == nullptr)
+          break;
+
+        parent_model = parent_model->getParentLinkModel()->getParentJointModel();
+        if (std::find(joint_names_.begin(), joint_names_.end(), parent_model->getName()) != joint_names_.end())
+          break;
       }
       fixed_link_resolution_map[link->getParentJointModel()->getName()] = parent_model->getName();
     }
@@ -963,7 +952,7 @@ void ChompOptimizer::setRobotStateFromPoint(ChompTrajectory& group_trajectory, i
   for (size_t j = 0; j < group_trajectory.getNumJoints(); ++j)
     joint_states.emplace_back(point(0, j));
 
-  state_.setJointGroupPositions(planning_group_, joint_states);
+  state_.setJointGroupActivePositions(planning_group_, joint_states);
   state_.update();
 }