Skip to content

Commit 97e1f17

Browse files
marrtsmarrts
authored
Add trajopt joint configs for fix state collision task (#637)
1 parent 907fc7d commit 97e1f17

File tree

5 files changed

+166
-7
lines changed

5 files changed

+166
-7
lines changed

‎tesseract_task_composer/planning/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,7 @@ set(WINDOWS_DEPENDS
8888
if(tesseract_motion_planners_trajopt_FOUND)
8989
list(APPEND FACTORIES_SOURCE_FILES src/factories/planning_task_composer_trajopt_plugin_factory.cpp)
9090
list(APPEND FACTORIES_SOURCE_LINK_LIBRARIES tesseract::tesseract_motion_planners_trajopt)
91+
list(APPEND LIB_SOURCE_LINK_LIBRARIES tesseract::tesseract_motion_planners_trajopt)
9192
list(APPEND LINUX_DEPENDS "${TESSERACT_PACKAGE_PREFIX}tesseract-motion-planners-trajopt")
9293
list(APPEND WINDOWS_DEPENDS "${TESSERACT_PACKAGE_PREFIX}tesseract-motion-planners-trajopt")
9394
endif()

‎tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_collision_profile.h‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3434

3535
#include <tesseract_collision/core/types.h>
3636
#include <tesseract_common/profile.h>
37+
#include <tesseract_motion_planners/trajopt/trajopt_waypoint_config.h>
3738

3839
namespace tesseract_planning
3940
{
@@ -88,6 +89,12 @@ struct FixStateCollisionProfile : public tesseract_common::Profile
8889
/** @brief Number of sampling attempts if TrajOpt correction fails*/
8990
int sampling_attempts{ 100 };
9091

92+
/** @brief The TrajOpt joint waypoint constraint config */
93+
TrajOptJointWaypointConfig trajopt_joint_constraint_config;
94+
95+
/** @brief The TrajOpt joint waypoint cost config */
96+
TrajOptJointWaypointConfig trajopt_joint_cost_config;
97+
9198
private:
9299
friend class boost::serialization::access;
93100
friend struct tesseract_common::Serialization;

‎tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp‎

Lines changed: 89 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -160,7 +160,6 @@ bool moveWaypointFromCollisionTrajopt(WaypointPoly& waypoint,
160160
CONSOLE_BRIDGE_logError("MoveWaypointFromCollision error: %s", e.what());
161161
return false;
162162
}
163-
auto num_jnts = static_cast<std::size_t>(start_pos.size());
164163

165164
// Setup trajopt problem with basic info
166165
ProblemConstructionInfo pci(env);
@@ -183,23 +182,106 @@ bool moveWaypointFromCollisionTrajopt(WaypointPoly& waypoint,
183182
Eigen::VectorXd pos_tolerance = range * profile.jiggle_factor;
184183
Eigen::VectorXd neg_tolerance = range * -profile.jiggle_factor;
185184

186-
{ // Add Constraint
185+
// Use trajopt_joint_constraint_config if enabled
186+
if (profile.trajopt_joint_constraint_config.enabled)
187+
{
187188
auto jp_cnt = std::make_shared<JointPosTermInfo>();
188-
jp_cnt->coeffs = std::vector<double>(num_jnts, 1.0);
189+
jp_cnt->coeffs = std::vector<double>(profile.trajopt_joint_constraint_config.coeff.data(),
190+
profile.trajopt_joint_constraint_config.coeff.data() +
191+
profile.trajopt_joint_constraint_config.coeff.size());
189192
jp_cnt->targets = std::vector<double>(start_pos.data(), start_pos.data() + start_pos.size());
190-
jp_cnt->upper_tols = std::vector<double>(pos_tolerance.data(), pos_tolerance.data() + pos_tolerance.size());
191-
jp_cnt->lower_tols = std::vector<double>(neg_tolerance.data(), neg_tolerance.data() + neg_tolerance.size());
193+
if (profile.trajopt_joint_constraint_config.use_tolerance_override)
194+
{
195+
// Handle lower tolerance
196+
if (profile.trajopt_joint_constraint_config.lower_tolerance.size() == 1)
197+
{
198+
jp_cnt->lower_tols = std::vector<double>(static_cast<std::size_t>(start_pos.size()),
199+
profile.trajopt_joint_constraint_config.lower_tolerance[0]);
200+
}
201+
else if (profile.trajopt_joint_constraint_config.lower_tolerance.size() == start_pos.size())
202+
{
203+
jp_cnt->lower_tols = std::vector<double>(profile.trajopt_joint_constraint_config.lower_tolerance.data(),
204+
profile.trajopt_joint_constraint_config.lower_tolerance.data() +
205+
profile.trajopt_joint_constraint_config.lower_tolerance.size());
206+
}
207+
else
208+
{
209+
jp_cnt->lower_tols = std::vector<double>(neg_tolerance.data(), neg_tolerance.data() + neg_tolerance.size());
210+
}
211+
212+
// Handle upper tolerance
213+
if (profile.trajopt_joint_constraint_config.upper_tolerance.size() == 1)
214+
{
215+
jp_cnt->upper_tols = std::vector<double>(static_cast<std::size_t>(start_pos.size()),
216+
profile.trajopt_joint_constraint_config.upper_tolerance[0]);
217+
}
218+
else if (profile.trajopt_joint_constraint_config.upper_tolerance.size() == start_pos.size())
219+
{
220+
jp_cnt->upper_tols = std::vector<double>(profile.trajopt_joint_constraint_config.upper_tolerance.data(),
221+
profile.trajopt_joint_constraint_config.upper_tolerance.data() +
222+
profile.trajopt_joint_constraint_config.upper_tolerance.size());
223+
}
224+
else
225+
{
226+
jp_cnt->upper_tols = std::vector<double>(pos_tolerance.data(), pos_tolerance.data() + pos_tolerance.size());
227+
}
228+
}
229+
else
230+
{
231+
jp_cnt->lower_tols = std::vector<double>(neg_tolerance.data(), neg_tolerance.data() + neg_tolerance.size());
232+
jp_cnt->upper_tols = std::vector<double>(pos_tolerance.data(), pos_tolerance.data() + pos_tolerance.size());
233+
}
192234
jp_cnt->first_step = 0;
193235
jp_cnt->last_step = 0;
194236
jp_cnt->name = "joint_pos_cnt";
195237
jp_cnt->term_type = TermType::TT_CNT;
196238
pci.cnt_infos.push_back(jp_cnt);
197239
}
198240

199-
{ // Add Costs
241+
// Use trajopt_joint_cost_config if enabled
242+
if (profile.trajopt_joint_cost_config.enabled)
243+
{
200244
auto jp_cost = std::make_shared<JointPosTermInfo>();
201-
jp_cost->coeffs = std::vector<double>(num_jnts, 5.0);
245+
jp_cost->coeffs = std::vector<double>(profile.trajopt_joint_cost_config.coeff.data(),
246+
profile.trajopt_joint_cost_config.coeff.data() +
247+
profile.trajopt_joint_cost_config.coeff.size());
202248
jp_cost->targets = std::vector<double>(start_pos.data(), start_pos.data() + start_pos.size());
249+
if (profile.trajopt_joint_cost_config.use_tolerance_override)
250+
{
251+
// Handle lower tolerance
252+
if (profile.trajopt_joint_cost_config.lower_tolerance.size() == 1)
253+
{
254+
jp_cost->lower_tols = std::vector<double>(static_cast<std::size_t>(start_pos.size()),
255+
profile.trajopt_joint_cost_config.lower_tolerance[0]);
256+
}
257+
else if (profile.trajopt_joint_cost_config.lower_tolerance.size() == start_pos.size())
258+
{
259+
jp_cost->lower_tols = std::vector<double>(profile.trajopt_joint_cost_config.lower_tolerance.data(),
260+
profile.trajopt_joint_cost_config.lower_tolerance.data() +
261+
profile.trajopt_joint_cost_config.lower_tolerance.size());
262+
}
263+
else
264+
{
265+
jp_cost->lower_tols = std::vector<double>(neg_tolerance.data(), neg_tolerance.data() + neg_tolerance.size());
266+
}
267+
268+
// Handle upper tolerance
269+
if (profile.trajopt_joint_cost_config.upper_tolerance.size() == 1)
270+
{
271+
jp_cost->upper_tols = std::vector<double>(static_cast<std::size_t>(start_pos.size()),
272+
profile.trajopt_joint_cost_config.upper_tolerance[0]);
273+
}
274+
else if (profile.trajopt_joint_cost_config.upper_tolerance.size() == start_pos.size())
275+
{
276+
jp_cost->upper_tols = std::vector<double>(profile.trajopt_joint_cost_config.upper_tolerance.data(),
277+
profile.trajopt_joint_cost_config.upper_tolerance.data() +
278+
profile.trajopt_joint_cost_config.upper_tolerance.size());
279+
}
280+
else
281+
{
282+
jp_cost->upper_tols = std::vector<double>(pos_tolerance.data(), pos_tolerance.data() + pos_tolerance.size());
283+
}
284+
}
203285
jp_cost->first_step = 0;
204286
jp_cost->last_step = 0;
205287
jp_cost->name = "joint_pos_cost";

‎tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@
2929
#include <boost/serialization/base_object.hpp>
3030
#include <boost/serialization/nvp.hpp>
3131
#include <boost/serialization/vector.hpp>
32+
#include <tesseract_common/eigen_serialization.h>
3233
#include <typeindex>
3334

3435
namespace tesseract_planning
@@ -38,6 +39,8 @@ FixStateCollisionProfile::FixStateCollisionProfile(Settings mode)
3839
{
3940
collision_check_config.contact_request.type = tesseract_collision::ContactTestType::FIRST;
4041
collision_check_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
42+
trajopt_joint_constraint_config.coeff = Eigen::VectorXd::Constant(1, 1, 1);
43+
trajopt_joint_cost_config.coeff = Eigen::VectorXd::Constant(1, 1, 5);
4144
}
4245

4346
std::size_t FixStateCollisionProfile::getStaticKey()
@@ -55,6 +58,8 @@ void FixStateCollisionProfile::serialize(Archive& ar, const unsigned int /*versi
5558
ar& BOOST_SERIALIZATION_NVP(contact_manager_config);
5659
ar& BOOST_SERIALIZATION_NVP(collision_check_config);
5760
ar& BOOST_SERIALIZATION_NVP(sampling_attempts);
61+
ar& BOOST_SERIALIZATION_NVP(trajopt_joint_constraint_config);
62+
ar& BOOST_SERIALIZATION_NVP(trajopt_joint_cost_config);
5863
}
5964

6065
} // namespace tesseract_planning

‎tesseract_task_composer/test/fix_state_collision_task_unit.cpp‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -195,6 +195,70 @@ TEST_F(FixStateCollisionTaskUnit, MoveWaypointFromCollisionTrajoptTest) // NOLI
195195
EXPECT_FALSE(waypointInCollision(wp, manip_, *env_, profile, contacts));
196196
}
197197

198+
TEST_F(FixStateCollisionTaskUnit, MoveWaypointFromCollisionTrajoptNoJointCostTest)
199+
{
200+
CompositeInstruction program = test_suite::freespaceExampleProgramABB();
201+
202+
// Create data storage
203+
auto task_data = std::make_shared<TaskComposerDataStorage>();
204+
task_data->setData("input_program", program);
205+
task_data->setData("environment", std::shared_ptr<const Environment>(env_));
206+
207+
FixStateCollisionProfile profile;
208+
209+
Eigen::VectorXd state = Eigen::VectorXd::Zero(2);
210+
JointWaypoint waypoint{ { "boxbot_x_joint", "boxbot_y_joint" }, state };
211+
212+
// Check that the safety margin is obeyed
213+
profile.contact_manager_config = tesseract_collision::ContactManagerConfig(0.1);
214+
profile.jiggle_factor = 1.0;
215+
profile.trajopt_joint_constraint_config.enabled = false; // Disable joint constraint
216+
profile.trajopt_joint_cost_config.enabled = false; // Disable joint cost
217+
waypoint.getPosition()[0] = 0.0;
218+
waypoint.getPosition()[1] = 1.09;
219+
WaypointPoly wp(waypoint);
220+
tesseract_collision::ContactResultMap contacts;
221+
222+
EXPECT_TRUE(waypointInCollision(wp, manip_, *env_, profile, contacts));
223+
EXPECT_TRUE(moveWaypointFromCollisionTrajopt(wp, manip_, env_, profile));
224+
EXPECT_FALSE(waypointInCollision(wp, manip_, *env_, profile, contacts));
225+
}
226+
227+
TEST_F(FixStateCollisionTaskUnit, MoveWaypointFromCollisionTrajoptFailsWithZeroToleranceTest)
228+
{
229+
CompositeInstruction program = test_suite::freespaceExampleProgramABB();
230+
231+
// Create data storage
232+
auto task_data = std::make_shared<TaskComposerDataStorage>();
233+
task_data->setData("input_program", program);
234+
task_data->setData("environment", std::shared_ptr<const Environment>(env_));
235+
236+
FixStateCollisionProfile profile;
237+
238+
Eigen::VectorXd state = Eigen::VectorXd::Zero(2);
239+
JointWaypoint waypoint{ { "boxbot_x_joint", "boxbot_y_joint" }, state };
240+
241+
// Set up the profile to use zero tolerance override and large coeff
242+
profile.contact_manager_config = tesseract_collision::ContactManagerConfig(0.1);
243+
profile.jiggle_factor = 1.0;
244+
profile.trajopt_joint_constraint_config.enabled = true;
245+
profile.trajopt_joint_constraint_config.use_tolerance_override = true;
246+
profile.trajopt_joint_constraint_config.lower_tolerance = Eigen::VectorXd::Zero(2);
247+
profile.trajopt_joint_constraint_config.upper_tolerance = Eigen::VectorXd::Zero(2);
248+
profile.trajopt_joint_constraint_config.coeff = Eigen::VectorXd::Constant(2, 1000.0); // Large coeff
249+
profile.trajopt_joint_cost_config.enabled = false; // No cost
250+
251+
waypoint.getPosition()[0] = 0.0;
252+
waypoint.getPosition()[1] = 1.09;
253+
WaypointPoly wp(waypoint);
254+
tesseract_collision::ContactResultMap contacts;
255+
256+
EXPECT_TRUE(waypointInCollision(wp, manip_, *env_, profile, contacts));
257+
// Should fail to move out of collision due to zero tolerance and large coeff
258+
EXPECT_FALSE(moveWaypointFromCollisionTrajopt(wp, manip_, env_, profile));
259+
EXPECT_TRUE(waypointInCollision(wp, manip_, *env_, profile, contacts));
260+
}
261+
198262
int main(int argc, char** argv)
199263
{
200264
testing::InitGoogleTest(&argc, argv);

0 commit comments

Comments
 (0)