Skip to content

Commit 348fb45

Browse files
rjoomenrjoomen
committed
Refactor for consistency/clarity
1 parent 7cac9af commit 348fb45

8 files changed

+94
-93
lines changed

‎tesseract_motion_planners/simple/src/interpolation.cpp‎

Lines changed: 34 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -378,68 +378,70 @@ std::vector<MoveInstructionPoly> interpolateCartCartWaypoint(const KinematicGrou
378378
tesseract_common::enforceLimits<double>(seed, base.manip->getLimits().joint_limits);
379379

380380
std::array<Eigen::VectorXd, 2> sol;
381+
auto& j1 = sol[0];
382+
auto& j2 = sol[1];
381383
const auto& base_cwp = base.instruction.getWaypoint().as<CartesianWaypointPoly>();
382384
const auto& prev_cwp = prev.instruction.getWaypoint().as<CartesianWaypointPoly>();
383385
const bool base_has_seed = base_cwp.hasSeed();
384386
const bool prev_has_seed = prev_cwp.hasSeed();
385387

386388
if (base_has_seed && prev_has_seed)
387389
{
388-
sol[0] = prev_cwp.getSeed().position;
389-
sol[1] = base_cwp.getSeed().position;
390+
j1 = prev_cwp.getSeed().position;
391+
j2 = base_cwp.getSeed().position;
390392
}
391393
else if (!base_has_seed && prev_has_seed)
392394
{
393-
sol[0] = prev_cwp.getSeed().position;
394-
sol[1] = getClosestJointSolution(base, sol[0]);
395+
j1 = prev_cwp.getSeed().position;
396+
j2 = getClosestJointSolution(base, j1);
395397
}
396398
else if (base_has_seed && !prev_has_seed)
397399
{
398-
sol[1] = base_cwp.getSeed().position;
399-
sol[0] = getClosestJointSolution(prev, sol[1]);
400+
j2 = base_cwp.getSeed().position;
401+
j1 = getClosestJointSolution(prev, j2);
400402
}
401403
else
402404
{
403405
sol = getClosestJointSolution(prev, base, seed);
404406
}
405407

406408
Eigen::MatrixXd states;
407-
if (sol[0].size() != 0 && sol[1].size() != 0)
409+
if ((j1.size() != 0) && (j2.size() != 0))
408410
{
409411
if (base.instruction.isLinear())
410412
{
411413
if (linear_steps > 1)
412-
states = interpolate(sol[0], sol[1], linear_steps);
414+
states = interpolate(j1, j2, linear_steps);
413415
else
414-
states = sol[1].replicate(1, 2);
416+
states = j2.replicate(1, 2);
415417
}
416418
else if (base.instruction.isFreespace())
417419
{
418420
if (freespace_steps > 1)
419-
states = interpolate(sol[0], sol[1], freespace_steps);
421+
states = interpolate(j1, j2, freespace_steps);
420422
else
421-
states = sol[1].replicate(1, 2);
423+
states = j2.replicate(1, 2);
422424
}
423425
else
424426
{
425427
throw std::runtime_error("SimplePlannerFixedSizePlanProfile: Unsupported MoveInstructionType!");
426428
}
427429
}
428-
else if (sol[0].size() != 0)
430+
else if (j1.size() != 0)
429431
{
430432
if (base.instruction.isLinear())
431-
states = sol[0].replicate(1, linear_steps + 1);
433+
states = j1.replicate(1, linear_steps + 1);
432434
else if (base.instruction.isFreespace())
433-
states = sol[0].replicate(1, freespace_steps + 1);
435+
states = j1.replicate(1, freespace_steps + 1);
434436
else
435437
throw std::runtime_error("SimplePlannerFixedSizePlanProfile: Unsupported MoveInstructionType!");
436438
}
437-
else if (sol[1].size() != 0)
439+
else if (j2.size() != 0)
438440
{
439441
if (base.instruction.isLinear())
440-
states = sol[1].replicate(1, linear_steps + 1);
442+
states = j2.replicate(1, linear_steps + 1);
441443
else if (base.instruction.isFreespace())
442-
states = sol[1].replicate(1, freespace_steps + 1);
444+
states = j2.replicate(1, freespace_steps + 1);
443445
else
444446
throw std::runtime_error("SimplePlannerFixedSizePlanProfile: Unsupported MoveInstructionType!");
445447
}
@@ -681,34 +683,36 @@ std::vector<MoveInstructionPoly> interpolateCartCartWaypoint(const KinematicGrou
681683
int steps = std::max(trans_steps, rot_steps);
682684

683685
std::array<Eigen::VectorXd, 2> sol;
686+
auto& j1 = sol[0];
687+
auto& j2 = sol[1];
684688
const auto& base_cwp = base.instruction.getWaypoint().as<CartesianWaypointPoly>();
685689
const auto& prev_cwp = prev.instruction.getWaypoint().as<CartesianWaypointPoly>();
686690
const bool base_has_seed = base_cwp.hasSeed();
687691
const bool prev_has_seed = prev_cwp.hasSeed();
688692
if (base_has_seed && prev_has_seed)
689693
{
690-
sol[0] = prev_cwp.getSeed().position;
691-
sol[1] = base_cwp.getSeed().position;
694+
j1 = prev_cwp.getSeed().position;
695+
j2 = base_cwp.getSeed().position;
692696
}
693697
else if (!base_has_seed && prev_has_seed)
694698
{
695-
sol[0] = prev_cwp.getSeed().position;
696-
sol[1] = getClosestJointSolution(base, sol[0]);
699+
j1 = prev_cwp.getSeed().position;
700+
j2 = getClosestJointSolution(base, j1);
697701
}
698702
else if (base_has_seed && !prev_has_seed)
699703
{
700-
sol[1] = base_cwp.getSeed().position;
701-
sol[0] = getClosestJointSolution(prev, sol[1]);
704+
j2 = base_cwp.getSeed().position;
705+
j1 = getClosestJointSolution(prev, j2);
702706
}
703707
else
704708
{
705709
sol = getClosestJointSolution(prev, base, seed);
706710
}
707711

708712
Eigen::MatrixXd states;
709-
if (sol[0].size() != 0 && sol[1].size() != 0)
713+
if ((j1.size() != 0) && (j2.size() != 0))
710714
{
711-
double joint_dist = (sol[1] - sol[0]).norm();
715+
double joint_dist = (j2 - j1).norm();
712716
int state_steps = int(joint_dist / state_lvs_length) + 1;
713717
steps = std::max(steps, state_steps);
714718

@@ -717,23 +721,23 @@ std::vector<MoveInstructionPoly> interpolateCartCartWaypoint(const KinematicGrou
717721
steps = std::min(steps, max_steps);
718722

719723
// Interpolate
720-
states = interpolate(sol[0], sol[1], steps);
724+
states = interpolate(j1, j2, steps);
721725
}
722-
else if (sol[0].size() != 0)
726+
else if (j1.size() != 0)
723727
{
724728
// Check min steps requirement
725729
steps = std::max(steps, min_steps);
726730

727731
// Interpolate
728-
states = sol[0].replicate(1, steps + 1);
732+
states = j1.replicate(1, steps + 1);
729733
}
730-
else if (sol[1].size() != 0)
734+
else if (j2.size() != 0)
731735
{
732736
// Check min steps requirement
733737
steps = std::max(steps, min_steps);
734738

735739
// Interpolate
736-
states = sol[1].replicate(1, steps + 1);
740+
states = j2.replicate(1, steps + 1);
737741
}
738742
else
739743
{

‎tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp‎

Lines changed: 11 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -33,9 +33,10 @@
3333
#include <tesseract_common/kinematic_limits.h>
3434
#include <tesseract_environment/environment.h>
3535
#include <tesseract_kinematics/core/kinematic_group.h>
36-
3736
#include <tesseract_command_language/poly/move_instruction_poly.h>
3837

38+
#include <boost/serialization/nvp.hpp>
39+
3940
namespace tesseract_planning
4041
{
4142
SimplePlannerFixedSizeAssignNoIKPlanProfile::SimplePlannerFixedSizeAssignNoIKPlanProfile(int freespace_steps,
@@ -58,31 +59,31 @@ std::vector<MoveInstructionPoly> SimplePlannerFixedSizeAssignNoIKPlanProfile::ge
5859
Eigen::MatrixXd states;
5960
if (!prev.has_cartesian_waypoint && !base.has_cartesian_waypoint)
6061
{
61-
const Eigen::VectorXd& jp = base.extractJointPosition();
62+
const Eigen::VectorXd& j2 = base.extractJointPosition();
6263
if (base.instruction.isLinear())
63-
states = jp.replicate(1, linear_steps + 1);
64+
states = j2.replicate(1, linear_steps + 1);
6465
else if (base.instruction.isFreespace())
65-
states = jp.replicate(1, freespace_steps + 1);
66+
states = j2.replicate(1, freespace_steps + 1);
6667
else
6768
throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!");
6869
}
6970
else if (!prev.has_cartesian_waypoint && base.has_cartesian_waypoint)
7071
{
71-
const Eigen::VectorXd& jp = prev.extractJointPosition();
72+
const Eigen::VectorXd& j1 = prev.extractJointPosition();
7273
if (base.instruction.isLinear())
73-
states = jp.replicate(1, linear_steps + 1);
74+
states = j1.replicate(1, linear_steps + 1);
7475
else if (base.instruction.isFreespace())
75-
states = jp.replicate(1, freespace_steps + 1);
76+
states = j1.replicate(1, freespace_steps + 1);
7677
else
7778
throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!");
7879
}
7980
else if (prev.has_cartesian_waypoint && !base.has_cartesian_waypoint)
8081
{
81-
const Eigen::VectorXd& jp = base.extractJointPosition();
82+
const Eigen::VectorXd& j2 = base.extractJointPosition();
8283
if (base.instruction.isLinear())
83-
states = jp.replicate(1, linear_steps + 1);
84+
states = j2.replicate(1, linear_steps + 1);
8485
else if (base.instruction.isFreespace())
85-
states = jp.replicate(1, freespace_steps + 1);
86+
states = j2.replicate(1, freespace_steps + 1);
8687
else
8788
throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!");
8889
}

‎tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,6 @@
2424
* limitations under the License.
2525
*/
2626

27-
#include <tesseract_command_language/cartesian_waypoint.h>
2827
#include <tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h>
2928
#include <tesseract_motion_planners/simple/interpolation.h>
3029
#include <tesseract_motion_planners/core/types.h>
@@ -36,7 +35,6 @@
3635
#include <tesseract_kinematics/core/kinematic_group.h>
3736
#include <tesseract_command_language/poly/move_instruction_poly.h>
3837

39-
#include <boost/serialization/base_object.hpp>
4038
#include <boost/serialization/nvp.hpp>
4139

4240
namespace tesseract_planning
@@ -124,7 +122,7 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre
124122
for (auto& pose : poses)
125123
pose = base.working_frame_transform.inverse() * pose;
126124

127-
assert(static_cast<Eigen::Index>(poses.size()) == states.cols());
125+
assert(poses.size() == states.cols());
128126
return getInterpolatedInstructions(poses, base.manip->getJointNames(), states, base.instruction);
129127
}
130128

@@ -143,4 +141,4 @@ void SimplePlannerFixedSizeAssignPlanProfile::serialize(Archive& ar, const unsig
143141

144142
#include <tesseract_common/serialization.h>
145143
TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile)
146-
BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile)
144+
BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile)

‎tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp‎

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,6 @@
3333
#include <tesseract_environment/environment.h>
3434
#include <tesseract_command_language/poly/move_instruction_poly.h>
3535

36-
#include <boost/serialization/base_object.hpp>
3736
#include <boost/serialization/nvp.hpp>
3837

3938
namespace tesseract_planning
@@ -51,19 +50,19 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst
5150
const std::shared_ptr<const tesseract_environment::Environment>& env,
5251
const tesseract_common::ManipulatorInfo& global_manip_info) const
5352
{
54-
KinematicGroupInstructionInfo info1(prev_instruction, *env, global_manip_info);
55-
KinematicGroupInstructionInfo info2(base_instruction, *env, global_manip_info);
53+
KinematicGroupInstructionInfo prev(prev_instruction, *env, global_manip_info);
54+
KinematicGroupInstructionInfo base(base_instruction, *env, global_manip_info);
5655

57-
if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint)
58-
return interpolateJointJointWaypoint(info1, info2, linear_steps, freespace_steps);
56+
if (!prev.has_cartesian_waypoint && !base.has_cartesian_waypoint)
57+
return interpolateJointJointWaypoint(prev, base, linear_steps, freespace_steps);
5958

60-
if (!info1.has_cartesian_waypoint && info2.has_cartesian_waypoint)
61-
return interpolateJointCartWaypoint(info1, info2, linear_steps, freespace_steps);
59+
if (!prev.has_cartesian_waypoint && base.has_cartesian_waypoint)
60+
return interpolateJointCartWaypoint(prev, base, linear_steps, freespace_steps);
6261

63-
if (info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint)
64-
return interpolateCartJointWaypoint(info1, info2, linear_steps, freespace_steps);
62+
if (prev.has_cartesian_waypoint && !base.has_cartesian_waypoint)
63+
return interpolateCartJointWaypoint(prev, base, linear_steps, freespace_steps);
6564

66-
return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, env->getState());
65+
return interpolateCartCartWaypoint(prev, base, linear_steps, freespace_steps, env->getState());
6766
}
6867

6968
template <class Archive>

‎tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp‎

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -24,15 +24,17 @@
2424
* limitations under the License.
2525
*/
2626

27+
#include <tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h>
2728
#include <tesseract_command_language/cartesian_waypoint.h>
2829
#include <tesseract_command_language/poly/move_instruction_poly.h>
2930
#include <tesseract_environment/environment.h>
3031
#include <tesseract_kinematics/core/kinematic_group.h>
31-
#include <tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_plan_profile.h>
3232
#include <tesseract_motion_planners/simple/interpolation.h>
3333
#include <tesseract_motion_planners/core/utils.h>
3434
#include <tesseract_motion_planners/core/types.h>
3535

36+
#include <boost/serialization/nvp.hpp>
37+
3638
namespace tesseract_planning
3739
{
3840
SimplePlannerLVSAssignNoIKPlanProfile::SimplePlannerLVSAssignNoIKPlanProfile(
@@ -62,40 +64,34 @@ SimplePlannerLVSAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_
6264

6365
Eigen::VectorXd j1;
6466
Eigen::Isometry3d p1_world;
65-
bool has_j1 = false;
6667
if (prev.has_cartesian_waypoint)
6768
{
6869
p1_world = prev.extractCartesianPose();
6970
if (prev.instruction.getWaypoint().as<CartesianWaypointPoly>().hasSeed())
7071
{
7172
j1 = prev.instruction.getWaypoint().as<CartesianWaypointPoly>().getSeed().position;
72-
has_j1 = true;
7373
}
7474
}
7575
else
7676
{
7777
j1 = prev.extractJointPosition();
7878
p1_world = prev.calcCartesianPose(j1);
79-
has_j1 = true;
8079
}
8180

8281
Eigen::VectorXd j2;
8382
Eigen::Isometry3d p2_world;
84-
bool has_j2 = false;
8583
if (base.has_cartesian_waypoint)
8684
{
8785
p2_world = base.extractCartesianPose();
8886
if (base.instruction.getWaypoint().as<CartesianWaypointPoly>().hasSeed())
8987
{
9088
j2 = base.instruction.getWaypoint().as<CartesianWaypointPoly>().getSeed().position;
91-
has_j2 = true;
9289
}
9390
}
9491
else
9592
{
9693
j2 = base.extractJointPosition();
9794
p2_world = base.calcCartesianPose(j2);
98-
has_j2 = true;
9995
}
10096

10197
double trans_dist = (p2_world.translation() - p1_world.translation()).norm();
@@ -105,7 +101,7 @@ SimplePlannerLVSAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_
105101
int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1;
106102

107103
int steps = std::max(trans_steps, rot_steps);
108-
if (has_j1 && has_j2)
104+
if ((j1.size() != 0) && (j2.size() != 0))
109105
{
110106
double joint_dist = (j2 - j1).norm();
111107
int joint_steps = int(joint_dist / state_longest_valid_segment_length) + 1;
@@ -115,16 +111,19 @@ SimplePlannerLVSAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_
115111
steps = std::min(steps, max_steps);
116112

117113
Eigen::MatrixXd states;
118-
if (has_j2)
114+
if (j2.size() != 0)
119115
{
116+
// Replicate base_instruction joint position
120117
states = j2.replicate(1, steps + 1);
121118
}
122-
else if (has_j1)
119+
else if (j1.size() != 0)
123120
{
121+
// Replicate prev_instruction joint position
124122
states = j1.replicate(1, steps + 1);
125123
}
126124
else
127125
{
126+
// Replicate current joint position
128127
Eigen::VectorXd seed = env->getCurrentJointValues(base.manip->getJointNames());
129128
tesseract_common::enforceLimits<double>(seed, base.manip->getLimits().joint_limits);
130129
states = seed.replicate(1, steps + 1);

‎tesseract_motion_planners/simple/src/profile/simple_planner_lvs_assign_plan_profile.cpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,15 +24,17 @@
2424
* limitations under the License.
2525
*/
2626

27+
#include <tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h>
2728
#include <tesseract_command_language/cartesian_waypoint.h>
2829
#include <tesseract_command_language/poly/move_instruction_poly.h>
2930
#include <tesseract_environment/environment.h>
3031
#include <tesseract_kinematics/core/kinematic_group.h>
31-
#include <tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_plan_profile.h>
3232
#include <tesseract_motion_planners/simple/interpolation.h>
3333
#include <tesseract_motion_planners/core/utils.h>
3434
#include <tesseract_motion_planners/core/types.h>
3535

36+
#include <boost/serialization/nvp.hpp>
37+
3638
namespace tesseract_planning
3739
{
3840
SimplePlannerLVSAssignPlanProfile::SimplePlannerLVSAssignPlanProfile(double state_longest_valid_segment_length,

0 commit comments

Comments
 (0)