Skip to content

Commit e26d475

Browse files
mpowelsonLevi-Armstrong
mpowelson
authored and
Levi-Armstrong
committed
Fix SimplePlanner step generators to correctly set profile
1 parent d346e02 commit e26d475

File tree

7 files changed

+67
-31
lines changed

7 files changed

+67
-31
lines changed

‎tesseract/tesseract_planning/tesseract_motion_planners/src/simple/simple_motion_planner.cpp

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -124,9 +124,8 @@ tesseract_common::StatusCode SimpleMotionPlanner::solve(const PlannerRequest& re
124124
return response.status;
125125
}
126126

127-
// Set start instruction and Manipulator Information
127+
// Set start instruction
128128
seed.setStartInstruction(start_instruction);
129-
seed.setManipulatorInfo(request.instructions.getManipulatorInfo());
130129

131130
// Fill out the response
132131
response.results = seed;
@@ -191,7 +190,7 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp
191190
const PlannerRequest& request) const
192191
{
193192
Waypoint start_waypoint = initial_start_waypoint;
194-
CompositeInstruction seed;
193+
CompositeInstruction seed(instructions.getProfile(), instructions.getOrder(), instructions.getManipulatorInfo());
195194
for (const auto& instruction : instructions)
196195
{
197196
if (isCompositeInstruction(instruction))

‎tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/fixed_size_assign_position.cpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,7 @@ CompositeInstruction fixedSizeAssignStateWaypoint(const Eigen::Ref<const Eigen::
6666
MoveInstruction move_instruction(StateWaypoint(fwd_kin->getJointNames(), position), mv_type);
6767
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
6868
move_instruction.setDescription(base_instruction.getDescription());
69+
move_instruction.setProfile(base_instruction.getProfile());
6970
composite.push_back(move_instruction);
7071
}
7172

‎tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/fixed_size_interpolation.cpp

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,7 @@ CompositeInstruction fixedSizeInterpolateStateWaypoint(const JointWaypoint& star
5959
MoveInstruction move_instruction(StateWaypoint(start.joint_names, states.col(i)), MoveInstructionType::FREESPACE);
6060
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
6161
move_instruction.setDescription(base_instruction.getDescription());
62+
move_instruction.setProfile(base_instruction.getProfile());
6263
composite.push_back(move_instruction);
6364
}
6465
return composite;
@@ -121,6 +122,7 @@ CompositeInstruction fixedSizeInterpolateStateWaypoint(const JointWaypoint& star
121122
MoveInstruction move_instruction(StateWaypoint(start.joint_names, states.col(i)), MoveInstructionType::FREESPACE);
122123
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
123124
move_instruction.setDescription(base_instruction.getDescription());
125+
move_instruction.setProfile(base_instruction.getProfile());
124126
composite.push_back(move_instruction);
125127
}
126128
return composite;
@@ -183,6 +185,7 @@ CompositeInstruction fixedSizeInterpolateStateWaypoint(const CartesianWaypoint&
183185
MoveInstruction move_instruction(StateWaypoint(end.joint_names, states.col(i)), MoveInstructionType::FREESPACE);
184186
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
185187
move_instruction.setDescription(base_instruction.getDescription());
188+
move_instruction.setProfile(base_instruction.getProfile());
186189
composite.push_back(move_instruction);
187190
}
188191

@@ -257,6 +260,7 @@ CompositeInstruction fixedSizeInterpolateStateWaypoint(const CartesianWaypoint&
257260
MoveInstructionType::FREESPACE);
258261
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
259262
move_instruction.setDescription(base_instruction.getDescription());
263+
move_instruction.setProfile(base_instruction.getProfile());
260264
composite.push_back(move_instruction);
261265
}
262266

@@ -303,6 +307,7 @@ CompositeInstruction fixedSizeInterpolateCartStateWaypoint(const JointWaypoint&
303307
tesseract_planning::MoveInstruction move_instruction(CartesianWaypoint(poses[p]), MoveInstructionType::LINEAR);
304308
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
305309
move_instruction.setDescription(base_instruction.getDescription());
310+
move_instruction.setProfile(base_instruction.getProfile());
306311
composite.push_back(move_instruction);
307312
}
308313

@@ -346,6 +351,7 @@ CompositeInstruction fixedSizeInterpolateCartStateWaypoint(const JointWaypoint&
346351
tesseract_planning::MoveInstruction move_instruction(CartesianWaypoint(poses[p]), MoveInstructionType::LINEAR);
347352
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
348353
move_instruction.setDescription(base_instruction.getDescription());
354+
move_instruction.setProfile(base_instruction.getProfile());
349355
composite.push_back(move_instruction);
350356
}
351357

@@ -389,6 +395,7 @@ CompositeInstruction fixedSizeInterpolateCartStateWaypoint(const CartesianWaypoi
389395
tesseract_planning::MoveInstruction move_instruction(CartesianWaypoint(poses[p]), MoveInstructionType::LINEAR);
390396
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
391397
move_instruction.setDescription(base_instruction.getDescription());
398+
move_instruction.setProfile(base_instruction.getProfile());
392399
composite.push_back(move_instruction);
393400
}
394401

@@ -416,6 +423,7 @@ CompositeInstruction fixedSizeInterpolateCartStateWaypoint(const CartesianWaypoi
416423
tesseract_planning::MoveInstruction move_instruction(CartesianWaypoint(poses[p]), MoveInstructionType::LINEAR);
417424
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
418425
move_instruction.setDescription(base_instruction.getDescription());
426+
move_instruction.setProfile(base_instruction.getProfile());
419427
composite.push_back(move_instruction);
420428
}
421429
return composite;

‎tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/lvs_interpolation.cpp

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const JointWaypoint& start,
9797
MoveInstruction move_instruction(StateWaypoint(start.joint_names, states.col(i)), MoveInstructionType::LINEAR);
9898
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
9999
move_instruction.setDescription(base_instruction.getDescription());
100+
move_instruction.setProfile(base_instruction.getProfile());
100101
composite.push_back(move_instruction);
101102
}
102103
}
@@ -117,6 +118,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const JointWaypoint& start,
117118
MoveInstruction move_instruction(StateWaypoint(start.joint_names, states.col(i)), MoveInstructionType::FREESPACE);
118119
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
119120
move_instruction.setDescription(base_instruction.getDescription());
121+
move_instruction.setProfile(base_instruction.getProfile());
120122
composite.push_back(move_instruction);
121123
}
122124
}
@@ -209,6 +211,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const JointWaypoint& start,
209211
MoveInstruction move_instruction(StateWaypoint(start.joint_names, states.col(i)), MoveInstructionType::LINEAR);
210212
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
211213
move_instruction.setDescription(base_instruction.getDescription());
214+
move_instruction.setProfile(base_instruction.getProfile());
212215
composite.push_back(move_instruction);
213216
}
214217
}
@@ -229,6 +232,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const JointWaypoint& start,
229232
MoveInstruction move_instruction(StateWaypoint(start.joint_names, states.col(i)), MoveInstructionType::FREESPACE);
230233
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
231234
move_instruction.setDescription(base_instruction.getDescription());
235+
move_instruction.setProfile(base_instruction.getProfile());
232236
composite.push_back(move_instruction);
233237
}
234238
}
@@ -321,6 +325,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const CartesianWaypoint& start,
321325
MoveInstruction move_instruction(StateWaypoint(end.joint_names, states.col(i)), MoveInstructionType::LINEAR);
322326
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
323327
move_instruction.setDescription(base_instruction.getDescription());
328+
move_instruction.setProfile(base_instruction.getProfile());
324329
composite.push_back(move_instruction);
325330
}
326331
}
@@ -341,6 +346,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const CartesianWaypoint& start,
341346
MoveInstruction move_instruction(StateWaypoint(end.joint_names, states.col(i)), MoveInstructionType::FREESPACE);
342347
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
343348
move_instruction.setDescription(base_instruction.getDescription());
349+
move_instruction.setProfile(base_instruction.getProfile());
344350
composite.push_back(move_instruction);
345351
}
346352
}
@@ -437,6 +443,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const CartesianWaypoint& start,
437443
MoveInstructionType::LINEAR);
438444
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
439445
move_instruction.setDescription(base_instruction.getDescription());
446+
move_instruction.setProfile(base_instruction.getProfile());
440447
composite.push_back(move_instruction);
441448
}
442449
}
@@ -458,6 +465,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const CartesianWaypoint& start,
458465
MoveInstructionType::FREESPACE);
459466
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
460467
move_instruction.setDescription(base_instruction.getDescription());
468+
move_instruction.setProfile(base_instruction.getProfile());
461469
composite.push_back(move_instruction);
462470
}
463471
}
@@ -517,6 +525,7 @@ CompositeInstruction LVSInterpolateCartStateWaypoint(const JointWaypoint& start,
517525
tesseract_planning::MoveInstruction move_instruction(CartesianWaypoint(pose), MoveInstructionType::LINEAR);
518526
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
519527
move_instruction.setDescription(base_instruction.getDescription());
528+
move_instruction.setProfile(base_instruction.getProfile());
520529
composite.push_back(move_instruction);
521530
}
522531

@@ -573,6 +582,7 @@ CompositeInstruction LVSInterpolateCartStateWaypoint(const JointWaypoint& start,
573582
tesseract_planning::MoveInstruction move_instruction(CartesianWaypoint(pose), MoveInstructionType::LINEAR);
574583
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
575584
move_instruction.setDescription(base_instruction.getDescription());
585+
move_instruction.setProfile(base_instruction.getProfile());
576586
composite.push_back(move_instruction);
577587
}
578588

@@ -629,6 +639,7 @@ CompositeInstruction LVSInterpolateCartStateWaypoint(const CartesianWaypoint& st
629639
tesseract_planning::MoveInstruction move_instruction(CartesianWaypoint(pose), MoveInstructionType::LINEAR);
630640
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
631641
move_instruction.setDescription(base_instruction.getDescription());
642+
move_instruction.setProfile(base_instruction.getProfile());
632643
composite.push_back(move_instruction);
633644
}
634645

@@ -668,6 +679,7 @@ CompositeInstruction LVSInterpolateCartStateWaypoint(const CartesianWaypoint& st
668679
tesseract_planning::MoveInstruction move_instruction(CartesianWaypoint(pose), MoveInstructionType::LINEAR);
669680
move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo());
670681
move_instruction.setDescription(base_instruction.getDescription());
682+
move_instruction.setProfile(base_instruction.getProfile());
671683
composite.push_back(move_instruction);
672684
}
673685
return composite;

‎tesseract/tesseract_planning/tesseract_motion_planners/test/simple_planner_fixed_size_assign_position.cpp

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -92,7 +92,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, Eigen_AssignJo
9292
request.env_state = tesseract_ptr_->getEnvironment()->getCurrentState();
9393
Eigen::VectorXd vec1 = Eigen::VectorXd::Zero(7);
9494
JointWaypoint wp1(joint_names_, vec1);
95-
PlanInstruction instr(wp1, PlanInstructionType::FREESPACE, "DEFAULT", manip_info_);
95+
PlanInstruction instr(wp1, PlanInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
9696
auto composite = fixedSizeAssignStateWaypoint(vec1, instr, request, ManipulatorInfo(), 10);
9797
EXPECT_EQ(composite.size(), 10);
9898
for (const auto& c : composite)
@@ -101,6 +101,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, Eigen_AssignJo
101101
EXPECT_TRUE(isStateWaypoint(c.cast_const<MoveInstruction>()->getWaypoint()));
102102
const auto* mi = c.cast_const<MoveInstruction>();
103103
EXPECT_TRUE(wp1.isApprox(mi->getWaypoint().cast_const<StateWaypoint>()->position, 1e-5));
104+
EXPECT_EQ(c.cast_const<MoveInstruction>()->getProfile(), instr.getProfile());
104105
}
105106
}
106107

@@ -111,7 +112,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian
111112
request.env_state = tesseract_ptr_->getEnvironment()->getCurrentState();
112113
JointWaypoint wp1(joint_names_, Eigen::VectorXd::Zero(7));
113114
CartesianWaypoint wp2 = Eigen::Isometry3d::Identity();
114-
PlanInstruction instr(wp1, PlanInstructionType::FREESPACE, "DEFAULT", manip_info_);
115+
PlanInstruction instr(wp1, PlanInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
115116
auto composite = fixedSizeAssignStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 10);
116117
EXPECT_EQ(composite.size(), 10);
117118
for (const auto& c : composite)
@@ -120,6 +121,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian
120121
EXPECT_TRUE(isStateWaypoint(c.cast_const<MoveInstruction>()->getWaypoint()));
121122
const auto* mi = c.cast_const<MoveInstruction>();
122123
EXPECT_TRUE(wp1.isApprox(mi->getWaypoint().cast_const<StateWaypoint>()->position, 1e-5));
124+
EXPECT_EQ(c.cast_const<MoveInstruction>()->getProfile(), instr.getProfile());
123125
}
124126
}
125127

@@ -130,7 +132,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint
130132
request.env_state = tesseract_ptr_->getEnvironment()->getCurrentState();
131133
CartesianWaypoint wp1 = Eigen::Isometry3d::Identity();
132134
JointWaypoint wp2(joint_names_, Eigen::VectorXd::Zero(7));
133-
PlanInstruction instr(wp1, PlanInstructionType::FREESPACE, "DEFAULT", manip_info_);
135+
PlanInstruction instr(wp1, PlanInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
134136
auto composite = fixedSizeAssignStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 10);
135137
EXPECT_EQ(composite.size(), 10);
136138
for (const auto& c : composite)
@@ -139,6 +141,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint
139141
EXPECT_TRUE(isStateWaypoint(c.cast_const<MoveInstruction>()->getWaypoint()));
140142
const auto* mi = c.cast_const<MoveInstruction>();
141143
EXPECT_TRUE(wp2.isApprox(mi->getWaypoint().cast_const<StateWaypoint>()->position, 1e-5));
144+
EXPECT_EQ(c.cast_const<MoveInstruction>()->getProfile(), instr.getProfile());
142145
}
143146
}
144147

@@ -149,7 +152,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCarte
149152
request.env_state = tesseract_ptr_->getEnvironment()->getCurrentState();
150153
CartesianWaypoint wp1 = Eigen::Isometry3d::Identity();
151154
CartesianWaypoint wp2 = Eigen::Isometry3d::Identity();
152-
PlanInstruction instr(wp1, PlanInstructionType::FREESPACE, "DEFAULT", manip_info_);
155+
PlanInstruction instr(wp1, PlanInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
153156
auto composite = fixedSizeAssignStateWaypoint(wp1, wp2, instr, request, manip_info_, 10);
154157
auto fwd_kin = tesseract_ptr_->getManipulatorManager()->getFwdKinematicSolver(manip_info_.manipulator);
155158
Eigen::VectorXd position = request.env_state->getJointValues(fwd_kin->getJointNames());
@@ -160,6 +163,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCarte
160163
EXPECT_TRUE(isStateWaypoint(c.cast_const<MoveInstruction>()->getWaypoint()));
161164
const auto* mi = c.cast_const<MoveInstruction>();
162165
EXPECT_TRUE(position.isApprox(mi->getWaypoint().cast_const<StateWaypoint>()->position, 1e-5));
166+
EXPECT_EQ(c.cast_const<MoveInstruction>()->getProfile(), instr.getProfile());
163167
}
164168
}
165169

0 commit comments

Comments
 (0)