Skip to content

Commit

Permalink
Fix a bug when checking a pose is empty and TOTG corner case (moveit#…
Browse files Browse the repository at this point in the history 
…1274)

* Fix having empty object pose would use the shape pose as the object pose

* TOTG: Fix parameterizing a trajectory would produce a different last waypoint than the input last waypoint

* Update moveit_core/planning_scene/test/test_planning_scene.cpp

Co-authored-by: AndyZe <andyz@utexas.edu>

Co-authored-by: AndyZe <andyz@utexas.edu>
  • Loading branch information
@JafarAbdi @AndyZe @peterdavidfagan
2 people authored and peterdavidfagan committed Jul 14, 2022
1 parent ff20a99 commit 7e1683f
Show file tree
Hide file tree
Showing 4 changed files with 154 additions and 1 deletion.
28 changes: 28 additions & 0 deletions moveit_core/planning_scene/test/test_planning_scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,10 +44,38 @@
#include <sstream>
#include <string>
#include <boost/filesystem/path.hpp>
#include <tf2_eigen/tf2_eigen.hpp>

#include <moveit/collision_detection/collision_common.h>
#include <moveit/collision_detection/collision_plugin_cache.h>

// Test not setting the object's pose should use the shape pose as the object pose
TEST(PlanningScene, TestOneShapeObjectPose)
{
urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
srdf::ModelSharedPtr srdf_model = std::make_shared<srdf::Model>();
planning_scene::PlanningScene ps(urdf_model, srdf_model);

const std::string object_name = "object";
const Eigen::Isometry3d expected_transfrom = Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.5, -0.25, 0.0);

moveit_msgs::msg::CollisionObject co;
co.header.frame_id = "base_footprint";
co.id = object_name;
co.operation = moveit_msgs::msg::CollisionObject::ADD;
co.primitives.push_back([] {
shape_msgs::msg::SolidPrimitive primitive;
primitive.type = shape_msgs::msg::SolidPrimitive::CYLINDER;
primitive.dimensions = { 0.25, 0.02 };
return primitive;
}());
co.primitive_poses.push_back(tf2::toMsg(expected_transfrom));

ps.processCollisionObjectMsg(co);

EXPECT_TRUE(expected_transfrom.isApprox(ps.getFrameTransform(object_name)));
}

TEST(PlanningScene, LoadRestore)
{
urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
Expand Down
4 changes: 4 additions & 0 deletions moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1096,6 +1096,10 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t

if (diverse_point)
points.push_back(new_point);
// If the last point is not a diverse_point we replace the last added point with it to make sure to always have the
// input end point as the last point
else if (p == num_points - 1)
points.back() = new_point;
}

// Return trajectory with only the first waypoint if there are not multiple diverse points
Expand Down
121 changes: 121 additions & 0 deletions moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -256,6 +256,127 @@ TEST(time_optimal_trajectory_generation, testLargeAccel)
}
}

// Test parameterizing a trajectory would always produce a trajectory with output end waypoint same as the input end waypoint
TEST(time_optimal_trajectory_generation, testLastWaypoint)
{
constexpr auto robot_name{ "panda" };
constexpr auto group_name{ "hand" };

auto robot_model = moveit::core::loadTestingRobotModel(robot_name);
ASSERT_TRUE((bool)robot_model) << "Failed to load robot model" << robot_name;
auto group = robot_model->getJointModelGroup(group_name);
ASSERT_TRUE((bool)group) << "Failed to load joint model group " << group_name;
moveit::core::RobotState waypoint_state(robot_model);
waypoint_state.setToDefaultValues();

robot_trajectory::RobotTrajectory trajectory(robot_model, group);
auto add_waypoint = [&](const std::vector<double>& waypoint) {
waypoint_state.setJointGroupPositions(group, waypoint);
trajectory.addSuffixWayPoint(waypoint_state, 0.1);
};
add_waypoint({ 0.000000000, 0.000000000 });
add_waypoint({ 0.000396742, 0.000396742 });
add_waypoint({ 0.000793484, 0.000793484 });
add_waypoint({ 0.001190226, 0.001190226 });
add_waypoint({ 0.001586968, 0.001586968 });
add_waypoint({ 0.001983710, 0.001983710 });
add_waypoint({ 0.002380452, 0.002380452 });
add_waypoint({ 0.002777194, 0.002777194 });
add_waypoint({ 0.003173936, 0.003173936 });
add_waypoint({ 0.003570678, 0.003570678 });
add_waypoint({ 0.003967420, 0.003967420 });
add_waypoint({ 0.004364162, 0.004364162 });
add_waypoint({ 0.004760904, 0.004760904 });
add_waypoint({ 0.005157646, 0.005157646 });
add_waypoint({ 0.005554388, 0.005554388 });
add_waypoint({ 0.005951130, 0.005951130 });
add_waypoint({ 0.006347872, 0.006347872 });
add_waypoint({ 0.006744614, 0.006744614 });
add_waypoint({ 0.007141356, 0.007141356 });
add_waypoint({ 0.007538098, 0.007538098 });
add_waypoint({ 0.007934840, 0.007934840 });
add_waypoint({ 0.008331582, 0.008331582 });
add_waypoint({ 0.008728324, 0.008728324 });
add_waypoint({ 0.009125066, 0.009125066 });
add_waypoint({ 0.009521808, 0.009521808 });
add_waypoint({ 0.009918550, 0.009918550 });
add_waypoint({ 0.010315292, 0.010315292 });
add_waypoint({ 0.010712034, 0.010712034 });
add_waypoint({ 0.011108776, 0.011108776 });
add_waypoint({ 0.011505518, 0.011505518 });
add_waypoint({ 0.011902261, 0.011902261 });
add_waypoint({ 0.012299003, 0.012299003 });
add_waypoint({ 0.012695745, 0.012695745 });
add_waypoint({ 0.013092487, 0.013092487 });
add_waypoint({ 0.013489229, 0.013489229 });
add_waypoint({ 0.013885971, 0.013885971 });
add_waypoint({ 0.014282713, 0.014282713 });
add_waypoint({ 0.014679455, 0.014679455 });
add_waypoint({ 0.015076197, 0.015076197 });
add_waypoint({ 0.015472939, 0.015472939 });
add_waypoint({ 0.015869681, 0.015869681 });
add_waypoint({ 0.016266423, 0.016266423 });
add_waypoint({ 0.016663165, 0.016663165 });
add_waypoint({ 0.017059907, 0.017059907 });
add_waypoint({ 0.017456649, 0.017456649 });
add_waypoint({ 0.017853391, 0.017853391 });
add_waypoint({ 0.018250133, 0.018250133 });
add_waypoint({ 0.018646875, 0.018646875 });
add_waypoint({ 0.019043617, 0.019043617 });
add_waypoint({ 0.019440359, 0.019440359 });
add_waypoint({ 0.019837101, 0.019837101 });
add_waypoint({ 0.020233843, 0.020233843 });
add_waypoint({ 0.020630585, 0.020630585 });
add_waypoint({ 0.021027327, 0.021027327 });
add_waypoint({ 0.021424069, 0.021424069 });
add_waypoint({ 0.021820811, 0.021820811 });
add_waypoint({ 0.022217553, 0.022217553 });
add_waypoint({ 0.022614295, 0.022614295 });
add_waypoint({ 0.023011037, 0.023011037 });
add_waypoint({ 0.023407779, 0.023407779 });
add_waypoint({ 0.023804521, 0.023804521 });
add_waypoint({ 0.024201263, 0.024201263 });
add_waypoint({ 0.024598005, 0.024598005 });
add_waypoint({ 0.024994747, 0.024994747 });
add_waypoint({ 0.025391489, 0.025391489 });
add_waypoint({ 0.025788231, 0.025788231 });
add_waypoint({ 0.026184973, 0.026184973 });
add_waypoint({ 0.026581715, 0.026581715 });
add_waypoint({ 0.026978457, 0.026978457 });
add_waypoint({ 0.027375199, 0.027375199 });
add_waypoint({ 0.027771941, 0.027771941 });
add_waypoint({ 0.028168683, 0.028168683 });
add_waypoint({ 0.028565425, 0.028565425 });
add_waypoint({ 0.028962167, 0.028962167 });
add_waypoint({ 0.029358909, 0.029358909 });
add_waypoint({ 0.029755651, 0.029755651 });
add_waypoint({ 0.030152393, 0.030152393 });
add_waypoint({ 0.030549135, 0.030549135 });
add_waypoint({ 0.030945877, 0.030945877 });
add_waypoint({ 0.031342619, 0.031342619 });
add_waypoint({ 0.031739361, 0.031739361 });
add_waypoint({ 0.032136103, 0.032136103 });
add_waypoint({ 0.032532845, 0.032532845 });
add_waypoint({ 0.032929587, 0.032929587 });
add_waypoint({ 0.033326329, 0.033326329 });
add_waypoint({ 0.033723071, 0.033723071 });
add_waypoint({ 0.034119813, 0.034119813 });
add_waypoint({ 0.034516555, 0.034516555 });

const std::vector<double> expected_last_waypoint = { 0.034913297, 0.034913297 };
add_waypoint(expected_last_waypoint);

TimeOptimalTrajectoryGeneration totg;
ASSERT_TRUE(totg.computeTimeStamps(trajectory)) << "Failed to compute time stamps";
const auto robot_state = trajectory.getLastWayPoint();
std::vector<double> actual_last_waypoint;
robot_state.copyJointGroupPositions(group, actual_last_waypoint);
EXPECT_TRUE(std::equal(actual_last_waypoint.cbegin(), actual_last_waypoint.cend(), expected_last_waypoint.cbegin(),
expected_last_waypoint.cend(), [](const double rhs, const double lhs) {
return std::abs(rhs - lhs) < std::numeric_limits<double>::epsilon();
}));
}

TEST(time_optimal_trajectory_generation, testPluginAPI)
{
constexpr auto robot_name{ "panda" };
Expand Down
2 changes: 1 addition & 1 deletion moveit_core/utils/src/message_checks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ bool isEmpty(const moveit_msgs::msg::Constraints& constr)

bool isEmpty(const geometry_msgs::msg::Quaternion& msg)
{
return msg.x == 0.0 && msg.y == 0.0 && msg.z == 0.0 && msg.w == 0.0;
return msg.x == 0.0 && msg.y == 0.0 && msg.z == 0.0 && msg.w == 1.0;
}

bool isEmpty(const geometry_msgs::msg::Pose& msg)
Expand Down

0 comments on commit 7e1683f

Please sign in to comment.