Merge PR #380: Fix Cartesian interpolation

Correctly consider an offset transform from link to reference frame,
such that rotations w.r.t. the reference frame don't move its origin.
This commit is contained in:
Robert Haschke 2022-11-01 23:51:53 +01:00
commit f88e25fe74
11 changed files with 98 additions and 77 deletions

View File

@ -66,8 +66,8 @@ public:
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
};
} // namespace solvers

View File

@ -63,8 +63,8 @@ public:
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
};
} // namespace solvers

View File

@ -84,8 +84,8 @@ public:
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
protected:

View File

@ -84,9 +84,10 @@ public:
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
/// plan trajectory from current robot state to Cartesian target
/// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
};

View File

@ -70,11 +70,13 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
}
// reach pose of forward kinematics
return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result, path_constraints);
return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg,
timeout, result, path_constraints);
}
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double /*timeout*/,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties();
@ -96,7 +98,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
moveit::core::MaxEEFStep(props.get<double>("step_size")),
moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid,
props.get<kinematics::KinematicsQueryOptions>("kinematics_options"));
props.get<kinematics::KinematicsQueryOptions>("kinematics_options"), offset);
assert(!trajectory.empty()); // there should be at least the start state
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);

View File

@ -99,9 +99,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
}
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto start_time = std::chrono::steady_clock::now();
@ -117,7 +117,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
return to->isStateValid(*robot_state, constraints, jmg->getName());
} };
if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) {
if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) {
// TODO(v4hn): planners need a way to add feedback to failing plans
// in case of an invalid solution feedback should include unwanted collisions or violated constraints
ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target");

View File

@ -172,8 +172,9 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
}
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties();
moveit_msgs::MotionPlanRequest req;
@ -181,7 +182,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
geometry_msgs::PoseStamped target;
target.header.frame_id = from->getPlanningFrame();
target.pose = tf2::toMsg(target_eigen);
target.pose = tf2::toMsg(target_eigen * offset.inverse());
req.goal_constraints.resize(1);
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(

View File

@ -104,8 +104,9 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c
return false;
}
// Create an arrow marker from start_pose to reached_pose, split into a red and green part based on achieved distance
static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Interface::Direction dir, bool success,
const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& link_pose,
const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& start_pose,
const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) {
double linear_norm = linear.norm();
@ -114,7 +115,7 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Inter
auto quat_cylinder = quat_target * Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
// link position before planning; reached link position after planning; target link position
Eigen::Vector3d pos_link = link_pose.translation();
Eigen::Vector3d pos_start = start_pose.translation();
Eigen::Vector3d pos_reached = reached_pose.translation();
Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0);
@ -124,7 +125,7 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Inter
if (dir == Interface::FORWARD) {
if (success) {
// valid part: green arrow
rviz_marker_tools::makeArrow(m, pos_link, pos_reached, 0.1 * linear_norm);
rviz_marker_tools::makeArrow(m, pos_start, pos_reached, 0.1 * linear_norm);
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
markers.push_back(m);
} else {
@ -139,14 +140,14 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Inter
rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, distance);
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
// position half-way between pos_link and pos_reached
m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_link + pos_reached) });
m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_start + pos_reached) });
m.pose.orientation = tf2::toMsg(quat_cylinder);
markers.push_back(m);
}
} else {
// valid part: green arrow
// head length according to above comment
rviz_marker_tools::makeArrow(m, pos_reached, pos_link, 0.1 * linear_norm, 0.23 * linear_norm);
rviz_marker_tools::makeArrow(m, pos_reached, pos_start, 0.1 * linear_norm, 0.23 * linear_norm);
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
markers.push_back(m);
if (!success) {
@ -203,10 +204,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
Eigen::Vector3d linear; // linear translation
Eigen::Vector3d angular; // angular rotation
double linear_norm = 0.0, angular_norm = 0.0;
Eigen::Isometry3d target_eigen;
Eigen::Isometry3d link_pose =
scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success
try { // try to extract Twist
const geometry_msgs::TwistStamped& target = boost::any_cast<geometry_msgs::TwistStamped>(direction);
@ -240,13 +238,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
angular *= -1.0;
}
// compute absolute transform for link
// compute target transform for ik_frame applying motion transform of twist
// linear+angular are expressed w.r.t. model frame and thus we need left-multiplication
linear = frame_pose.linear() * linear;
angular = frame_pose.linear() * angular;
target_eigen = ik_pose_world;
target_eigen.linear() =
target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular);
target_eigen.translation() += linear;
auto R = Eigen::AngleAxisd(angular_norm, angular);
auto p = ik_pose_world.translation();
target_eigen = Eigen::Translation3d(linear + p - R * p) * (R * ik_pose_world);
goto COMPUTE;
} catch (const boost::bad_any_cast&) { /* continue with Vector */
}
@ -267,31 +265,31 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
if (dir == Interface::BACKWARD)
linear *= -1.0;
// compute absolute transform for link
// compute target transform for ik_frame applying delta transform of twist
linear = frame_pose.linear() * linear;
target_eigen = ik_pose_world;
target_eigen.translation() += linear;
target_eigen = Eigen::Translation3d(linear) * ik_pose_world;
} catch (const boost::bad_any_cast&) {
solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name());
return false;
}
COMPUTE:
// transform target pose such that ik frame will reach there if link does
target_eigen = target_eigen * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link);
// offset from link to ik_frame
const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
success =
planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
reached_state->updateLinkTransforms();
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link);
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset;
double distance = 0.0;
if (use_rotation_distance) {
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose());
distance = rotation.angle();
} else
distance = (reached_pose.translation() - link_pose.translation()).norm();
distance = (reached_pose.translation() - ik_pose_world.translation()).norm();
// min_distance reached?
if (min_distance > 0.0) {
@ -309,8 +307,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
// visualize plan
auto ns = props.get<std::string>("marker_ns");
if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance
visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), link_pose, reached_pose, linear,
distance);
visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose,
linear, distance);
}
}

View File

@ -228,11 +228,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
add_frame(target, "target frame");
add_frame(ik_pose_world, "ik frame");
// transform target pose such that ik frame will reach there if link does
target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link);
// offset from link to ik_frame
Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
// plan to Cartesian target
success = planner_->plan(state.scene(), *link, target, jmg, timeout, robot_trajectory, path_constraints);
success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints);
}
// store result

View File

@ -69,23 +69,27 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
} else {
auto ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(property.value());
if (ik_pose_msg.header.frame_id.empty()) {
if (!(robot_link = get_tip())) {
solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found");
return false;
}
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame;
} else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) {
robot_link = scene.getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id);
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame;
} else {
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
robot_link = nullptr;
bool found = false;
auto ref_frame = scene.getCurrentState().getFrameInfo(ik_pose_msg.header.frame_id, robot_link, found);
if (!found && !ik_pose_msg.header.frame_id.empty()) {
std::stringstream ss;
ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'";
ss << "ik_frame specified in unknown frame '" << ik_pose_msg.header.frame_id << "'";
solution.markAsFailure(ss.str());
return false;
}
if (!robot_link)
robot_link = get_tip();
if (!robot_link) {
solution.markAsFailure("ik_frame doesn't specify a link frame");
return false;
} else if (!found) { // use robot link's frame as reference by default
ref_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
}
tip_in_global_frame = ref_frame * tip_in_global_frame;
}
return true;

View File

@ -6,6 +6,7 @@
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
@ -17,7 +18,7 @@ using namespace planning_scene;
using namespace moveit::core;
constexpr double TAU{ 2 * M_PI };
constexpr double EPS{ 1e-6 };
constexpr double EPS{ 5e-5 };
// provide a basic test fixture that prepares a Task
struct PandaMoveRelative : public testing::Test
@ -66,10 +67,24 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id)
return aco;
}
inline auto position(const PlanningSceneConstPtr& scene, const std::string& frame) {
return scene->getFrameTransform(frame).translation();
void expect_const_position(const SolutionBaseConstPtr& solution, const std::string& tip,
const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity()) {
const robot_trajectory::RobotTrajectory& t = *std::dynamic_pointer_cast<const SubTrajectory>(solution)->trajectory();
const Eigen::Vector3d start_position = (t.getFirstWayPoint().getFrameTransform(tip) * offset).translation();
for (size_t i = 0; i < t.getWayPointCount(); ++i) {
const Eigen::Vector3d position = (t.getWayPoint(i).getFrameTransform(tip) * offset).translation();
ASSERT_TRUE(start_position.isApprox(position, EPS))
<< "Rotation must maintain position\n"
<< i << ": " << start_position.transpose() << " != " << position.transpose();
}
}
#define EXPECT_CONST_POSITION(...) \
{ \
SCOPED_TRACE("expect_constant_position(" #__VA_ARGS__ ")"); \
expect_const_position(__VA_ARGS__); \
}
TEST_F(PandaMoveRelative, cartesianRotateEEF) {
move->setDirection([] {
geometry_msgs::TwistStamped twist;
@ -79,15 +94,22 @@ TEST_F(PandaMoveRelative, cartesianRotateEEF) {
}());
ASSERT_TRUE(t.plan()) << "Failed to plan";
EXPECT_CONST_POSITION(move->solutions().front(), group->getOnlyOneEndEffectorTip()->getName());
}
const auto& tip_name{ group->getOnlyOneEndEffectorTip()->getName() };
const auto start_eef_position{ position(scene, tip_name) };
const auto end_eef_position{ position(move->solutions().front()->end()->scene(), tip_name) };
TEST_F(PandaMoveRelative, cartesianCircular) {
const std::string tip = "panda_hand";
auto offset = Eigen::Translation3d(0, 0, 0.1);
move->setIKFrame(offset, tip);
move->setDirection([] {
geometry_msgs::TwistStamped twist;
twist.header.frame_id = "world";
twist.twist.angular.x = TAU / 4.0;
return twist;
}());
EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS))
<< "Cartesian rotation unexpectedly changed position of '" << tip_name << "' (must only change orientation)\n"
<< start_eef_position << "\nvs\n"
<< end_eef_position;
ASSERT_TRUE(t.plan()) << "Failed to plan";
EXPECT_CONST_POSITION(move->solutions().front(), tip, Eigen::Isometry3d(offset));
}
TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) {
@ -102,15 +124,8 @@ TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) {
return twist;
}());
ASSERT_TRUE(t.plan());
const auto start_eef_position{ position(scene, ATTACHED_OBJECT) };
const auto end_eef_position{ position(move->solutions().front()->end()->scene(), ATTACHED_OBJECT) };
EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS))
<< "Cartesian rotation unexpectedly changed position of ik frame (must only change orientation)\n"
<< start_eef_position << "\nvs\n"
<< end_eef_position;
ASSERT_TRUE(t.plan()) << "Failed to plan";
EXPECT_CONST_POSITION(move->solutions().front(), ATTACHED_OBJECT);
}
int main(int argc, char** argv) {