Merge moveit/moveit@a0ee202

moveit_core/planning_scene/src/planning_scene.cpp

@@ -1652,8 +1652,10 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::
       for (std::size_t i = 0; i < shape_vector.size(); ++i)
       {
         if (i >= shape_poses_vector.size())
+        {
           append(shapes::constructShapeFromMsg(shape_vector[i]),
                  geometry_msgs::msg::Pose());  // Empty shape pose => Identity
+        }
         else
           append(shapes::constructShapeFromMsg(shape_vector[i]), shape_poses_vector[i]);
       }

moveit_core/robot_state/src/robot_state.cpp

@@ -473,7 +473,7 @@ void RobotState::setVariableEffort(const std::map<std::string, double>& variable
 {
   markEffort();
   for (const std::pair<const std::string, double>& it : variable_map)
-    acceleration_[robot_model_->getVariableIndex(it.first)] = it.second;
+    effort_[robot_model_->getVariableIndex(it.first)] = it.second;
 }
 
 void RobotState::setVariableEffort(const std::map<std::string, double>& variable_map,

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

@@ -940,15 +940,16 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
     max_velocity[j] = 1.0;
     if (bounds.velocity_bounded_)
     {
-      max_velocity[j] = std::min(fabs(bounds.max_velocity_), fabs(bounds.min_velocity_)) * velocity_scaling_factor;
+      max_velocity[j] =
+          std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)) * velocity_scaling_factor;
       max_velocity[j] = std::max(0.01, max_velocity[j]);
     }
 
     max_acceleration[j] = 1.0;
     if (bounds.acceleration_bounded_)
     {
-      max_acceleration[j] =
-          std::min(fabs(bounds.max_acceleration_), fabs(bounds.min_acceleration_)) * acceleration_scaling_factor;
+      max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) *
+                            acceleration_scaling_factor;
       max_acceleration[j] = std::max(0.01, max_acceleration[j]);
     }
   }
@@ -960,13 +961,17 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
   {
     moveit::core::RobotStatePtr waypoint = trajectory.getWayPointPtr(p);
     Eigen::VectorXd new_point(num_joints);
+    // The first point should always be kept
     bool diverse_point = (p == 0);
 
     for (size_t j = 0; j < num_joints; ++j)
     {
       new_point[j] = waypoint->getVariablePosition(idx[j]);
-      if (p > 0 && std::abs(new_point[j] - points.back()[j]) > min_angle_change_)
+      // If any joint angle is different, it's a unique waypoint
+      if (p > 0 && std::fabs(new_point[j] - points.back()[j]) > min_angle_change_)
+      {
         diverse_point = true;
+      }
     }
 
     if (diverse_point)

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp

@@ -267,7 +267,7 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin
   }
 }
 
-void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& scene,
+void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& /*scene*/,
                                   const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
                                   const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
 {

moveit_planners/pilz_industrial_motion_planner/test/integration_tests/launch/python_move_group_planning.test

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Start joint_state_publisher, robot_state_publisher, and MoveGroup -->
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch"/>
+
+  <!-- Test MoveGroupInterface for Python -->
+  <test pkg="pilz_industrial_motion_planner" type="python_move_group_planning.py" test-name="python_move_group_planning"
+        time-limit="300" args=""/>
+</launch>

moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/python_move_group_planning.py

@@ -0,0 +1,118 @@
+#!/usr/bin/env python
+
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2021, Cristian C. Beltran-Hernandez
+# 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 Cristian C. Beltran-Hernandez 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: Cristian C. Beltran-Hernandez
+#
+# This test is used to ensure planning with an attached object
+# correctly validates collision states between the attached objects
+# and the environment
+
+import unittest
+import numpy as np
+import rospy
+import rostest
+import os
+
+from moveit_msgs.msg import MoveItErrorCodes
+import moveit_commander
+
+from geometry_msgs.msg import PoseStamped
+
+
+class PythonMoveGroupPlanningTest(unittest.TestCase):
+    @classmethod
+    def setUpClass(self):
+        PLANNING_GROUP = "panda_arm"
+        self.group = moveit_commander.MoveGroupCommander(PLANNING_GROUP)
+        self.planning_scene_interface = moveit_commander.PlanningSceneInterface(
+            synchronous=True
+        )
+
+    @classmethod
+    def tearDown(self):
+        pass
+
+    def test_planning_with_collision_objects(self):
+        # Add obstacle to the world
+        ps = PoseStamped()
+        ps.header.frame_id = "world"
+        ps.pose.position.x = 0.4
+        ps.pose.position.y = 0.1
+        ps.pose.position.z = 0.25
+        self.planning_scene_interface.add_box(
+            name="box1", pose=ps, size=(0.1, 0.1, 0.5)
+        )
+
+        # Attach object to robot's TCP
+        ps2 = PoseStamped()
+        tcp_link = self.group.get_end_effector_link()
+        ps2.header.frame_id = tcp_link
+        ps2.pose.position.z = 0.15
+        self.planning_scene_interface.attach_box(
+            link=tcp_link,
+            name="box2",
+            pose=ps2,
+            size=(0.1, 0.1, 0.1),
+            touch_links=["panda_rightfinger", "panda_leftfinger"],
+        )
+
+        # Plan a motion where the attached object 'box2' collides with the obstacle 'box1'
+        target_pose = self.group.get_current_pose(tcp_link)
+        target_pose.pose.position.y += 0.1
+
+        # # Set planner to be Pilz's Linear Planner
+        # self.group.set_planning_pipeline_id("pilz_industrial_motion_planner")
+        # self.group.set_planner_id("LIN")
+        # self.group.set_pose_target(target_pose)
+        # success, plan, time, error_code = self.group.plan()
+
+        # # Planning should fail
+        # self.assertEqual(error_code.val, MoveItErrorCodes.INVALID_MOTION_PLAN)
+
+        # Set planner to be Pilz's Point-To-Point Planner
+        self.group.set_planning_pipeline_id("pilz_industrial_motion_planner")
+        self.group.set_planner_id("PTP")
+        self.group.set_pose_target(target_pose)
+        success, plan, time, error_code = self.group.plan()
+
+        # Planning should fail
+        self.assertFalse(success)
+        self.assertEqual(error_code.val, MoveItErrorCodes.INVALID_MOTION_PLAN)
+
+
+if __name__ == "__main__":
+    PKGNAME = "moveit_ros_planning_interface"
+    NODENAME = "moveit_test_python_move_group"
+    rospy.init_node(NODENAME)
+    rostest.rosrun(PKGNAME, NODENAME, PythonMoveGroupPlanningTest)

moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp

@@ -231,7 +231,7 @@ bool testutils::checkCartesianLinearity(const moveit::core::RobotModelConstPtr&
                                         const trajectory_msgs::msg::JointTrajectory& trajectory,
                                         const planning_interface::MotionPlanRequest& req,
                                         const double translation_norm_tolerance, const double rot_axis_norm_tolerance,
-                                        const double rot_angle_tolerance)
+                                        const double /*rot_angle_tolerance*/)
 {
   std::string link_name;
   Eigen::Isometry3d goal_pose_expect;
@@ -1126,7 +1126,8 @@ bool testutils::checkBlendResult(const pilz_industrial_motion_planner::Trajector
                                  const pilz_industrial_motion_planner::TrajectoryBlendResponse& blend_res,
                                  const pilz_industrial_motion_planner::LimitsContainer& limits,
                                  double joint_velocity_tolerance, double joint_acceleration_tolerance,
-                                 double cartesian_velocity_tolerance, double cartesian_angular_velocity_tolerance)
+                                 double /*cartesian_velocity_tolerance*/,
+                                 double /*cartesian_angular_velocity_tolerance*/)
 {
   // ++++++++++++++++++++++
   // + Check trajectories +

moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h

@@ -790,13 +790,22 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
   /** \brief Stop any trajectory execution, if one is active */
   void stop();
 
-  /** \brief Specify whether the robot is allowed to look around before moving if it determines it should (default is
-   * true) */
-  void allowLooking(bool flag);
-
   /** \brief Specify whether the robot is allowed to replan if it detects changes in the environment */
   void allowReplanning(bool flag);
 
+  /** \brief Maximum number of replanning attempts */
+  void setReplanAttempts(int32_t attempts);
+
+  /** \brief Sleep this duration between replanning attempts (in walltime seconds) */
+  void setReplanDelay(double delay);
+
+  /** \brief Specify whether the robot is allowed to look around
+      before moving if it determines it should (default is false) */
+  void allowLooking(bool flag);
+
+  /** \brief How often is the system allowed to move the camera to update environment model when looking */
+  void setLookAroundAttempts(int32_t attempts);
+
   /** \brief Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it
       in \e request */
   void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request);

moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp

@@ -100,6 +100,8 @@ enum ActiveTargetType
 
 class MoveGroupInterface::MoveGroupInterfaceImpl
 {
+  friend MoveGroupInterface;
+
 public:
   MoveGroupInterfaceImpl(const rclcpp::Node::SharedPtr& node, const Options& opt,
                          const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const rclcpp::Duration& wait_for_servers)
@@ -128,8 +130,10 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     joint_state_target_->setToDefaultValues();
     active_target_ = JOINT;
     can_look_ = false;
+    look_around_attempts_ = 0;
     can_replan_ = false;
     replan_delay_ = 2.0;
+    replan_attempts_ = 1;
     goal_joint_tolerance_ = 1e-4;
     goal_position_tolerance_ = 1e-4;     // 0.1 mm
     goal_orientation_tolerance_ = 1e-3;  // ~0.1 deg
@@ -1108,29 +1112,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     return allowed_planning_time_;
   }
 
-  void allowLooking(bool flag)
-  {
-    can_look_ = flag;
-    RCLCPP_INFO(LOGGER, "Looking around: %s", can_look_ ? "yes" : "no");
-  }
-
-  void allowReplanning(bool flag)
-  {
-    can_replan_ = flag;
-    RCLCPP_INFO(LOGGER, "Replanning: %s", can_replan_ ? "yes" : "no");
-  }
-
-  void setReplanningDelay(double delay)
-  {
-    if (delay >= 0.0)
-      replan_delay_ = delay;
-  }
-
-  double getReplanningDelay() const
-  {
-    return replan_delay_;
-  }
-
   void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request) const
   {
     request.group_name = opt_.group_name_;
@@ -1389,7 +1370,9 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
   double goal_position_tolerance_;
   double goal_orientation_tolerance_;
   bool can_look_;
+  int32_t look_around_attempts_;
   bool can_replan_;
+  int32_t replan_attempts_;
   double replan_delay_;
 
   // joint state goal
@@ -2260,12 +2243,53 @@ void MoveGroupInterface::forgetJointValues(const std::string& name)
 
 void MoveGroupInterface::allowLooking(bool flag)
 {
-  impl_->allowLooking(flag);
+  impl_->can_look_ = flag;
+  RCLCPP_DEBUG(LOGGER, "Looking around: %s", flag ? "yes" : "no");
+}
+
+void MoveGroupInterface::setLookAroundAttempts(int32_t attempts)
+{
+  if (attempts < 0)
+  {
+    RCLCPP_ERROR(LOGGER, "Tried to set negative number of look-around attempts");
+  }
+  else
+  {
+    RCLCPP_DEBUG_STREAM(LOGGER, "Look around attempts: " << attempts);
+    impl_->look_around_attempts_ = attempts;
+  }
 }
 
 void MoveGroupInterface::allowReplanning(bool flag)
 {
-  impl_->allowReplanning(flag);
+  impl_->can_replan_ = flag;
+  RCLCPP_DEBUG(LOGGER, "Replanning: %s", flag ? "yes" : "no");
+}
+
+void MoveGroupInterface::setReplanAttempts(int32_t attempts)
+{
+  if (attempts < 0)
+  {
+    RCLCPP_ERROR(LOGGER, "Tried to set negative number of replan attempts");
+  }
+  else
+  {
+    RCLCPP_DEBUG_STREAM(LOGGER, "Replan Attempts: " << attempts);
+    impl_->replan_attempts_ = attempts;
+  }
+}
+
+void MoveGroupInterface::setReplanDelay(double delay)
+{
+  if (delay < 0.0)
+  {
+    RCLCPP_ERROR(LOGGER, "Tried to set negative replan delay");
+  }
+  else
+  {
+    RCLCPP_DEBUG_STREAM(LOGGER, "Replan Delay: " << delay);
+    impl_->replan_delay_ = delay;
+  }
 }
 
 std::vector<std::string> MoveGroupInterface::getKnownConstraints() const

moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp

@@ -160,18 +160,10 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl
       {
         if (std::find(object_ids.begin(), object_ids.end(), collision_object.id) != object_ids.end())
         {
-          if (collision_object.mesh_poses.empty() && collision_object.primitive_poses.empty())
-            continue;
-          if (!collision_object.mesh_poses.empty())
-            result[collision_object.id] = collision_object.mesh_poses[0];
-          else
-            result[collision_object.id] = collision_object.primitive_poses[0];
+          result[collision_object.id] = collision_object.pose;
         }
       }
     }
-    else
-      RCLCPP_WARN(LOGGER, "Could not call planning scene service to get object names");
-
     return result;
   }