Skip to content

Commit 09c3f78

Browse files
rjoomenrjoomen
authored
Add SimplePlannerLVSAssignPlanProfile, SimplePlannerLVSAssignNoIKPlanProfile, SimplePlannerFixedSizeAssignPlanProfile (with IK), rename SimplePlannerFixedSizeAssignNoIKPlanProfile (#448)
1 parent 105a8a2 commit 09c3f78

26 files changed

Lines changed: 5481 additions & 687 deletions

‎tesseract_motion_planners/simple/CMakeLists.txt‎

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -2,11 +2,14 @@ add_library(
22
${PROJECT_NAME}_simple
33
src/interpolation.cpp
44
src/simple_motion_planner.cpp
5-
src/profile/simple_planner_profile.cpp
6-
src/profile/simple_planner_lvs_move_profile.cpp
7-
src/profile/simple_planner_lvs_no_ik_move_profile.cpp
5+
src/profile/simple_planner_fixed_size_assign_no_ik_move_profile.cpp
86
src/profile/simple_planner_fixed_size_assign_move_profile.cpp
9-
src/profile/simple_planner_fixed_size_move_profile.cpp)
7+
src/profile/simple_planner_fixed_size_move_profile.cpp
8+
src/profile/simple_planner_lvs_assign_no_ik_move_profile.cpp
9+
src/profile/simple_planner_lvs_assign_move_profile.cpp
10+
src/profile/simple_planner_lvs_no_ik_move_profile.cpp
11+
src/profile/simple_planner_lvs_move_profile.cpp
12+
src/profile/simple_planner_profile.cpp)
1013
target_link_libraries(${PROJECT_NAME}_simple PUBLIC ${PROJECT_NAME}_core Boost::boost)
1114
target_compile_options(${PROJECT_NAME}_simple PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE})
1215
target_compile_options(${PROJECT_NAME}_simple PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})

‎tesseract_motion_planners/simple/README.md‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
# SimplePlanner
2+
3+
The SimplePlanner is a simple path interpolator. It is used to generate initial trajectories with a minimum number of waypoints needed for e.g. Trajopt.
4+
5+
## Profiles
6+
7+
Fixed size: fixed number of waypoints between initial waypoints.
8+
LVS: LVS number of waypoints between initial waypoints.
9+
Assign: No interpolation, but simple replication of (base/prev?) waypoints. This will allow Trajopt to evolve organically from the start to the goal.
10+
11+
SimplePlannerFixedSizeAssignNoIKMoveProfile
12+
SimplePlannerFixedSizeAssignMoveProfile
13+
SimplePlannerFixedSizeNoIKMoveProfile does not exist.
14+
SimplePlannerFixedSizeMoveProfile
15+
SimplePlannerLVSAssignNoIKMoveProfile
16+
SimplePlannerLVSAssignMoveProfile
17+
SimplePlannerLVSNoIKMoveProfile
18+
SimplePlannerLVSMoveProfile

‎tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_move_profile.h‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,13 @@
11
/**
2-
* @file simple_planner_default_move_profile.h
2+
* @file simple_planner_fixed_size_assign_move_profile.h
33
* @brief
44
*
5-
* @author Matthew Powelson
6-
* @date July 23, 2020
5+
* @author Roelof Oomen
6+
* @date May 29, 2024
77
* @version TODO
88
* @bug No known bugs
99
*
10-
* @copyright Copyright (c) 2020, Southwest Research Institute
10+
* @copyright Copyright (c) 2024, ROS Industrial Consortium
1111
*
1212
* @par License
1313
* Software License Agreement (Apache License)

‎tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_move_profile.h‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
/**
2+
* @file simple_planner_fixed_size_assign_no_ik_move_profile.h
3+
* @brief
4+
*
5+
* @author Matthew Powelson
6+
* @date July 23, 2020
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2020, Southwest Research Institute
11+
*
12+
* @par License
13+
* Software License Agreement (Apache License)
14+
* @par
15+
* Licensed under the Apache License, Version 2.0 (the "License");
16+
* you may not use this file except in compliance with the License.
17+
* You may obtain a copy of the License at
18+
* http://www.apache.org/licenses/LICENSE-2.0
19+
* @par
20+
* Unless required by applicable law or agreed to in writing, software
21+
* distributed under the License is distributed on an "AS IS" BASIS,
22+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23+
* See the License for the specific language governing permissions and
24+
* limitations under the License.
25+
*/
26+
27+
#ifndef TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_NO_IK_MOVE_PROFILE_H
28+
#define TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_NO_IK_MOVE_PROFILE_H
29+
30+
#include <tesseract_motion_planners/simple/profile/simple_planner_profile.h>
31+
32+
namespace tesseract_planning
33+
{
34+
class SimplePlannerFixedSizeAssignNoIKMoveProfile : public SimplePlannerMoveProfile
35+
{
36+
public:
37+
using Ptr = std::shared_ptr<SimplePlannerFixedSizeAssignNoIKMoveProfile>;
38+
using ConstPtr = std::shared_ptr<const SimplePlannerFixedSizeAssignNoIKMoveProfile>;
39+
40+
/**
41+
* @brief SimplePlannerFixedSizeAssignMoveProfile
42+
* @param freespace_steps The number of steps to use for freespace instruction
43+
* @param linear_steps The number of steps to use for linear instruction
44+
*/
45+
SimplePlannerFixedSizeAssignNoIKMoveProfile(int freespace_steps = 10, int linear_steps = 10);
46+
47+
std::vector<MoveInstructionPoly> generate(const MoveInstructionPoly& prev_instruction,
48+
const MoveInstructionPoly& prev_seed,
49+
const MoveInstructionPoly& base_instruction,
50+
const InstructionPoly& next_instruction,
51+
const std::shared_ptr<const tesseract_environment::Environment>& env,
52+
const tesseract_common::ManipulatorInfo& global_manip_info) const override;
53+
54+
/** @brief The number of steps to use for freespace instruction */
55+
int freespace_steps;
56+
57+
/** @brief The number of steps to use for linear instruction */
58+
int linear_steps;
59+
60+
protected:
61+
friend class boost::serialization::access;
62+
template <class Archive>
63+
void serialize(Archive&, const unsigned int); // NOLINT
64+
};
65+
66+
} // namespace tesseract_planning
67+
68+
BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerFixedSizeAssignNoIKMoveProfile)
69+
70+
#endif // TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_NO_IK_MOVE_PROFILE_H

‎tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_move_profile.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
/**
2-
* @file simple_planner_interpolation_move_profile.h
2+
* @file simple_planner_fixed_size_move_profile.h
33
* @brief
44
*
55
* @author Matthew Powelson

‎tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_move_profile.h‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
/**
2+
* @file simple_planner_lvs_assign_move_profile.h
3+
* @brief
4+
*
5+
* @author Roelof Oomen
6+
* @date March 19, 2024
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2024, ROS Industrial Consortium
11+
*
12+
* @par License
13+
* Software License Agreement (Apache License)
14+
* @par
15+
* Licensed under the Apache License, Version 2.0 (the "License");
16+
* you may not use this file except in compliance with the License.
17+
* You may obtain a copy of the License at
18+
* http://www.apache.org/licenses/LICENSE-2.0
19+
* @par
20+
* Unless required by applicable law or agreed to in writing, software
21+
* distributed under the License is distributed on an "AS IS" BASIS,
22+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23+
* See the License for the specific language governing permissions and
24+
* limitations under the License.
25+
*/
26+
27+
#ifndef TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_MOVE_PROFILE_H
28+
#define TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_MOVE_PROFILE_H
29+
30+
#include <tesseract_common/macros.h>
31+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
32+
#include <Eigen/Geometry>
33+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
34+
35+
#include <tesseract_motion_planners/simple/profile/simple_planner_profile.h>
36+
37+
namespace tesseract_planning
38+
{
39+
class SimplePlannerLVSAssignMoveProfile : public SimplePlannerMoveProfile
40+
{
41+
public:
42+
using Ptr = std::shared_ptr<SimplePlannerLVSAssignMoveProfile>;
43+
using ConstPtr = std::shared_ptr<const SimplePlannerLVSAssignMoveProfile>;
44+
45+
/**
46+
* @brief SimplePlannerFixedSizeAssignMoveProfile
47+
* @param freespace_steps The number of steps to use for freespace instruction
48+
* @param linear_steps The number of steps to use for linear instruction
49+
*/
50+
SimplePlannerLVSAssignMoveProfile(double state_longest_valid_segment_length = 5 * M_PI / 180,
51+
double translation_longest_valid_segment_length = 0.1,
52+
double rotation_longest_valid_segment_length = 5 * M_PI / 180,
53+
int min_steps = 1,
54+
int max_steps = std::numeric_limits<int>::max());
55+
56+
std::vector<MoveInstructionPoly> generate(const MoveInstructionPoly& prev_instruction,
57+
const MoveInstructionPoly& prev_seed,
58+
const MoveInstructionPoly& base_instruction,
59+
const InstructionPoly& next_instruction,
60+
const std::shared_ptr<const tesseract_environment::Environment>& env,
61+
const tesseract_common::ManipulatorInfo& global_manip_info) const override;
62+
63+
/** @brief The maximum joint distance, the norm of changes to all joint positions between successive steps. */
64+
double state_longest_valid_segment_length;
65+
66+
/** @brief The maximum translation distance between successive steps */
67+
double translation_longest_valid_segment_length;
68+
69+
/** @brief The maximum rotational distance between successive steps */
70+
double rotation_longest_valid_segment_length;
71+
72+
/** @brief The minimum number of steps for the plan */
73+
int min_steps;
74+
75+
/** @brief The maximum number of steps for the plan */
76+
int max_steps;
77+
78+
protected:
79+
friend class boost::serialization::access;
80+
template <class Archive>
81+
void serialize(Archive&, const unsigned int); // NOLINT
82+
};
83+
84+
} // namespace tesseract_planning
85+
86+
BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerLVSAssignMoveProfile)
87+
88+
#endif // TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_MOVE_PROFILE_H

‎tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_assign_no_ik_move_profile.h‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
/**
2+
* @file simple_planner_lvs_assign_no_ik_move_profile.h
3+
* @brief
4+
*
5+
* @author Roelof Oomen
6+
* @date May 29, 2024
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2024, ROS Industrial Consortium
11+
*
12+
* @par License
13+
* Software License Agreement (Apache License)
14+
* @par
15+
* Licensed under the Apache License, Version 2.0 (the "License");
16+
* you may not use this file except in compliance with the License.
17+
* You may obtain a copy of the License at
18+
* http://www.apache.org/licenses/LICENSE-2.0
19+
* @par
20+
* Unless required by applicable law or agreed to in writing, software
21+
* distributed under the License is distributed on an "AS IS" BASIS,
22+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23+
* See the License for the specific language governing permissions and
24+
* limitations under the License.
25+
*/
26+
27+
#ifndef TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_NO_IK_MOVE_PROFILE_H
28+
#define TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_NO_IK_MOVE_PROFILE_H
29+
30+
#include <tesseract_common/macros.h>
31+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
32+
#include <Eigen/Geometry>
33+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
34+
35+
#include <tesseract_motion_planners/simple/profile/simple_planner_profile.h>
36+
37+
namespace tesseract_planning
38+
{
39+
class SimplePlannerLVSAssignNoIKMoveProfile : public SimplePlannerMoveProfile
40+
{
41+
public:
42+
using Ptr = std::shared_ptr<SimplePlannerLVSAssignNoIKMoveProfile>;
43+
using ConstPtr = std::shared_ptr<const SimplePlannerLVSAssignNoIKMoveProfile>;
44+
45+
/**
46+
* @brief SimplePlannerFixedSizeAssignMoveProfile
47+
* @param freespace_steps The number of steps to use for freespace instruction
48+
* @param linear_steps The number of steps to use for linear instruction
49+
*/
50+
SimplePlannerLVSAssignNoIKMoveProfile(double state_longest_valid_segment_length = 5 * M_PI / 180,
51+
double translation_longest_valid_segment_length = 0.1,
52+
double rotation_longest_valid_segment_length = 5 * M_PI / 180,
53+
int min_steps = 1,
54+
int max_steps = std::numeric_limits<int>::max());
55+
56+
std::vector<MoveInstructionPoly> generate(const MoveInstructionPoly& prev_instruction,
57+
const MoveInstructionPoly& prev_seed,
58+
const MoveInstructionPoly& base_instruction,
59+
const InstructionPoly& next_instruction,
60+
const std::shared_ptr<const tesseract_environment::Environment>& env,
61+
const tesseract_common::ManipulatorInfo& global_manip_info) const override;
62+
63+
/** @brief The maximum joint distance, the norm of changes to all joint positions between successive steps. */
64+
double state_longest_valid_segment_length;
65+
66+
/** @brief The maximum translation distance between successive steps */
67+
double translation_longest_valid_segment_length;
68+
69+
/** @brief The maximum rotational distance between successive steps */
70+
double rotation_longest_valid_segment_length;
71+
72+
/** @brief The minimum number of steps for the plan */
73+
int min_steps;
74+
75+
/** @brief The maximum number of steps for the plan */
76+
int max_steps;
77+
78+
protected:
79+
friend class boost::serialization::access;
80+
template <class Archive>
81+
void serialize(Archive&, const unsigned int); // NOLINT
82+
};
83+
84+
} // namespace tesseract_planning
85+
86+
BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerLVSAssignNoIKMoveProfile)
87+
88+
#endif // TESSERACT_MOTION_PLANNERS_SIMPLE_PLANNER_LVS_ASSIGN_NO_IK_MOVE_PROFILE_H

0 commit comments

Comments
 (0)