Skip to content

Commit

Permalink
Merge branch 'fix-cartesian-interpolation' into ros2
Browse files Browse the repository at this point in the history
  • Loading branch information
@rhaschke
rhaschke committed Sep 6, 2022
2 parents 3b438f9 + 11be7ea commit c13fd31
Show file tree
Hide file tree
Showing 11 changed files with 65 additions and 59 deletions.
4 changes: 2 additions & 2 deletions core/include/moveit/task_constructor/solvers/cartesian_path.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,8 +66,8 @@ class CartesianPath : public PlannerInterface
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
};
} // namespace solvers
Expand Down
4 changes: 2 additions & 2 deletions core/include/moveit/task_constructor/solvers/joint_interpolation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,8 +63,8 @@ class JointInterpolationPlanner : public PlannerInterface
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
};
} // namespace solvers
Expand Down
4 changes: 2 additions & 2 deletions core/include/moveit/task_constructor/solvers/pipeline_planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,8 +90,8 @@ class PipelinePlanner : public PlannerInterface
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

protected:
Expand Down
5 changes: 3 additions & 2 deletions core/include/moveit/task_constructor/solvers/planner_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,9 +84,10 @@ class PlannerInterface
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
};
Expand Down
2 changes: 1 addition & 1 deletion core/include/moveit/task_constructor/utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,7 +141,7 @@ class Flags
};

const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
std::string frame);
const std::string& frame);

bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
const moveit::core::JointModelGroup* jmg, SolutionBase& solution,
Expand Down
8 changes: 5 additions & 3 deletions core/src/solvers/cartesian_path.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,11 +74,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::msg::Constraints& path_constraints) {
const auto& props = properties();
Expand All @@ -100,7 +102,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);
Expand Down
8 changes: 4 additions & 4 deletions core/src/solvers/joint_interpolation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,9 +102,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::msg::Constraints& path_constraints) {
const auto start_time = std::chrono::steady_clock::now();

Expand All @@ -120,7 +120,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
RCLCPP_WARN(LOGGER, "IK failed for pose target");
Expand Down
7 changes: 4 additions & 3 deletions core/src/solvers/pipeline_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -185,16 +185,17 @@ 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::msg::Constraints& path_constraints) {
const auto& props = properties();
moveit_msgs::msg::MotionPlanRequest req;
initMotionPlanRequest(req, props, jmg, timeout);

geometry_msgs::msg::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(
Expand Down
46 changes: 22 additions & 24 deletions core/src/stages/move_relative.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,8 +110,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::msg::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();

Expand All @@ -120,7 +121,7 @@ static void visualizePlan(std::deque<visualization_msgs::msg::Marker>& markers,
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);

Expand All @@ -130,7 +131,7 @@ static void visualizePlan(std::deque<visualization_msgs::msg::Marker>& markers,
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 {
Expand All @@ -145,14 +146,14 @@ static void visualizePlan(std::deque<visualization_msgs::msg::Marker>& markers,
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) {
Expand Down Expand Up @@ -209,10 +210,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::msg::TwistStamped& target = boost::any_cast<geometry_msgs::msg::TwistStamped>(direction);
Expand Down Expand Up @@ -246,13 +244,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 */
}
Expand All @@ -274,31 +272,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);

moveit::core::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) {
Expand All @@ -316,8 +314,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);
}
}

Expand Down
6 changes: 3 additions & 3 deletions core/src/stages/move_to.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -237,11 +237,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
Expand Down
30 changes: 17 additions & 13 deletions core/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ namespace task_constructor {
namespace utils {

const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
std::string frame) {
const std::string& frame) {
return state.getRigidlyConnectedParentLinkModel(frame);
}

Expand All @@ -79,23 +79,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::msg::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 = getRigidlyConnectedParentLinkModel(scene.getCurrentState(), 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.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;
Expand Down

0 comments on commit c13fd31

Please sign in to comment.