Skip to content

Commit c7175c5

Browse files
mpowelsonmpowelson
committed
Fix cartesian terms
1 parent 2b28121 commit c7175c5

File tree

7 files changed

+130
-308
lines changed

7 files changed

+130
-308
lines changed

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3636

3737
namespace tesseract_planning
3838
{
39-
trajopt::TermInfo::Ptr createCartesianWaypointTermInfo(const Eigen::Isometry3d& c_wp,
39+
trajopt::TermInfo::Ptr createCartesianPositionConstraint(const Eigen::Isometry3d& c_wp,
4040
int index,
4141
std::string working_frame,
4242
Eigen::Isometry3d tcp,

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h

Lines changed: 48 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -39,13 +39,22 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3939

4040
namespace tesseract_planning
4141
{
42-
ifopt::ConstraintSet::Ptr createCartesianWaypointTermInfo(const Eigen::Isometry3d& c_wp,
43-
int index,
44-
std::string working_frame,
45-
const ToolCenterPoint& tcp,
46-
const Eigen::VectorXd& coeffs,
47-
std::string link);
42+
ifopt::ConstraintSet::Ptr createCartesianPositionConstraint(const CartesianWaypoint& cart_waypoint,
43+
trajopt::JointPosition::ConstPtr var,
44+
const trajopt::CartPosKinematicInfo::ConstPtr& kin_info,
45+
const Eigen::VectorXd& coeffs);
46+
47+
bool addCartesianPositionConstraint(std::shared_ptr<ifopt::Problem> nlp,
48+
const CartesianWaypoint& cart_waypoint,
49+
trajopt::JointPosition::ConstPtr var,
50+
const trajopt::CartPosKinematicInfo::ConstPtr& kin_info,
51+
const Eigen::Ref<const Eigen::VectorXd>& coeff);
4852

53+
bool addCartesianPositionSquaredCost(std::shared_ptr<ifopt::Problem> nlp,
54+
const CartesianWaypoint& cart_waypoint,
55+
trajopt::JointPosition::ConstPtr var,
56+
const trajopt::CartPosKinematicInfo::ConstPtr& kin_info,
57+
const Eigen::Ref<const Eigen::VectorXd>& coeff);
4958

5059
ifopt::ConstraintSet::Ptr createJointPositionConstraint(const JointWaypoint& joint_waypoint,
5160
trajopt::JointPosition::ConstPtr var,
@@ -64,8 +73,7 @@ bool addJointPositionSquaredCost(std::shared_ptr<ifopt::Problem> nlp,
6473

6574

6675
ifopt::ConstraintSet::Ptr createCollisionTermInfo(
67-
int start_index,
68-
int end_index,
76+
std::vector<trajopt::JointPosition::ConstPtr> vars,
6977
double collision_safety_margin,
7078
double collision_safety_margin_buffer,
7179
trajopt::CollisionEvaluatorType evaluator_type,
@@ -74,38 +82,38 @@ ifopt::ConstraintSet::Ptr createCollisionTermInfo(
7482
tesseract_collision::ContactTestType contact_test_type = tesseract_collision::ContactTestType::ALL,
7583
double longest_valid_segment_length = 0.5);
7684

77-
//ifopt::ConstraintSet::Ptr createSmoothVelocityTermInfo(int start_index,
78-
// int end_index,
79-
// int n_joints,
80-
// double coeff = 5.0,
81-
// trajopt::TermType type = trajopt::TermType::TT_COST);
82-
83-
//ifopt::ConstraintSet::Ptr createSmoothVelocityTermInfo(int start_index,
84-
// int end_index,
85-
// const Eigen::Ref<const Eigen::VectorXd>& coeff,
86-
// trajopt::TermType type = trajopt::TermType::TT_COST);
87-
88-
//ifopt::ConstraintSet::Ptr createSmoothAccelerationTermInfo(int start_index,
89-
// int end_index,
90-
// int n_joints,
91-
// double coeff = 1.0,
92-
// trajopt::TermType type = trajopt::TermType::TT_COST);
93-
94-
//ifopt::ConstraintSet::Ptr createSmoothAccelerationTermInfo(int start_index,
95-
// int end_index,
96-
// const Eigen::Ref<const Eigen::VectorXd>& coeff,
97-
// trajopt::TermType type = trajopt::TermType::TT_COST);
98-
99-
//ifopt::ConstraintSet::Ptr createSmoothJerkTermInfo(int start_index,
100-
// int end_index,
101-
// int n_joints,
102-
// double coeff = 1.0,
103-
// trajopt::TermType type = trajopt::TermType::TT_COST);
104-
105-
//ifopt::ConstraintSet::Ptr createSmoothJerkTermInfo(int start_index,
106-
// int end_index,
107-
// const Eigen::Ref<const Eigen::VectorXd>& coeff,
108-
// trajopt::TermType type = trajopt::TermType::TT_COST);
85+
ifopt::ConstraintSet::Ptr createSmoothVelocityTermInfo(int start_index,
86+
int end_index,
87+
int n_joints,
88+
double coeff = 5.0,
89+
trajopt::TermType type = trajopt::TermType::TT_COST);
90+
91+
ifopt::ConstraintSet::Ptr createSmoothVelocityTermInfo(int start_index,
92+
int end_index,
93+
const Eigen::Ref<const Eigen::VectorXd>& coeff,
94+
trajopt::TermType type = trajopt::TermType::TT_COST);
95+
96+
ifopt::ConstraintSet::Ptr createSmoothAccelerationTermInfo(int start_index,
97+
int end_index,
98+
int n_joints,
99+
double coeff = 1.0,
100+
trajopt::TermType type = trajopt::TermType::TT_COST);
101+
102+
ifopt::ConstraintSet::Ptr createSmoothAccelerationTermInfo(int start_index,
103+
int end_index,
104+
const Eigen::Ref<const Eigen::VectorXd>& coeff,
105+
trajopt::TermType type = trajopt::TermType::TT_COST);
106+
107+
ifopt::ConstraintSet::Ptr createSmoothJerkTermInfo(int start_index,
108+
int end_index,
109+
int n_joints,
110+
double coeff = 1.0,
111+
trajopt::TermType type = trajopt::TermType::TT_COST);
112+
113+
ifopt::ConstraintSet::Ptr createSmoothJerkTermInfo(int start_index,
114+
int end_index,
115+
const Eigen::Ref<const Eigen::VectorXd>& coeff,
116+
trajopt::TermType type = trajopt::TermType::TT_COST);
109117

110118
//ifopt::ConstraintSet::Ptr createUserDefinedTermInfo(int start_index,
111119
// int end_index,

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -130,7 +130,7 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
130130
if (cartesian_waypoint.isToleranced())
131131
CONSOLE_BRIDGE_logWarn("Toleranced cartesian waypoints are not supported in this version of TrajOpt.");
132132

133-
ti = createCartesianWaypointTermInfo(
133+
ti = createCartesianPositionConstraint(
134134
cartesian_waypoint, index, mi.working_frame, tcp, cartesian_coeff, pci.kin->getTipLinkName(), term_type);
135135
}
136136

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@
2727

2828
namespace tesseract_planning
2929
{
30-
trajopt::TermInfo::Ptr createCartesianWaypointTermInfo(const Eigen::Isometry3d& c_wp,
30+
trajopt::TermInfo::Ptr createCartesianPositionConstraint(const Eigen::Isometry3d& c_wp,
3131
int index,
3232
std::string working_frame,
3333
Eigen::Isometry3d tcp,

0 commit comments

Comments
 (0)