Skip to content

Commit 2b28121

Browse files
mpowelsonmpowelson
committed
building
1 parent e71ff70 commit 2b28121

File tree

7 files changed

+116
-224
lines changed

7 files changed

+116
-224
lines changed

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h

Lines changed: 13 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -33,9 +33,17 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3333
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3434

3535
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h>
36+
#include <tesseract_command_language/joint_waypoint.h>
37+
#include <tesseract_command_language/cartesian_waypoint.h>
3638

3739
namespace tesseract_planning
3840
{
41+
enum class TrajOptIfoptTermType
42+
{
43+
CONSTRAINT,
44+
SQUARED_COST
45+
};
46+
3947
class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
4048
{
4149
public:
@@ -45,40 +53,20 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
4553
TrajOptIfoptDefaultPlanProfile() = default;
4654
TrajOptIfoptDefaultPlanProfile(const tinyxml2::XMLElement& xml_element);
4755

48-
// Eigen::VectorXd cartesian_coeff{ Eigen::VectorXd::Constant(1, 1, 5) };
49-
// Eigen::VectorXd joint_coeff{ Eigen::VectorXd::Constant(1, 1, 5) };
50-
// TrajOptIfopt::TermType term_type{ TrajOptIfopt::TermType::TT_CNT };
56+
Eigen::VectorXd cartesian_coeff{ Eigen::VectorXd::Constant(1, 1, 5) };
57+
Eigen::VectorXd joint_coeff{ Eigen::VectorXd::Constant(1, 1, 5) };
58+
TrajOptIfoptTermType term_type{ TrajOptIfoptTermType::CONSTRAINT };
5159

52-
// /** @brief Error function that is set as a constraint for each timestep.
53-
// *
54-
// * This is a vector of std::tuple<Error Function, Error Function Jacobian, Constraint Type, Coeff>, the error
55-
// * function, constraint type, and coeff is required, but the jacobian is optional (nullptr).
56-
// *
57-
// * Error Function:
58-
// * arg: VectorXd will be all of the joint values for one timestep.
59-
// * return: VectorXd of violations for each joint. Anything != 0 will be a violation
60-
// *
61-
// * Error Function Jacobian:
62-
// * arg: VectorXd will be all of the joint values for one timestep.
63-
// * return: Eigen::MatrixXd that represents the change in the error function with respect to joint values
64-
// *
65-
// * Error Constraint Type
66-
// *
67-
// * Coefficients/Weights
68-
// *
69-
// */
70-
// std::vector<std::tuple<sco::VectorOfVector::func, sco::MatrixOfVector::func, sco::ConstraintType, Eigen::VectorXd>>
71-
// constraint_error_functions;
7260

7361
void apply(TrajOptIfoptProblem& problem,
74-
const Eigen::Isometry3d& cartesian_waypoint,
62+
const CartesianWaypoint& cartesian_waypoint,
7563
const Instruction& parent_instruction,
7664
const ManipulatorInfo& manip_info,
7765
const std::vector<std::string>& active_links,
7866
int index) override;
7967

8068
void apply(TrajOptIfoptProblem& problem,
81-
const Eigen::VectorXd& joint_waypoint,
69+
const JointWaypoint& joint_waypoint,
8270
const Instruction& parent_instruction,
8371
const ManipulatorInfo& manip_info,
8472
const std::vector<std::string>& active_links,

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3737
#include <tesseract_command_language/core/instruction.h>
3838
#include <tesseract_command_language/manipulator_info.h>
3939
#include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h>
40+
#include <tesseract_command_language/joint_waypoint.h>
41+
#include <tesseract_command_language/cartesian_waypoint.h>
4042

4143
namespace tesseract_planning
4244
{
@@ -47,14 +49,14 @@ class TrajOptIfoptPlanProfile
4749
using ConstPtr = std::shared_ptr<const TrajOptIfoptPlanProfile>;
4850

4951
virtual void apply(TrajOptIfoptProblem& problem,
50-
const Eigen::Isometry3d& cartesian_waypoint,
52+
const CartesianWaypoint& cartesian_waypoint,
5153
const Instruction& parent_instruction,
5254
const ManipulatorInfo& manip_info,
5355
const std::vector<std::string>& active_links,
5456
int index) = 0;
5557

5658
virtual void apply(TrajOptIfoptProblem& problem,
57-
const Eigen::VectorXd& joint_waypoint,
59+
const JointWaypoint& joint_waypoint,
5860
const Instruction& parent_instruction,
5961
const ManipulatorInfo& manip_info,
6062
const std::vector<std::string>& active_links,

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3131
#include <ifopt/problem.h>
3232
#include <vector>
3333
#include <memory>
34+
#include <trajopt_ifopt/variable_sets/joint_position_variable.h>
3435
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3536

3637
#include <tesseract/tesseract.h>
@@ -49,6 +50,7 @@ struct TrajOptIfoptProblem
4950
tesseract_kinematics::InverseKinematics::ConstPtr manip_inv_kin;
5051

5152
std::shared_ptr<ifopt::Problem> nlp;
53+
std::vector<trajopt::JointPosition::ConstPtr> vars;
5254
};
5355

5456
} // namespace tesseract_planning

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

Lines changed: 12 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -42,27 +42,25 @@ namespace tesseract_planning
4242
ifopt::ConstraintSet::Ptr createCartesianWaypointTermInfo(const Eigen::Isometry3d& c_wp,
4343
int index,
4444
std::string working_frame,
45-
Eigen::Isometry3d tcp,
45+
const ToolCenterPoint& tcp,
4646
const Eigen::VectorXd& coeffs,
4747
std::string link);
4848

4949

50-
ifopt::ConstraintSet::Ptr createJointPositionConstraint(const Eigen::VectorXd& target,
51-
const std::vector<std::string>& joint_names,
52-
std::vector<trajopt::JointPosition::ConstPtr> vars,
53-
const Eigen::VectorXd& coeffs);
50+
ifopt::ConstraintSet::Ptr createJointPositionConstraint(const JointWaypoint& joint_waypoint,
51+
trajopt::JointPosition::ConstPtr var,
52+
const Eigen::VectorXd& /*coeffs*/);
5453

55-
ifopt::ConstraintSet::Ptr createJointPositionConstraint(std::vector<trajopt::JointPosition::ConstPtr> vars,
56-
const Eigen::Ref<const Eigen::VectorXd>& coeff);
54+
bool addJointPositionConstraint(std::shared_ptr<ifopt::Problem> nlp,
55+
const JointWaypoint& joint_waypoint,
56+
trajopt::JointPosition::ConstPtr var,
57+
const Eigen::Ref<const Eigen::VectorXd>& coeff);
5758

5859

59-
bool addJointPositionConstraint(std::shared_ptr<ifopt::Problem> nlp,
60-
std::vector<trajopt::JointPosition::ConstPtr> vars,
61-
const Eigen::Ref<const Eigen::VectorXd>& coeff);
62-
63-
bool addJointPositionSquaredCost(std::shared_ptr<ifopt::Problem> nlp,
64-
std::vector<trajopt::JointPosition::ConstPtr> vars,
65-
const Eigen::Ref<const Eigen::VectorXd>& coeff);
60+
bool addJointPositionSquaredCost(std::shared_ptr<ifopt::Problem> nlp,
61+
const JointWaypoint& joint_waypoint,
62+
trajopt::JointPosition::ConstPtr var,
63+
const Eigen::Ref<const Eigen::VectorXd>& coeff);
6664

6765

6866
ifopt::ConstraintSet::Ptr createCollisionTermInfo(

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

Lines changed: 9 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -74,7 +74,6 @@ DefaultTrajoptIfoptProblemGenerator(const std::string& name,
7474
// ----------------
7575
// Create the vector of variables to be optimized
7676
Eigen::MatrixX2d joint_limits_eigen = kin->getLimits().joint_limits;
77-
std::vector<trajopt::JointPosition::ConstPtr> vars;
7877

7978
// Create a variable for each instruction in the seed, setting to correct initial state
8079
for (std::size_t i = 0; i < seed_flat.size(); i++)
@@ -85,7 +84,7 @@ DefaultTrajoptIfoptProblemGenerator(const std::string& name,
8584
kin->getJointNames(),
8685
"Joint_Position_" + std::to_string(i));
8786
var->SetBounds(joint_limits_eigen);
88-
vars.push_back(var);
87+
problem->vars.push_back(var);
8988
problem->nlp->AddVariableSet(var);
9089
}
9190

@@ -198,7 +197,9 @@ DefaultTrajoptIfoptProblemGenerator(const std::string& name,
198197
}
199198
else if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
200199
{
201-
const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
200+
const JointWaypoint& cur_position = *plan_instruction->getWaypoint().cast_const<JointWaypoint>();
201+
if (isStateWaypoint(plan_instruction->getWaypoint())); // TODO: Handle this ideally halfway efficiently
202+
202203
Eigen::Isometry3d cur_pose = Eigen::Isometry3d::Identity();
203204
if (!kin->calcFwdKin(cur_pose, cur_position))
204205
throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: failed to solve forward kinematics!");
@@ -247,7 +248,9 @@ DefaultTrajoptIfoptProblemGenerator(const std::string& name,
247248
{
248249
if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
249250
{
250-
const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
251+
// const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint()); // This doesn't keep tolerances or other metadata
252+
const JointWaypoint& cur_position = *plan_instruction->getWaypoint().cast_const<JointWaypoint>();
253+
if (isStateWaypoint(plan_instruction->getWaypoint())); // TODO: Handle this ideally halfway efficiently
251254

252255
// Increment index to account for intermediate points in seed
253256
for (std::size_t s = 0; s < seed_composite->size() - 1; ++s)
@@ -298,11 +301,11 @@ DefaultTrajoptIfoptProblemGenerator(const std::string& name,
298301
// ----------------
299302
profile = getProfileString(request.instructions.getProfile(), name, request.composite_profile_remapping);
300303
TrajOptIfoptCompositeProfile::Ptr cur_composite_profile = getProfile<TrajOptIfoptCompositeProfile>(
301-
profile, composite_profiles, nullptr /*std::make_shared<TrajOptIfoptDefaultCompositeProfile>()*/);
304+
profile, composite_profiles, std::make_shared<TrajOptIfoptDefaultCompositeProfile>());
302305
if (!cur_composite_profile)
303306
throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: Invalid profile");
304307

305-
cur_composite_profile->apply(*problem, 0, static_cast<int>(vars.size()), composite_mi, active_links, fixed_steps);
308+
cur_composite_profile->apply(*problem, 0, static_cast<int>(problem->vars.size()), composite_mi, active_links, fixed_steps);
306309

307310
// ----------------
308311
// Return problem

‎tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.cpp

Lines changed: 48 additions & 144 deletions
Original file line numberDiff line numberDiff line change
@@ -30,152 +30,56 @@
3030
#include <tesseract_command_language/move_instruction.h>
3131
#include <tesseract_command_language/plan_instruction.h>
3232
#include <tesseract_command_language/instruction_type.h>
33+
#include <trajopt_ifopt/trajopt_ifopt.h>
34+
#include <trajopt_ifopt/utils/ifopt_utils.h>
3335

3436
namespace tesseract_planning
3537
{
36-
//TrajOptDefaultPlanProfile::TrajOptDefaultPlanProfile(const tinyxml2::XMLElement& xml_element)
37-
//{
38-
// const tinyxml2::XMLElement* cartesian_coeff_element = xml_element.FirstChildElement("CartesianCoeff");
39-
// const tinyxml2::XMLElement* joint_coeff_element = xml_element.FirstChildElement("JointCoeff");
40-
// const tinyxml2::XMLElement* term_type_element = xml_element.FirstChildElement("Term");
41-
// const tinyxml2::XMLElement* cnt_error_fn_element = xml_element.FirstChildElement("ConstraintErrorFunctions");
42-
43-
// tinyxml2::XMLError status;
44-
45-
// if (cartesian_coeff_element)
46-
// {
47-
// std::vector<std::string> cart_coeff_tokens;
48-
// std::string cart_coeff_string;
49-
// status = tesseract_common::QueryStringText(cartesian_coeff_element, cart_coeff_string);
50-
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
51-
// throw std::runtime_error("TrajoptPlanProfile: Error parsing CartesianCoeff string");
52-
53-
// boost::split(cart_coeff_tokens, cart_coeff_string, boost::is_any_of(" "), boost::token_compress_on);
54-
55-
// if (!tesseract_common::isNumeric(cart_coeff_tokens))
56-
// throw std::runtime_error("TrajoptPlanProfile: CartesianCoeff are not all numeric values.");
57-
58-
// cartesian_coeff.resize(static_cast<long>(cart_coeff_tokens.size()));
59-
// for (std::size_t i = 0; i < cart_coeff_tokens.size(); ++i)
60-
// tesseract_common::toNumeric<double>(cart_coeff_tokens[i], cartesian_coeff[static_cast<long>(i)]);
61-
// }
62-
63-
// if (joint_coeff_element)
64-
// {
65-
// std::vector<std::string> joint_coeff_tokens;
66-
// std::string joint_coeff_string;
67-
// status = tesseract_common::QueryStringText(joint_coeff_element, joint_coeff_string);
68-
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
69-
// throw std::runtime_error("TrajoptPlanProfile: Error parsing JointCoeff string");
70-
71-
// boost::split(joint_coeff_tokens, joint_coeff_string, boost::is_any_of(" "), boost::token_compress_on);
72-
73-
// if (!tesseract_common::isNumeric(joint_coeff_tokens))
74-
// throw std::runtime_error("TrajoptPlanProfile: JointCoeff are not all numeric values.");
75-
76-
// joint_coeff.resize(static_cast<long>(joint_coeff_tokens.size()));
77-
// for (std::size_t i = 0; i < joint_coeff_tokens.size(); ++i)
78-
// tesseract_common::toNumeric<double>(joint_coeff_tokens[i], joint_coeff[static_cast<long>(i)]);
79-
// }
80-
81-
// if (term_type_element)
82-
// {
83-
// int type = static_cast<int>(trajopt::TermType::TT_CNT);
84-
// status = term_type_element->QueryIntAttribute("type", &type);
85-
// if (status != tinyxml2::XML_SUCCESS)
86-
// throw std::runtime_error("TrajoptPlanProfile: Error parsing Term type attribute.");
87-
88-
// term_type = static_cast<trajopt::TermType>(type);
89-
// }
90-
91-
// if (cnt_error_fn_element)
92-
// {
93-
// std::string error_fn_name;
94-
// status = tesseract_common::QueryStringAttribute(cnt_error_fn_element, "type", error_fn_name);
95-
// if (status != tinyxml2::XML_SUCCESS)
96-
// throw std::runtime_error("TrajoptPlanProfile: Error parsing ConstraintErrorFunctions plugin attribute.");
97-
98-
// // TODO: Implement plugin capabilities
99-
// }
100-
//}
101-
//void TrajOptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem,
102-
// const Eigen::Isometry3d& cartesian_waypoint,
103-
// const Instruction& parent_instruction,
104-
// const ManipulatorInfo& manip_info,
105-
// const std::vector<std::string>& active_links,
106-
// int index)
107-
//{
108-
// assert(isPlanInstruction(parent_instruction));
109-
// const auto* base_instruction = parent_instruction.cast_const<PlanInstruction>();
110-
// assert(!(manip_info.isEmpty() && base_instruction->getManipulatorInfo().isEmpty()));
111-
// const ManipulatorInfo& mi =
112-
// (base_instruction->getManipulatorInfo().isEmpty()) ? manip_info : base_instruction->getManipulatorInfo();
113-
114-
// trajopt::TermInfo::Ptr ti{ nullptr };
115-
116-
// /* Check if this cartesian waypoint is dynamic
117-
// * (i.e. defined relative to a frame that will move with the kinematic chain)
118-
// */
119-
// auto it = std::find(active_links.begin(), active_links.end(), mi.working_frame);
120-
// if (it != active_links.end())
121-
// {
122-
// ti = createDynamicCartesianWaypointTermInfo(
123-
// cartesian_waypoint, index, mi.working_frame, mi.tcp, cartesian_coeff, pci.kin->getTipLinkName(), term_type);
124-
// }
125-
// else
126-
// {
127-
// ti = createCartesianWaypointTermInfo(
128-
// cartesian_waypoint, index, mi.working_frame, mi.tcp, cartesian_coeff, pci.kin->getTipLinkName(), term_type);
129-
// }
130-
131-
// if (term_type == trajopt::TermType::TT_CNT)
132-
// pci.cnt_infos.push_back(ti);
133-
// else
134-
// pci.cost_infos.push_back(ti);
135-
//}
136-
137-
//void TrajOptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem,
138-
// const Eigen::VectorXd& joint_waypoint,
139-
// const Instruction& /*parent_instruction*/,
140-
// const ManipulatorInfo& /*manip_info*/,
141-
// const std::vector<std::string>& /*active_links*/,
142-
// int index)
143-
//{
144-
// auto ti = createJointWaypointTermInfo(joint_waypoint, index, joint_coeff, term_type);
145-
146-
// if (term_type == trajopt::TermType::TT_CNT)
147-
// pci.cnt_infos.push_back(ti);
148-
// else
149-
// pci.cost_infos.push_back(ti);
150-
//}
151-
152-
//tinyxml2::XMLElement* TrajOptDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc) const
153-
//{
154-
// Eigen::IOFormat eigen_format(Eigen::StreamPrecision, 0, " ", " ");
155-
156-
// tinyxml2::XMLElement* xml_planner = doc.NewElement("Planner");
157-
// xml_planner->SetAttribute("type", std::to_string(1).c_str());
158-
159-
// tinyxml2::XMLElement* xml_trajopt = doc.NewElement("TrajoptPlanProfile");
160-
161-
// tinyxml2::XMLElement* xml_cart_coeff = doc.NewElement("CartesianCoefficients");
162-
// std::stringstream cart_coeff;
163-
// cart_coeff << cartesian_coeff.format(eigen_format);
164-
// xml_cart_coeff->SetText(cart_coeff.str().c_str());
165-
// xml_trajopt->InsertEndChild(xml_cart_coeff);
166-
167-
// tinyxml2::XMLElement* xml_joint_coeff = doc.NewElement("JointCoefficients");
168-
// std::stringstream jnt_coeff;
169-
// jnt_coeff << joint_coeff.format(eigen_format);
170-
// xml_joint_coeff->SetText(jnt_coeff.str().c_str());
171-
// xml_trajopt->InsertEndChild(xml_joint_coeff);
172-
173-
// tinyxml2::XMLElement* xml_term_type = doc.NewElement("Term");
174-
// xml_term_type->SetAttribute("type", static_cast<int>(term_type));
175-
// xml_trajopt->InsertEndChild(xml_term_type);
176-
177-
// xml_planner->InsertEndChild(xml_trajopt);
38+
void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem,
39+
const CartesianWaypoint& cartesian_waypoint,
40+
const Instruction& parent_instruction,
41+
const ManipulatorInfo& manip_info,
42+
const std::vector<std::string>& active_links,
43+
int index)
44+
{
45+
assert(isPlanInstruction(parent_instruction));
46+
const auto* base_instruction = parent_instruction.cast_const<PlanInstruction>();
47+
assert(!(manip_info.isEmpty() && base_instruction->getManipulatorInfo().isEmpty()));
48+
const ManipulatorInfo& mi =
49+
(base_instruction->getManipulatorInfo().empty()) ? manip_info : base_instruction->getManipulatorInfo();
50+
51+
/* Check if this cartesian waypoint is dynamic
52+
* (i.e. defined relative to a frame that will move with the kinematic chain)
53+
*/
54+
auto it = std::find(active_links.begin(), active_links.end(), mi.working_frame);
55+
if (it != active_links.end())
56+
{
57+
CONSOLE_BRIDGE_logWarn("Dynamic cartesian terms are not supported by trajopt_ifopt. PRs welcome");
58+
}
59+
else
60+
{
61+
auto ti = createCartesianWaypointTermInfo(
62+
cartesian_waypoint, index, mi.working_frame, mi.tcp, cartesian_coeff, problem.manip_fwd_kin->getTipLinkName());
63+
}
64+
}
65+
66+
void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem,
67+
const JointWaypoint& joint_waypoint,
68+
const Instruction& /*parent_instruction*/,
69+
const ManipulatorInfo& /*manip_info*/,
70+
const std::vector<std::string>& /*active_links*/,
71+
int index)
72+
{
73+
auto idx = static_cast<std::size_t>(index);
74+
switch (term_type)
75+
{
76+
case TrajOptIfoptTermType::CONSTRAINT:
77+
addJointPositionConstraint(problem.nlp, joint_waypoint, problem.vars[idx], joint_coeff);
78+
break;
79+
case TrajOptIfoptTermType::SQUARED_COST:
80+
addJointPositionConstraint(problem.nlp, joint_waypoint, problem.vars[idx], joint_coeff);
81+
break;
82+
}
83+
}
17884

179-
// return xml_planner;
180-
//}
18185
} // namespace tesseract_planning

0 commit comments

Comments
 (0)