-
Notifications
You must be signed in to change notification settings - Fork 38
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Rename SimplePlannerFixedSizeAssignPlanProfile to SimplePlannerFixedS…
…izeAssignNoIKPlanProfile and add SimplePlannerFixedSizeAssignPlanProfile (with IK)
- Loading branch information
Showing
5 changed files
with
221 additions
and
28 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
63 changes: 63 additions & 0 deletions
63
...ract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,63 @@ | ||
| /** | ||
| * @file simple_planner_fixed_size_assign_no_ik_plan_profile.h | ||
| * @brief | ||
| * | ||
| * @author Matthew Powelson | ||
| * @date July 23, 2020 | ||
| * @version TODO | ||
| * @bug No known bugs | ||
| * | ||
| * @copyright Copyright (c) 2020, Southwest Research Institute | ||
| * | ||
| * @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_FIXED_SIZE_ASSIGN_NO_IK_PLAN_PROFILE_H | ||
| #define TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_NO_IK_PLAN_PROFILE_H | ||
|
|
||
| #include <tesseract_motion_planners/simple/profile/simple_planner_profile.h> | ||
|
|
||
| namespace tesseract_planning | ||
| { | ||
| class SimplePlannerFixedSizeAssignNoIKPlanProfile : public SimplePlannerPlanProfile | ||
| { | ||
| public: | ||
| using Ptr = std::shared_ptr<SimplePlannerFixedSizeAssignNoIKPlanProfile>; | ||
| using ConstPtr = std::shared_ptr<const SimplePlannerFixedSizeAssignNoIKPlanProfile>; | ||
|
|
||
| /** | ||
| * @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 | ||
| */ | ||
| SimplePlannerFixedSizeAssignNoIKPlanProfile(int freespace_steps = 10, int linear_steps = 10); | ||
|
|
||
| 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 number of steps to use for freespace instruction */ | ||
| int freespace_steps; | ||
|
|
||
| /** @brief The number of steps to use for linear instruction */ | ||
| int linear_steps; | ||
| }; | ||
|
|
||
| } // namespace tesseract_planning | ||
|
|
||
| #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_NO_IK_PLAN_PROFILE_H |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
128 changes: 128 additions & 0 deletions
128
...otion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,128 @@ | ||
| /** | ||
| * @file simple_planner_fixed_size_assign_no_ik_plan_profile.cpp | ||
| * @brief | ||
| * | ||
| * @author Matthew Powelson | ||
| * @date July 23, 2020 | ||
| * @version TODO | ||
| * @bug No known bugs | ||
| * | ||
| * @copyright Copyright (c) 2020, Southwest Research Institute | ||
| * | ||
| * @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_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h> | ||
| #include <tesseract_motion_planners/simple/interpolation.h> | ||
| #include <tesseract_motion_planners/core/types.h> | ||
| #include <tesseract_motion_planners/core/utils.h> | ||
|
|
||
| #include <tesseract_common/manipulator_info.h> | ||
| #include <tesseract_common/kinematic_limits.h> | ||
|
|
||
| #include <tesseract_kinematics/core/kinematic_group.h> | ||
|
|
||
| #include <tesseract_command_language/poly/move_instruction_poly.h> | ||
|
|
||
| namespace tesseract_planning | ||
| { | ||
| SimplePlannerFixedSizeAssignNoIKPlanProfile::SimplePlannerFixedSizeAssignNoIKPlanProfile(int freespace_steps, | ||
| int linear_steps) | ||
| : freespace_steps(freespace_steps), linear_steps(linear_steps) | ||
| { | ||
| } | ||
|
|
||
| std::vector<MoveInstructionPoly> | ||
| SimplePlannerFixedSizeAssignNoIKPlanProfile::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::MatrixXd states; | ||
| if (!prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) | ||
| { | ||
| const Eigen::VectorXd& jp = base.extractJointPosition(); | ||
| if (base.instruction.isLinear()) | ||
| states = jp.replicate(1, linear_steps + 1); | ||
| else if (base.instruction.isFreespace()) | ||
| states = jp.replicate(1, freespace_steps + 1); | ||
| else | ||
| throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); | ||
| } | ||
| else if (!prev.has_cartesian_waypoint && base.has_cartesian_waypoint) | ||
| { | ||
| const Eigen::VectorXd& jp = prev.extractJointPosition(); | ||
| if (base.instruction.isLinear()) | ||
| states = jp.replicate(1, linear_steps + 1); | ||
| else if (base.instruction.isFreespace()) | ||
| states = jp.replicate(1, freespace_steps + 1); | ||
| else | ||
| throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); | ||
| } | ||
| else if (prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) | ||
| { | ||
| const Eigen::VectorXd& jp = base.extractJointPosition(); | ||
| if (base.instruction.isLinear()) | ||
| states = jp.replicate(1, linear_steps + 1); | ||
| else if (base.instruction.isFreespace()) | ||
| states = jp.replicate(1, freespace_steps + 1); | ||
| else | ||
| throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); | ||
| } | ||
| else | ||
| { | ||
| Eigen::VectorXd seed = request.env_state.getJointValues(base.manip->getJointNames()); | ||
| tesseract_common::enforceLimits<double>(seed, base.manip->getLimits().joint_limits); | ||
|
|
||
| if (base.instruction.isLinear()) | ||
| states = seed.replicate(1, linear_steps + 1); | ||
| else if (base.instruction.isFreespace()) | ||
| states = seed.replicate(1, freespace_steps + 1); | ||
| else | ||
| throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); | ||
| } | ||
|
|
||
| // Linearly interpolate in cartesian space if linear move | ||
| if (base_instruction.isLinear()) | ||
| { | ||
| Eigen::Isometry3d p1_world; | ||
| if (prev.has_cartesian_waypoint) | ||
| p1_world = prev.extractCartesianPose(); | ||
| else | ||
| p1_world = prev.calcCartesianPose(prev.extractJointPosition()); | ||
|
|
||
| Eigen::Isometry3d p2_world; | ||
| if (base.has_cartesian_waypoint) | ||
| p2_world = base.extractCartesianPose(); | ||
| else | ||
| p2_world = base.calcCartesianPose(base.extractJointPosition()); | ||
|
|
||
| tesseract_common::VectorIsometry3d poses = interpolate(p1_world, p2_world, linear_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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters