Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Correctly handle cartesian waypoints in SimplePlannerFixedSizeAssignPlanProfile #447

Closed
wants to merge 2 commits into from
Closed

Correctly handle cartesian waypoints in SimplePlannerFixedSizeAssignPlanProfile #447

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
faa0d50
Correctly handle cartesian waypoints in SimplePlannerFixedSizeAssignP…
rjoomen Mar 18, 2024
ee2ff2c
Simplify
rjoomen Mar 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .../tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/**
* @file simple_planner_default_plan_profile.h
* @file simple_planner_fixed_size_assign_plan_profile.h
* @brief
*
* @author Matthew Powelson
Expand Down
81 changes: 43 additions & 38 deletions ...ract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/**
* @file simple_planner_default_plan_profile.cpp
* @file simple_planner_fixed_size_assign_plan_profile.cpp
* @brief
*
* @author Matthew Powelson
Expand All @@ -24,6 +24,7 @@
* limitations under the License.
*/

#include <tesseract_command_language/cartesian_waypoint.h>
#include <tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h>
#include <tesseract_motion_planners/simple/interpolation.h>
#include <tesseract_motion_planners/core/utils.h>
Expand All @@ -46,50 +47,54 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre
KinematicGroupInstructionInfo info1(prev_instruction, request, global_manip_info);
KinematicGroupInstructionInfo info2(base_instruction, request, global_manip_info);

Eigen::MatrixXd states;
if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint)
{
const Eigen::VectorXd& jp = info2.extractJointPosition();
if (info2.instruction.isLinear())
states = jp.replicate(1, linear_steps + 1);
else if (info2.instruction.isFreespace())
states = jp.replicate(1, freespace_steps + 1);
else
throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!");
}
else if (!info1.has_cartesian_waypoint && info2.has_cartesian_waypoint)
{
const Eigen::VectorXd& jp = info1.extractJointPosition();
if (info2.instruction.isLinear())
states = jp.replicate(1, linear_steps + 1);
else if (info2.instruction.isFreespace())
states = jp.replicate(1, freespace_steps + 1);
else
throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!");
}
else if (info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint)
Eigen::VectorXd j2;
if (!info2.has_cartesian_waypoint)
{
const Eigen::VectorXd& jp = info2.extractJointPosition();
if (info2.instruction.isLinear())
states = jp.replicate(1, linear_steps + 1);
else if (info2.instruction.isFreespace())
states = jp.replicate(1, freespace_steps + 1);
else
throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!");
j2 = info2.extractJointPosition();
}
else
{
Eigen::VectorXd seed = request.env_state.getJointValues(info2.manip->getJointNames());
tesseract_common::enforcePositionLimits<double>(seed, info2.manip->getLimits().joint_limits);

if (info2.instruction.isLinear())
states = seed.replicate(1, linear_steps + 1);
else if (info2.instruction.isFreespace())
states = seed.replicate(1, freespace_steps + 1);
// Determine base_instruction joint position and replicate
const auto& base_cwp = info2.instruction.getWaypoint().as<CartesianWaypointPoly>();
if (base_cwp.hasSeed())
{
// Use joint position of cartesian base_instruction
j2 = base_cwp.getSeed().position;
}
else
throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!");
{
if (info1.has_cartesian_waypoint)
{
const auto& prev_cwp = info1.instruction.getWaypoint().as<CartesianWaypointPoly>();
if (prev_cwp.hasSeed())
{
// Use joint position of cartesian prev_instruction as seed
j2 = getClosestJointSolution(info2, prev_cwp.getSeed().position);
}
else
{
// Use current env_state as seed
j2 = getClosestJointSolution(info2, request.env_state.getJointValues(info2.manip->getJointNames()));
}
}
else
{
// Use prev_instruction as seed
j2 = getClosestJointSolution(info2, info1.extractJointPosition());
}
}
tesseract_common::enforcePositionLimits<double>(j2, info2.manip->getLimits().joint_limits);
}

Eigen::MatrixXd states;
// Replicate base_instruction joint position
if (info2.instruction.isLinear())
states = j2.replicate(1, linear_steps + 1);
else if (info2.instruction.isFreespace())
states = j2.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())
{
Expand Down
Loading