Skip to content

Commit 3ddf97b

Browse files
mpowelsonmpowelson
committed
Add tolerances to joint/cartesian waypoints
1 parent ef6c868 commit 3ddf97b

File tree

8 files changed

+96
-12
lines changed

8 files changed

+96
-12
lines changed

‎tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -264,6 +264,25 @@ class CartesianWaypoint
264264

265265
/** @brief The Cartesian Waypoint */
266266
Eigen::Isometry3d waypoint;
267+
/** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */
268+
Eigen::VectorXd lower_tolerance;
269+
/** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/
270+
Eigen::VectorXd upper_tolerance;
271+
272+
bool isToleranced() const
273+
{
274+
// Check if they are empty
275+
if (lower_tolerance.size() == 0 || upper_tolerance.size() == 0)
276+
return false;
277+
278+
// Check if they are close to 0
279+
Eigen::VectorXd range = upper_tolerance - lower_tolerance;
280+
double sum = range.sum();
281+
if (sum < 1e-5)
282+
return false;
283+
284+
return true;
285+
}
267286
};
268287

269288
} // namespace tesseract_planning

‎tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h

Lines changed: 20 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -265,13 +265,32 @@ class JointWaypoint
265265
inline operator Eigen::Ref<Eigen::VectorXd>() { return Eigen::Ref<Eigen::VectorXd>(waypoint); }
266266

267267
//////////////////////////////////
268-
// Cartesian Waypoint Container //
268+
// Joint Waypoint Container //
269269
//////////////////////////////////
270270

271271
#endif // SWIG
272272

273273
Eigen::VectorXd waypoint;
274274
std::vector<std::string> joint_names;
275+
/** @brief Joint distance below waypoint that is allowed. Each element should be <= 0 */
276+
Eigen::VectorXd lower_tolerance;
277+
/** @brief Joint distance above waypoint that is allowed. Each element should be >= 0 */
278+
Eigen::VectorXd upper_tolerance;
279+
280+
bool isToleranced() const
281+
{
282+
// Check if they are empty
283+
if (lower_tolerance.size() == 0 || upper_tolerance.size() == 0)
284+
return false;
285+
286+
// Check if they are close to 0
287+
Eigen::VectorXd range = upper_tolerance - lower_tolerance;
288+
double sum = range.sum();
289+
if (sum < 1e-5)
290+
return false;
291+
292+
return true;
293+
}
275294
};
276295
} // namespace tesseract_planning
277296

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3333
#include <Eigen/Geometry>
3434
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3535

36+
#include <tesseract_command_language/cartesian_waypoint.h>
37+
#include <tesseract_command_language/joint_waypoint.h>
3638
#include <tesseract_motion_planners/trajopt/profile/trajopt_profile.h>
3739

3840
#ifdef SWIG
@@ -81,14 +83,14 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile
8183
constraint_error_functions;
8284

8385
void apply(trajopt::ProblemConstructionInfo& pci,
84-
const Eigen::Isometry3d& cartesian_waypoint,
86+
const CartesianWaypoint& cartesian_waypoint,
8587
const Instruction& parent_instruction,
8688
const ManipulatorInfo& manip_info,
8789
const std::vector<std::string>& active_links,
8890
int index) const override;
8991

9092
void apply(trajopt::ProblemConstructionInfo& pci,
91-
const Eigen::VectorXd& joint_waypoint,
93+
const JointWaypoint& joint_waypoint,
9294
const Instruction& parent_instruction,
9395
const ManipulatorInfo& manip_info,
9496
const std::vector<std::string>& active_links,

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -58,14 +58,14 @@ class TrajOptPlanProfile
5858
TrajOptPlanProfile& operator=(TrajOptPlanProfile&&) = default;
5959

6060
virtual void apply(trajopt::ProblemConstructionInfo& pci,
61-
const Eigen::Isometry3d& cartesian_waypoint,
61+
const CartesianWaypoint& cartesian_waypoint,
6262
const Instruction& parent_instruction,
6363
const ManipulatorInfo& manip_info,
6464
const std::vector<std::string>& active_links,
6565
int index) const = 0;
6666

6767
virtual void apply(trajopt::ProblemConstructionInfo& pci,
68-
const Eigen::VectorXd& joint_waypoint,
68+
const JointWaypoint& joint_waypoint,
6969
const Instruction& parent_instruction,
7070
const ManipulatorInfo& manip_info,
7171
const std::vector<std::string>& active_links,

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,14 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
6363
const Eigen::VectorXd& coeffs,
6464
trajopt::TermType type);
6565

66+
trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
67+
const Eigen::VectorXd& lower_tol,
68+
const Eigen::VectorXd& upper_tol,
69+
int index,
70+
const Eigen::VectorXd& coeffs,
71+
trajopt::TermType type);
72+
73+
6674
trajopt::TermInfo::Ptr createCollisionTermInfo(
6775
int start_index,
6876
int end_index,

‎tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -138,8 +138,9 @@ DefaultTrajoptProblemGenerator(const std::string& name,
138138
}
139139
else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
140140
{
141-
const Eigen::VectorXd& position = getJointPosition(start_waypoint);
142-
start_plan_profile->apply(*pci, position, *start_instruction, composite_mi, active_links, index);
141+
JointWaypoint position_wp;
142+
position_wp.waypoint = getJointPosition(start_waypoint);
143+
start_plan_profile->apply(*pci, position_wp, *start_instruction, composite_mi, active_links, index);
143144
}
144145
else
145146
{
@@ -225,7 +226,8 @@ DefaultTrajoptProblemGenerator(const std::string& name,
225226
}
226227
else if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
227228
{
228-
const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
229+
JointWaypoint cur_position;
230+
cur_position.waypoint = getJointPosition(plan_instruction->getWaypoint());
229231
Eigen::Isometry3d cur_pose = Eigen::Isometry3d::Identity();
230232
if (!pci->kin->calcFwdKin(cur_pose, cur_position))
231233
throw std::runtime_error("TrajOptPlannerUniversalConfig: failed to solve forward kinematics!");
@@ -283,7 +285,8 @@ DefaultTrajoptProblemGenerator(const std::string& name,
283285
{
284286
if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
285287
{
286-
const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
288+
JointWaypoint cur_position;
289+
cur_position.waypoint = getJointPosition(plan_instruction->getWaypoint());
287290

288291
// Add intermediate points with path costs and constraints
289292
for (std::size_t s = 0; s < seed_composite->size() - 1; ++s)

‎tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/profile/trajopt_default_plan_profile.cpp

Lines changed: 11 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -99,7 +99,7 @@ TrajOptDefaultPlanProfile::TrajOptDefaultPlanProfile(const tinyxml2::XMLElement&
9999
}
100100
}
101101
void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
102-
const Eigen::Isometry3d& cartesian_waypoint,
102+
const CartesianWaypoint& cartesian_waypoint,
103103
const Instruction& parent_instruction,
104104
const ManipulatorInfo& manip_info,
105105
const std::vector<std::string>& active_links,
@@ -119,6 +119,9 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
119119
auto it = std::find(active_links.begin(), active_links.end(), mi.working_frame);
120120
if (it != active_links.end())
121121
{
122+
if (cartesian_waypoint.isToleranced())
123+
CONSOLE_BRIDGE_logWarn("Toleranced cartesian waypoints are not supported in this version of TrajOpt.");
124+
122125
if (mi.tcp.isExternal())
123126
{
124127
// If external, the part is attached to the robot so working frame is passed as link instead of target frame
@@ -147,13 +150,18 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
147150
}
148151

149152
void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
150-
const Eigen::VectorXd& joint_waypoint,
153+
const JointWaypoint& joint_waypoint,
151154
const Instruction& /*parent_instruction*/,
152155
const ManipulatorInfo& /*manip_info*/,
153156
const std::vector<std::string>& /*active_links*/,
154157
int index) const
155158
{
156-
auto ti = createJointWaypointTermInfo(joint_waypoint, index, joint_coeff, term_type);
159+
trajopt::TermInfo::Ptr ti;
160+
if (joint_waypoint.isToleranced())
161+
ti = createTolerancedJointWaypointTermInfo(
162+
joint_waypoint, joint_waypoint.lower_tolerance, joint_waypoint.upper_tolerance, index, joint_coeff, term_type);
163+
else
164+
ti = createJointWaypointTermInfo(joint_waypoint, index, joint_coeff, term_type);
157165

158166
if (term_type == trajopt::TermType::TT_CNT)
159167
pci.cnt_infos.push_back(ti);

‎tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -138,6 +138,31 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
138138
return joint_info;
139139
}
140140

141+
trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
142+
const Eigen::VectorXd& lower_tol,
143+
const Eigen::VectorXd& upper_tol,
144+
int index,
145+
const Eigen::VectorXd& coeffs,
146+
trajopt::TermType type)
147+
{
148+
auto joint_info = std::make_shared<trajopt::JointPosTermInfo>();
149+
if (coeffs.size() == 1)
150+
joint_info->coeffs = std::vector<double>(static_cast<std::size_t>(j_wp.size()), coeffs(0));
151+
else if (coeffs.size() == j_wp.size())
152+
joint_info->coeffs = std::vector<double>(coeffs.data(), coeffs.data() + coeffs.rows() * coeffs.cols());
153+
154+
joint_info->targets = std::vector<double>(j_wp.data(), j_wp.data() + j_wp.rows() * j_wp.cols());
155+
joint_info->lower_tols = std::vector<double>(lower_tol.data(), lower_tol.data() + lower_tol.rows() * lower_tol.cols());
156+
joint_info->upper_tols = std::vector<double>(upper_tol.data(), upper_tol.data() + upper_tol.rows() * upper_tol.cols());
157+
joint_info->first_step = index;
158+
joint_info->last_step = index;
159+
joint_info->name = "joint_waypoint_" + std::to_string(index);
160+
joint_info->term_type = type;
161+
162+
return joint_info;
163+
}
164+
165+
141166
trajopt::TermInfo::Ptr createCollisionTermInfo(int start_index,
142167
int end_index,
143168
double collision_safety_margin,

0 commit comments

Comments
 (0)