Skip to content

Commit

Permalink
Add additional constructor to simple planner interpolation tools
Browse files Browse the repository at this point in the history
  • Loading branch information
Levi Armstrong authored and Levi-Armstrong committed Oct 8, 2024
1 parent 249e2e7 commit 5f5bd9d
Show file tree
Hide file tree
Showing 2 changed files with 28 additions and 6 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,10 @@ struct JointGroupInstructionInfo
const PlannerRequest& request,
const tesseract_common::ManipulatorInfo& manip_info);

JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
const tesseract_environment::Environment& env,
const tesseract_common::ManipulatorInfo& manip_info);

~JointGroupInstructionInfo();
JointGroupInstructionInfo(const JointGroupInstructionInfo&) = delete;
JointGroupInstructionInfo& operator=(const JointGroupInstructionInfo&) = delete;
Expand Down Expand Up @@ -101,6 +105,10 @@ struct KinematicGroupInstructionInfo
const PlannerRequest& request,
const tesseract_common::ManipulatorInfo& manip_info);

KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
const tesseract_environment::Environment& env,
const tesseract_common::ManipulatorInfo& manip_info);

~KinematicGroupInstructionInfo();
KinematicGroupInstructionInfo(const KinematicGroupInstructionInfo&) = delete;
KinematicGroupInstructionInfo& operator=(const KinematicGroupInstructionInfo&) = delete;
Expand Down
26 changes: 20 additions & 6 deletions tesseract_motion_planners/simple/src/interpolation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,13 @@ namespace tesseract_planning
JointGroupInstructionInfo::JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
const PlannerRequest& request,
const tesseract_common::ManipulatorInfo& manip_info)
: JointGroupInstructionInfo(plan_instruction, *request.env, manip_info)
{
}

JointGroupInstructionInfo::JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
const tesseract_environment::Environment& env,
const tesseract_common::ManipulatorInfo& manip_info)
: instruction(plan_instruction)
{
assert(!(manip_info.empty() && plan_instruction.getManipulatorInfo().empty()));
Expand All @@ -74,13 +81,13 @@ JointGroupInstructionInfo::JointGroupInstructionInfo(const MoveInstructionPoly&
throw std::runtime_error("InstructionInfo, working frame is empty!");

// Get Previous Instruction Kinematics
manip = request.env->getJointGroup(mi.manipulator);
manip = env.getJointGroup(mi.manipulator);

// Get Previous Instruction TCP and Working Frame
working_frame = mi.working_frame;
working_frame_transform = request.env_state.link_transforms.at(working_frame);
working_frame_transform = env.getLinkTransform(working_frame);
tcp_frame = mi.tcp_frame;
tcp_offset = request.env->findTCPOffset(mi);
tcp_offset = env.findTCPOffset(mi);

// Get Previous Instruction Waypoint Info
if (plan_instruction.getWaypoint().isStateWaypoint() || plan_instruction.getWaypoint().isJointWaypoint())
Expand Down Expand Up @@ -122,6 +129,13 @@ const Eigen::VectorXd& JointGroupInstructionInfo::extractJointPosition() const
KinematicGroupInstructionInfo::KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
const PlannerRequest& request,
const tesseract_common::ManipulatorInfo& manip_info)
: KinematicGroupInstructionInfo(plan_instruction, *request.env, manip_info)
{
}

KinematicGroupInstructionInfo::KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
const tesseract_environment::Environment& env,
const tesseract_common::ManipulatorInfo& manip_info)
: instruction(plan_instruction)
{
assert(!(manip_info.empty() && plan_instruction.getManipulatorInfo().empty()));
Expand All @@ -138,13 +152,13 @@ KinematicGroupInstructionInfo::KinematicGroupInstructionInfo(const MoveInstructi
throw std::runtime_error("InstructionInfo, working frame is empty!");

// Get Previous Instruction Kinematics
manip = request.env->getKinematicGroup(mi.manipulator, mi.manipulator_ik_solver);
manip = env.getKinematicGroup(mi.manipulator, mi.manipulator_ik_solver);

// Get Previous Instruction TCP and Working Frame
working_frame = mi.working_frame;
working_frame_transform = request.env_state.link_transforms.at(working_frame);
working_frame_transform = env.getLinkTransform(working_frame);
tcp_frame = mi.tcp_frame;
tcp_offset = request.env->findTCPOffset(mi);
tcp_offset = env.findTCPOffset(mi);

// Get Previous Instruction Waypoint Info
if (plan_instruction.getWaypoint().isStateWaypoint() || plan_instruction.getWaypoint().isJointWaypoint())
Expand Down

0 comments on commit 5f5bd9d

Please sign in to comment.