Skip to content

Commit 60d2ff2

Browse files
Levi ArmstrongLevi-Armstrong
Levi Armstrong
authored and
Levi-Armstrong
committed
Add generateNaiveSeedGenerator function
1 parent 8b79339 commit 60d2ff2

File tree

2 files changed

+142
-0
lines changed
  • tesseract/tesseract_planning/tesseract_motion_planners

2 files changed

+142
-0
lines changed

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/utils.h

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -623,6 +623,18 @@ inline bool DEPRECATED("Please use overload with CollisionCheckConfig")
623623
config.longest_valid_segment_length = longest_valid_segment_length;
624624
return contactCheckProgram(contacts, manager, state_solver, program, config);
625625
}
626+
627+
/**
628+
* @brief This generates a naive seed for the provided program
629+
* @details This will generate a seed where each plan instruction has a single move instruction associated to it using
630+
* the current state.
631+
* @param composite_instructions The input program
632+
* @param env The environment information
633+
* @return The generated seed
634+
*/
635+
CompositeInstruction generateNaiveSeed(const CompositeInstruction& composite_instructions,
636+
const tesseract_environment::Environment& env);
637+
626638
} // namespace tesseract_planning
627639

628640
#endif // TESSERACT_PLANNING_UTILS_H

‎tesseract/tesseract_planning/tesseract_motion_planners/src/core/utils.cpp

Lines changed: 130 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,4 +40,134 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
4040

4141
namespace tesseract_planning
4242
{
43+
void generateNaiveSeedHelper(CompositeInstruction& composite_instructions,
44+
const tesseract_environment::Environment& env,
45+
const tesseract_environment::EnvState& env_state,
46+
const ManipulatorInfo& manip_info)
47+
{
48+
for (auto& i : composite_instructions)
49+
{
50+
if (isCompositeInstruction(i))
51+
{
52+
generateNaiveSeedHelper(*(i.cast<CompositeInstruction>()), env, env_state, manip_info);
53+
}
54+
else if (isPlanInstruction(i))
55+
{
56+
PlanInstruction* base_instruction = i.cast<PlanInstruction>();
57+
ManipulatorInfo mi = manip_info.getCombined(base_instruction->getManipulatorInfo());
58+
59+
CompositeInstruction ci;
60+
ci.setProfile(base_instruction->getProfile());
61+
ci.setDescription(base_instruction->getDescription());
62+
ci.setManipulatorInfo(base_instruction->getManipulatorInfo());
63+
64+
auto fwd_kin = env.getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);
65+
Eigen::VectorXd jv = env_state.getJointValues(fwd_kin->getJointNames());
66+
67+
// Get move type base on base instruction type
68+
MoveInstructionType move_type;
69+
if (base_instruction->isLinear())
70+
move_type = MoveInstructionType::LINEAR;
71+
else if (base_instruction->isFreespace())
72+
move_type = MoveInstructionType::FREESPACE;
73+
else
74+
throw std::runtime_error("generateNaiveSeed: Unsupported Move Instruction Type!");
75+
76+
if (isStateWaypoint(base_instruction->getWaypoint()))
77+
{
78+
MoveInstruction move_instruction(base_instruction->getWaypoint(), move_type);
79+
move_instruction.setManipulatorInfo(base_instruction->getManipulatorInfo());
80+
move_instruction.setDescription(base_instruction->getDescription());
81+
move_instruction.setProfile(base_instruction->getProfile());
82+
ci.push_back(move_instruction);
83+
}
84+
else if (isJointWaypoint(base_instruction->getWaypoint()))
85+
{
86+
const auto* jwp = base_instruction->getWaypoint().cast<JointWaypoint>();
87+
MoveInstruction move_instruction(StateWaypoint(jwp->joint_names, jwp->waypoint), move_type);
88+
move_instruction.setManipulatorInfo(base_instruction->getManipulatorInfo());
89+
move_instruction.setDescription(base_instruction->getDescription());
90+
move_instruction.setProfile(base_instruction->getProfile());
91+
ci.push_back(move_instruction);
92+
}
93+
else
94+
{
95+
MoveInstruction move_instruction(StateWaypoint(fwd_kin->getJointNames(), jv), move_type);
96+
move_instruction.setManipulatorInfo(base_instruction->getManipulatorInfo());
97+
move_instruction.setDescription(base_instruction->getDescription());
98+
move_instruction.setProfile(base_instruction->getProfile());
99+
ci.push_back(move_instruction);
100+
}
101+
102+
i = ci;
103+
}
104+
}
105+
}
106+
107+
CompositeInstruction generateNaiveSeed(const CompositeInstruction& composite_instructions,
108+
const tesseract_environment::Environment& env)
109+
{
110+
if (!composite_instructions.hasStartInstruction())
111+
throw std::runtime_error("Top most composite instruction is missing start instruction!");
112+
113+
tesseract_environment::EnvState::ConstPtr env_state = env.getCurrentState();
114+
CompositeInstruction seed = composite_instructions;
115+
ManipulatorInfo mi = composite_instructions.getManipulatorInfo();
116+
117+
Waypoint wp = NullWaypoint();
118+
ManipulatorInfo base_mi;
119+
std::string description;
120+
std::string profile;
121+
if (isPlanInstruction(seed.getStartInstruction()))
122+
{
123+
const auto* pi = seed.getStartInstruction().cast<PlanInstruction>();
124+
wp = pi->getWaypoint();
125+
base_mi = pi->getManipulatorInfo();
126+
description = pi->getDescription();
127+
profile = pi->getProfile();
128+
}
129+
else if (isMoveInstruction(seed.getStartInstruction()))
130+
{
131+
const auto* pi = seed.getStartInstruction().cast<MoveInstruction>();
132+
wp = pi->getWaypoint();
133+
base_mi = pi->getManipulatorInfo();
134+
description = pi->getDescription();
135+
profile = pi->getProfile();
136+
}
137+
else
138+
throw std::runtime_error("Top most composite instruction start instruction has invalid waypoint type!");
139+
140+
ManipulatorInfo start_mi = mi.getCombined(base_mi);
141+
auto fwd_kin = env.getManipulatorManager()->getFwdKinematicSolver(start_mi.manipulator);
142+
Eigen::VectorXd jv = env_state->getJointValues(fwd_kin->getJointNames());
143+
144+
if (isStateWaypoint(wp))
145+
{
146+
MoveInstruction move_instruction(wp, MoveInstructionType::START);
147+
move_instruction.setManipulatorInfo(base_mi);
148+
move_instruction.setDescription(description);
149+
move_instruction.setProfile(profile);
150+
seed.setStartInstruction(move_instruction);
151+
}
152+
else if (isJointWaypoint(wp))
153+
{
154+
const auto* jwp = wp.cast<JointWaypoint>();
155+
MoveInstruction move_instruction(StateWaypoint(jwp->joint_names, jwp->waypoint), MoveInstructionType::START);
156+
move_instruction.setManipulatorInfo(base_mi);
157+
move_instruction.setDescription(description);
158+
move_instruction.setProfile(profile);
159+
seed.setStartInstruction(move_instruction);
160+
}
161+
else
162+
{
163+
MoveInstruction move_instruction(StateWaypoint(fwd_kin->getJointNames(), jv), MoveInstructionType::START);
164+
move_instruction.setManipulatorInfo(base_mi);
165+
move_instruction.setDescription(description);
166+
move_instruction.setProfile(profile);
167+
seed.setStartInstruction(move_instruction);
168+
}
169+
170+
generateNaiveSeedHelper(seed, env, *env_state, mi);
171+
return seed;
172+
}
43173
}; // namespace tesseract_planning

0 commit comments

Comments
 (0)