Add SimplePlannerLVSAssignNoIKPlanProfile

tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h

@@ -0,0 +1,81 @@
+/**
+ * @file simple_planner_lvs_assign_no_ik_plan_profile.h
+ * @brief
+ *
+ * @author Roelof Oomen
+ * @date May 29, 2024
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2024, ROS Industrial Consortium
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * @par
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_NO_IK_PLAN_PROFILE_H
+#define TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_NO_IK_PLAN_PROFILE_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <Eigen/Geometry>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_motion_planners/simple/profile/simple_planner_profile.h>
+
+namespace tesseract_planning
+{
+class SimplePlannerLVSAssignNoIKPlanProfile : public SimplePlannerPlanProfile
+{
+public:
+  using Ptr = std::shared_ptr<SimplePlannerLVSAssignNoIKPlanProfile>;
+  using ConstPtr = std::shared_ptr<const SimplePlannerLVSAssignNoIKPlanProfile>;
+
+  /**
+   * @brief SimplePlannerFixedSizeAssignPlanProfile
+   * @param freespace_steps The number of steps to use for freespace instruction
+   * @param linear_steps The number of steps to use for linear instruction
+   */
+  SimplePlannerLVSAssignNoIKPlanProfile(double state_longest_valid_segment_length = 5 * M_PI / 180,
+                                        double translation_longest_valid_segment_length = 0.1,
+                                        double rotation_longest_valid_segment_length = 5 * M_PI / 180,
+                                        int min_steps = 1,
+                                        int max_steps = std::numeric_limits<int>::max());
+
+  std::vector<MoveInstructionPoly> generate(const MoveInstructionPoly& prev_instruction,
+                                            const MoveInstructionPoly& prev_seed,
+                                            const MoveInstructionPoly& base_instruction,
+                                            const InstructionPoly& next_instruction,
+                                            const PlannerRequest& request,
+                                            const tesseract_common::ManipulatorInfo& global_manip_info) const override;
+
+  /** @brief The maximum joint distance, the norm of changes to all joint positions between successive steps. */
+  double state_longest_valid_segment_length;
+
+  /** @brief The maximum translation distance between successive steps */
+  double translation_longest_valid_segment_length;
+
+  /** @brief The maximum rotational distance between successive steps */
+  double rotation_longest_valid_segment_length;
+
+  /** @brief The minimum number of steps for the plan */
+  int min_steps;
+
+  /** @brief The maximum number of steps for the plan */
+  int max_steps;
+};
+
+}  // namespace tesseract_planning
+
+#endif  // TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_NO_IK_PLAN_PROFILE_H

tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp

@@ -0,0 +1,143 @@
+/**
+ * @file simple_planner_lvs_assign_no_ik_plan_profile.cpp
+ * @brief
+ *
+ * @author Roelof Oomen
+ * @date May 29, 2024
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2024, ROS Industrial Consortium
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * @par
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <tesseract_command_language/cartesian_waypoint.h>
+#include <tesseract_command_language/poly/move_instruction_poly.h>
+#include <tesseract_kinematics/core/kinematic_group.h>
+#include <tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h>
+#include <tesseract_motion_planners/simple/interpolation.h>
+#include <tesseract_motion_planners/core/utils.h>
+#include <tesseract_motion_planners/core/types.h>
+
+namespace tesseract_planning
+{
+SimplePlannerLVSAssignNoIKPlanProfile::SimplePlannerLVSAssignNoIKPlanProfile(
+    double state_longest_valid_segment_length,
+    double translation_longest_valid_segment_length,
+    double rotation_longest_valid_segment_length,
+    int min_steps,
+    int max_steps)
+  : state_longest_valid_segment_length(state_longest_valid_segment_length)
+  , translation_longest_valid_segment_length(translation_longest_valid_segment_length)
+  , rotation_longest_valid_segment_length(rotation_longest_valid_segment_length)
+  , min_steps(min_steps)
+  , max_steps(max_steps)
+{
+}
+
+std::vector<MoveInstructionPoly>
+SimplePlannerLVSAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instruction,
+                                                const MoveInstructionPoly& /*prev_seed*/,
+                                                const MoveInstructionPoly& base_instruction,
+                                                const InstructionPoly& /*next_instruction*/,
+                                                const PlannerRequest& request,
+                                                const tesseract_common::ManipulatorInfo& global_manip_info) const
+{
+  JointGroupInstructionInfo prev(prev_instruction, request, global_manip_info);
+  JointGroupInstructionInfo base(base_instruction, request, global_manip_info);
+
+  Eigen::VectorXd j1;
+  Eigen::Isometry3d p1_world;
+  bool has_j1 = false;
+  if (prev.has_cartesian_waypoint)
+  {
+    p1_world = prev.extractCartesianPose();
+    if (prev.instruction.getWaypoint().as<CartesianWaypointPoly>().hasSeed())
+    {
+      j1 = prev.instruction.getWaypoint().as<CartesianWaypointPoly>().getSeed().position;
+      has_j1 = true;
+    }
+  }
+  else
+  {
+    j1 = prev.extractJointPosition();
+    p1_world = prev.calcCartesianPose(j1);
+    has_j1 = true;
+  }
+
+  Eigen::VectorXd j2;
+  Eigen::Isometry3d p2_world;
+  bool has_j2 = false;
+  if (base.has_cartesian_waypoint)
+  {
+    p2_world = base.extractCartesianPose();
+    if (base.instruction.getWaypoint().as<CartesianWaypointPoly>().hasSeed())
+    {
+      j2 = base.instruction.getWaypoint().as<CartesianWaypointPoly>().getSeed().position;
+      has_j2 = true;
+    }
+  }
+  else
+  {
+    j2 = base.extractJointPosition();
+    p2_world = base.calcCartesianPose(j2);
+    has_j2 = true;
+  }
+
+  double trans_dist = (p2_world.translation() - p1_world.translation()).norm();
+  double rot_dist = Eigen::Quaterniond(p1_world.linear()).angularDistance(Eigen::Quaterniond(p2_world.linear()));
+  int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1;
+  int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1;
+  int steps = std::max(trans_steps, rot_steps);
+
+  if (has_j1 && has_j2)
+  {
+    double joint_dist = (j2 - j1).norm();
+    int joint_steps = int(joint_dist / state_longest_valid_segment_length) + 1;
+    steps = std::max(steps, joint_steps);
+  }
+
+  Eigen::MatrixXd states;
+  if (has_j2)
+  {
+    states = j2.replicate(1, steps + 1);
+  }
+  else if (has_j1)
+  {
+    states = j1.replicate(1, steps + 1);
+  }
+  else
+  {
+    Eigen::VectorXd seed = request.env_state.getJointValues(base.manip->getJointNames());
+    tesseract_common::enforceLimits<double>(seed, base.manip->getLimits().joint_limits);
+    states = seed.replicate(1, steps + 1);
+  }
+
+  // Linearly interpolate in cartesian space if linear move
+  if (base_instruction.isLinear())
+  {
+    tesseract_common::VectorIsometry3d poses = interpolate(p1_world, p2_world, steps);
+    for (auto& pose : poses)
+      pose = base.working_frame_transform.inverse() * pose;
+
+    assert(poses.size() == states.cols());
+    return getInterpolatedInstructions(poses, base.manip->getJointNames(), states, base.instruction);
+  }
+
+  return getInterpolatedInstructions(base.manip->getJointNames(), states, base.instruction);
+}
+
+}  // namespace tesseract_planning

tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp

@@ -3,7 +3,7 @@
  * @brief
  *
  * @author Roelof Oomen
- * @date March 19, 2024
+ * @date May 29, 2024
  * @version TODO
  * @bug No known bugs
  *
@@ -96,7 +96,7 @@ SimplePlannerLVSAssignPlanProfile::generate(const MoveInstructionPoly& prev_inst
         j1 = getClosestJointSolution(prev, base.extractJointPosition());
       }
     }
-    tesseract_common::enforcePositionLimits<double>(j1, prev.manip->getLimits().joint_limits);
+    tesseract_common::enforceLimits<double>(j1, prev.manip->getLimits().joint_limits);
   }
 
   Eigen::VectorXd j2;
@@ -137,7 +137,7 @@ SimplePlannerLVSAssignPlanProfile::generate(const MoveInstructionPoly& prev_inst
         j2 = getClosestJointSolution(base, prev.extractJointPosition());
       }
     }
-    tesseract_common::enforcePositionLimits<double>(j2, base.manip->getLimits().joint_limits);
+    tesseract_common::enforceLimits<double>(j2, base.manip->getLimits().joint_limits);
   }
 
   double trans_dist = (p2_world.translation() - p1_world.translation()).norm();