From 64d917eca7e31b25e18eeb434c56ebba3d0da022 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 30 Jul 2024 23:48:06 -0700 Subject: [PATCH 1/6] Add new accessor methods to move group interface (#2925) Signed-off-by: methylDragon --- .../move_group_interface.h | 14 +++++ .../src/move_group_interface.cpp | 55 +++++++++++++------ 2 files changed, 53 insertions(+), 16 deletions(-) diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index c8b673a1b3..87f03685e0 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -236,6 +236,9 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface If the value is greater than 1, it is set to 1.0. */ void setMaxVelocityScalingFactor(double max_velocity_scaling_factor); + /** \brief Get the max velocity scaling factor set by setMaxVelocityScalingFactor(). */ + double getMaxVelocityScalingFactor() const; + /** \brief Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1]. The maximum joint acceleration specified in the robot model is multiplied by the factor. If the value is 0, it is set to @@ -243,6 +246,9 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface If the value is greater than 1, it is set to 1.0. */ void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor); + /** \brief Get the max acceleration scaling factor set by setMaxAccelerationScalingFactor(). */ + double getMaxAccelerationScalingFactor() const; + /** \brief Get the number of seconds set by setPlanningTime() */ double getPlanningTime() const; @@ -796,6 +802,14 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface /** \brief How often is the system allowed to move the camera to update environment model when looking */ void setLookAroundAttempts(int32_t attempts); + /** \brief Build a RobotState message for use with plan() or computeCartesianPath() + * If the move_group has a custom set start state, this method will use that as the robot state. + * + * Otherwise, the robot state will be with `is_diff` set to true, causing it to be an offset from the current state + * of the robot at time of the state's use. + */ + void constructRobotState(moveit_msgs::msg::RobotState& state); + /** \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); diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 4f93f72781..028819a611 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -359,11 +359,21 @@ class MoveGroupInterface::MoveGroupInterfaceImpl setMaxScalingFactor(max_velocity_scaling_factor_, value, "velocity_scaling_factor", 0.1); } + double getMaxVelocityScalingFactor() const + { + return max_velocity_scaling_factor_; + } + void setMaxAccelerationScalingFactor(double value) { setMaxScalingFactor(max_acceleration_scaling_factor_, value, "acceleration_scaling_factor", 0.1); } + double getMaxAccelerationScalingFactor() const + { + return max_acceleration_scaling_factor_; + } + void setMaxScalingFactor(double& variable, const double target_value, const char* factor_name, double fallback_value) { if (target_value > 1.0) @@ -857,14 +867,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl auto req = std::make_shared(); moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response; - if (considered_start_state_) - { - moveit::core::robotStateToRobotStateMsg(*considered_start_state_, req->start_state); - } - else - { - req->start_state.is_diff = true; - } + constructRobotState(req->start_state); req->group_name = opt_.group_name; req->header.frame_id = getPoseReferenceFrame(); @@ -1009,6 +1012,18 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return allowed_planning_time_; } + void constructRobotState(moveit_msgs::msg::RobotState& state) const + { + if (considered_start_state_) + { + moveit::core::robotStateToRobotStateMsg(*considered_start_state_, state); + } + else + { + state.is_diff = true; + } + } + void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request) const { request.group_name = opt_.group_name; @@ -1020,14 +1035,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.planner_id = planner_id_; request.workspace_parameters = workspace_parameters_; - if (considered_start_state_) - { - moveit::core::robotStateToRobotStateMsg(*considered_start_state_, request.start_state); - } - else - { - request.start_state.is_diff = true; - } + constructRobotState(request.start_state); if (active_target_ == JOINT) { @@ -1441,11 +1449,21 @@ void MoveGroupInterface::setMaxVelocityScalingFactor(double max_velocity_scaling impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor); } +double MoveGroupInterface::getMaxVelocityScalingFactor() const +{ + return impl_->getMaxVelocityScalingFactor(); +} + void MoveGroupInterface::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) { impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor); } +double MoveGroupInterface::getMaxAccelerationScalingFactor() const +{ + return impl_->getMaxAccelerationScalingFactor(); +} + moveit::core::MoveItErrorCode MoveGroupInterface::asyncMove() { return impl_->move(false); @@ -2326,6 +2344,11 @@ bool MoveGroupInterface::detachObject(const std::string& name) return impl_->detachObject(name); } +void MoveGroupInterface::constructRobotState(moveit_msgs::msg::RobotState& state) +{ + impl_->constructRobotState(state); +} + void MoveGroupInterface::constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& goal_out) { impl_->constructMotionPlanRequest(goal_out); From 97ea2044fdfa4454c9feba512484c2592320dcc6 Mon Sep 17 00:00:00 2001 From: FSund Date: Wed, 31 Jul 2024 10:08:56 +0200 Subject: [PATCH 2/6] Propagate "clear octomap" actions to monitoring planning scenes (#3134) Set a non-empty octomap.id if the octomap was cleared and perform an octomap update if that id is non-empty. Co-authored-by: Robert Haschke # Conflicts: # moveit_core/planning_scene/src/planning_scene.cpp # moveit_core/planning_scene/test/test_planning_scene.cpp --- .../planning_scene/src/planning_scene.cpp | 7 ++- .../test/test_planning_scene.cpp | 55 +++++++++++++++++++ 2 files changed, 60 insertions(+), 2 deletions(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 816bad2f8b..bf1a83eead 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -654,7 +654,10 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce { if (it.first == OCTOMAP_NS) { - do_omap = true; + if (it.second == collision_detection::World::DESTROY) + scene_msg.world.octomap.octomap.id = "cleared"; // indicate cleared octomap + else + do_omap = true; } else if (it.second == collision_detection::World::DESTROY) { @@ -1260,7 +1263,7 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScen result &= processCollisionObjectMsg(collision_object); // if an octomap was specified, replace the one we have with that one - if (!scene_msg.world.octomap.octomap.data.empty()) + if (!scene_msg.world.octomap.octomap.id.empty()) processOctomapMsg(scene_msg.world.octomap); return result; diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 4bd5c25ce9..909f3f6576 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -44,6 +44,8 @@ #include #include #include +#include +#include #include #include @@ -89,6 +91,59 @@ TEST(PlanningScene, LoadRestore) EXPECT_EQ(ps.getRobotModel()->getName(), ps_msg.robot_model_name); } +TEST(PlanningScene, LoadOctomap) +{ + urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); + srdf::ModelSharedPtr srdf_model(new srdf::Model()); + planning_scene::PlanningScene ps(urdf_model, srdf_model); + + { // check octomap before doing any operations on it + octomap_msgs::msg::OctomapWithPose msg; + ps.getOctomapMsg(msg); + EXPECT_TRUE(msg.octomap.id.empty()); + EXPECT_TRUE(msg.octomap.data.empty()); + } + + { // fill PlanningScene's octomap + octomap::OcTree octomap(0.1); + octomap::point3d origin(0, 0, 0); + octomap::point3d end(0, 1, 2); + octomap.insertRay(origin, end); + + // populate PlanningScene with octomap + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = true; + octomap_msgs::fullMapToMsg(octomap, msg.world.octomap.octomap); + ps.setPlanningSceneDiffMsg(msg); + + // validate octomap message + octomap_msgs::msg::OctomapWithPose octomap_msg; + ps.getOctomapMsg(octomap_msg); + EXPECT_EQ(octomap_msg.octomap.id, "OcTree"); + EXPECT_EQ(octomap_msg.octomap.data.size(), msg.world.octomap.octomap.data.size()); + } + + { // verify that a PlanningScene msg with an empty octomap id does not modify the octomap + // create planning scene + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = true; + ps.setPlanningSceneDiffMsg(msg); + + octomap_msgs::msg::OctomapWithPose octomap_msg; + ps.getOctomapMsg(octomap_msg); + EXPECT_EQ(octomap_msg.octomap.id, "OcTree"); + EXPECT_FALSE(octomap_msg.octomap.data.empty()); + } + + { // check that a non-empty octomap id, but empty octomap will clear the octomap + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = true; + msg.world.octomap.octomap.id = "xxx"; + ps.setPlanningSceneDiffMsg(msg); + EXPECT_FALSE(static_cast(ps.getWorld()->getObject(planning_scene::PlanningScene::OCTOMAP_NS))); + } +} + TEST(PlanningScene, LoadRestoreDiff) { urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); From 72a6e92c25dc36a7b710c72ba38915ed8b1aef70 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 31 Jul 2024 10:14:45 +0200 Subject: [PATCH 3/6] Silent "empty quaternion" warning from poseMsgToEigen() (#3435) # Conflicts: # moveit_core/planning_scene/src/planning_scene.cpp --- .../planning_scene/src/planning_scene.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index bf1a83eead..479f3426ce 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1726,16 +1726,16 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: shapes.reserve(num_shapes); shape_poses.reserve(num_shapes); - utilities::poseMsgToEigen(object.pose, object_pose); - bool switch_object_pose_and_shape_pose = false; - if (num_shapes == 1) + if (num_shapes == 1 && moveit::core::isEmpty(object.pose)) { - if (moveit::core::isEmpty(object.pose)) - { - switch_object_pose_and_shape_pose = true; // If the object pose is not set but the shape pose is, - // use the shape's pose as the object pose. - } + // If the object pose is not set but the shape pose is, use the shape's pose as the object pose. + switch_object_pose_and_shape_pose = true; + object_pose.setIdentity(); + } + else + { + utilities::poseMsgToEigen(object.pose, object_pose); } auto append = [&object_pose, &shapes, &shape_poses, From 47681f3e24f5c0d85fcf62246e1680639e87bb5d Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Wed, 15 May 2024 03:49:57 -0400 Subject: [PATCH 4/6] Allow moving of all shapes of an object in one go (#3599) --- .../moveit/collision_detection/world.h | 3 ++ moveit_core/collision_detection/src/world.cpp | 20 ++++++++++ .../planning_scene/src/planning_scene.cpp | 40 +++++++++++++++++++ 3 files changed, 63 insertions(+) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index 92799537c5..d610d4b43c 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -217,6 +217,9 @@ class World bool moveShapeInObject(const std::string& object_id, const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& shape_pose); + /** \brief Update the pose of all shapes in an object. Shape size is verified. Returns true on success. */ + bool moveShapesInObject(const std::string& object_id, const EigenSTL::vector_Isometry3d& shape_poses); + /** \brief Move the object pose (thus moving all shapes and subframes in the object) * according to the given transform specified in world frame. * The transform is relative to and changes the object pose. It does not replace it. diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index eae07cf01c..ce7c6006fc 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -259,6 +259,26 @@ bool World::moveShapeInObject(const std::string& object_id, const shapes::ShapeC return false; } +bool World::moveShapesInObject(const std::string& object_id, const EigenSTL::vector_Isometry3d& shape_poses) +{ + auto it = objects_.find(object_id); + if (it != objects_.end()) + { + if (shape_poses.size() == it->second->shapes_.size()) + { + for (std::size_t i = 0; i < shape_poses.size(); ++i) + { + ASSERT_ISOMETRY(shape_poses[i]) // unsanitized input, could contain a non-isometry + it->second->shape_poses_[i] = shape_poses[i]; + it->second->global_shape_poses_[i] = it->second->pose_ * shape_poses[i]; + } + notify(it->second, MOVE_SHAPE); + return true; + } + } + return false; +} + bool World::moveObject(const std::string& object_id, const Eigen::Isometry3d& transform) { const auto it = objects_.find(object_id); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 479f3426ce..9dbcab0add 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1857,6 +1857,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision { if (world_->hasObject(object.id)) { + // update object pose if (!object.primitives.empty() || !object.meshes.empty() || !object.planes.empty()) { RCLCPP_WARN(getLogger(), "Move operation for object '%s' ignores the geometry specified in the message.", @@ -1870,6 +1871,45 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * header_to_pose_transform; world_->setObjectPose(object.id, object_frame_transform); + + // update shape poses + if (!object.primitive_poses.empty() || !object.mesh_poses.empty() || !object.plane_poses.empty()) + { + auto world_object = world_->getObject(object.id); // object exists, checked earlier + + std::size_t shape_size = object.primitive_poses.size() + object.mesh_poses.size() + object.plane_poses.size(); + if (shape_size != world_object->shape_poses_.size()) + { + ROS_ERROR_NAMED(LOGNAME, "Move operation for object '%s' must have same number of geometry poses. Cannot move.", + object.id.c_str()); + return false; + } + + // order matters -> primitive, mesh and plane + EigenSTL::vector_Isometry3d shape_poses; + for (const auto& shape_pose : object.primitive_poses) + { + shape_poses.emplace_back(); + PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + } + for (const auto& shape_pose : object.mesh_poses) + { + shape_poses.emplace_back(); + PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + } + for (const auto& shape_pose : object.plane_poses) + { + shape_poses.emplace_back(); + PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + } + + if (!world_->moveShapesInObject(object.id, shape_poses)) + { + ROS_ERROR_NAMED(LOGNAME, "Move operation for object '%s' internal world error. Cannot move.", object.id.c_str()); + return false; + } + } + return true; } From 1d93035fc4fbc2831916bce26d7b03e15b8a86d5 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Fri, 19 Jul 2024 06:49:45 -0400 Subject: [PATCH 5/6] Optimize MOVE_SHAPE operations for FCL (#3601) Don't recreate the FCL object when moving shapes, but just update its transforms. --- .../test_collision_common_panda.h | 13 ++++++++ .../src/collision_env_fcl.cpp | 32 +++++++++++++++++++ .../planning_scene/src/planning_scene.cpp | 18 +++++++---- 3 files changed, 57 insertions(+), 6 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index 92163406aa..061a304085 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -155,11 +155,24 @@ TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_1) ASSERT_TRUE(res.collision); res.clear(); + pos1.translation().z() = 0.25; + this->cenv_->getWorld()->moveObject("box", pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + pos1.translation().z() = 0.05; this->cenv_->getWorld()->moveObject("box", pos1); this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_TRUE(res.collision); res.clear(); + pos1.translation().z() = 0.25; + this->cenv_->getWorld()->moveObject("box", pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + this->cenv_->getWorld()->moveObject("box", pos1); ASSERT_FALSE(res.collision); } diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index 54a007d325..86a48b8320 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -450,6 +450,38 @@ void CollisionEnvFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Actio } cleanCollisionGeometryCache(); } + else if (action == World::MOVE_SHAPE) + { + auto it = fcl_objs_.find(obj->id_); + if (it == fcl_objs_.end()) + { + RCLCPP_ERROR(getLogger(), "Cannot move shapes of unknown FCL object: '%s'", obj->id_.c_str()); + return; + } + + if (obj->global_shape_poses_.size() != it->second.collision_objects_.size()) + { + RCLCPP_ERROR(getLogger(), + "Cannot move shapes, shape size mismatch between FCL object and world object: '%s'. Respectively " + "%zu and %zu.", + obj->id_.c_str(), it->second.collision_objects_.size(), it->second.collision_objects_.size()); + return; + } + + for (std::size_t i = 0; i < it->second.collision_objects_.size(); ++i) + { + it->second.collision_objects_[i]->setTransform(transform2fcl(obj->global_shape_poses_[i])); + + // compute AABB, order matters + it->second.collision_geometry_[i]->collision_geometry_->computeLocalAABB(); + it->second.collision_objects_[i]->computeAABB(); + } + + // update AABB in the FCL broadphase manager tree + // see https://github.com/moveit/moveit/pull/3601 for benchmarks + it->second.unregisterFrom(manager_.get()); + it->second.registerTo(manager_.get()); + } else { updateFCLObject(obj->id_); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 9dbcab0add..0b955ac888 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -655,9 +655,13 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce if (it.first == OCTOMAP_NS) { if (it.second == collision_detection::World::DESTROY) + { scene_msg.world.octomap.octomap.id = "cleared"; // indicate cleared octomap + } else + { do_omap = true; + } } else if (it.second == collision_detection::World::DESTROY) { @@ -1880,8 +1884,9 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision std::size_t shape_size = object.primitive_poses.size() + object.mesh_poses.size() + object.plane_poses.size(); if (shape_size != world_object->shape_poses_.size()) { - ROS_ERROR_NAMED(LOGNAME, "Move operation for object '%s' must have same number of geometry poses. Cannot move.", - object.id.c_str()); + RCLCPP_ERROR(getLogger(), + "Move operation for object '%s' must have same number of geometry poses. Cannot move.", + object.id.c_str()); return false; } @@ -1890,22 +1895,23 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision for (const auto& shape_pose : object.primitive_poses) { shape_poses.emplace_back(); - PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + utilities::poseMsgToEigen(shape_pose, shape_poses.back()); } for (const auto& shape_pose : object.mesh_poses) { shape_poses.emplace_back(); - PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + utilities::poseMsgToEigen(shape_pose, shape_poses.back()); } for (const auto& shape_pose : object.plane_poses) { shape_poses.emplace_back(); - PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + utilities::poseMsgToEigen(shape_pose, shape_poses.back()); } if (!world_->moveShapesInObject(object.id, shape_poses)) { - ROS_ERROR_NAMED(LOGNAME, "Move operation for object '%s' internal world error. Cannot move.", object.id.c_str()); + RCLCPP_ERROR(getLogger(), "Move operation for object '%s' internal world error. Cannot move.", + object.id.c_str()); return false; } } From 955f308f166b07b9c1a4fbf34bab922a9968ee0b Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Fri, 2 Aug 2024 10:26:48 -0400 Subject: [PATCH 6/6] Deduplicate joint trajectory points in Pilz Move Group Sequence capability (#2943) * Deduplicate joint trajectory points before sending them to controllers * Fix max loop value * Move deduplication code to Pilz * Clean up * Add todo --- .../robot_trajectory/robot_trajectory.h | 11 ++++++++++ .../cartesian_trajectory_point.h | 1 - .../src/command_list_manager.cpp | 21 ++++++++++++++++++- 3 files changed, 31 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 1f77946234..ed2887e2a2 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -244,6 +244,17 @@ class RobotTrajectory void swap(robot_trajectory::RobotTrajectory& other) noexcept; + /** + * \brief Remove a point from the trajectory + * \param index - the index to remove + */ + RobotTrajectory& removeWayPoint(std::size_t index) + { + waypoints_.erase(waypoints_.begin() + index); + duration_from_previous_.erase(duration_from_previous_.begin() + index); + return *this; + } + RobotTrajectory& clear() { waypoints_.clear(); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h index 4835dc8876..e810ae4726 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h @@ -38,7 +38,6 @@ #include #include -#include namespace pilz_industrial_motion_planner { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp index be11aa84da..de829de34a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -112,7 +112,26 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst // therefore: "i-1". (i > 0 ? radii.at(i - 1) : 0.)); } - return plan_comp_builder_.build(); + + const auto res_vec = plan_comp_builder_.build(); + + // De-duplicate trajectory points with the same time value. + // This is necessary since some controllers do not allow times that are not monotonically increasing. + // TODO: Ideally, we would not need this code if the trajectory segments were created without + // duplicate time points in the first place. Leaving this note to revisit this with a more principled fix. + for (const auto& traj : res_vec) + { + for (size_t i = 0; i < traj->size() - 1; ++i) + { + if (traj->getWayPointDurationFromStart(i) == traj->getWayPointDurationFromStart(i + 1)) + { + RCLCPP_WARN(getLogger(), "Removed duplicate point at time=%f", traj->getWayPointDurationFromStart(i)); + traj->removeWayPoint(i + 1); + } + } + } + + return res_vec; } bool CommandListManager::checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A,