Eigen::Affine3 -> Eigen::Isometry3

This commit is contained in:
Robert Haschke 2018-10-21 02:25:48 +02:00
parent 5690d68311
commit ae75c3aa7f
20 changed files with 54 additions and 54 deletions

View File

@ -65,7 +65,7 @@ public:
bool plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link,
const Eigen::Affine3d& target,
const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup *jmg,
double timeout,
robot_trajectory::RobotTrajectoryPtr& result,

View File

@ -61,7 +61,7 @@ public:
bool plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link,
const Eigen::Affine3d& target,
const Eigen::Isometry3d& target,
const core::JointModelGroup *jmg,
double timeout,
robot_trajectory::RobotTrajectoryPtr& result,

View File

@ -65,7 +65,7 @@ public:
bool plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link,
const Eigen::Affine3d& target,
const Eigen::Isometry3d& target,
const core::JointModelGroup *jmg,
double timeout,
robot_trajectory::RobotTrajectoryPtr& result,

View File

@ -86,7 +86,7 @@ public:
/// plan trajectory from current robot state to Cartesian target
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link,
const Eigen::Affine3d& target,
const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup *jmg,
double timeout,
robot_trajectory::RobotTrajectoryPtr& result,

View File

@ -77,24 +77,24 @@ public:
void setIKFrame(const geometry_msgs::PoseStamped &pose) {
setProperty("ik_frame", pose);
}
void setIKFrame(const Eigen::Affine3d& pose, const std::string& link);
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T>
void setIKFrame(const T& p, const std::string& link) {
Eigen::Affine3d pose; pose = p;
Eigen::Isometry3d pose; pose = p;
setIKFrame(pose, link);
}
void setIKFrame(const std::string& link) {
setIKFrame(Eigen::Affine3d::Identity(), link);
setIKFrame(Eigen::Isometry3d::Identity(), link);
}
/// setters for target pose
void setTargetPose(const geometry_msgs::PoseStamped &pose) {
setProperty("target_pose", pose);
}
void setTargetPose(const Eigen::Affine3d& pose, const std::string& frame = "");
void setTargetPose(const Eigen::Isometry3d& pose, const std::string& frame = "");
template <typename T>
void setTargetPose(const T& p, const std::string& frame = "") {
Eigen::Affine3d pose; pose = p;
Eigen::Isometry3d pose; pose = p;
setTargetPose(pose, frame);
}

View File

@ -65,14 +65,14 @@ public:
void setIKFrame(const geometry_msgs::PoseStamped &pose) {
setProperty("ik_frame", pose);
}
void setIKFrame(const Eigen::Affine3d& pose, const std::string& link);
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T>
void setIKFrame(const T& p, const std::string& link) {
Eigen::Affine3d pose; pose = p;
Eigen::Isometry3d pose; pose = p;
setIKFrame(pose, link);
}
void setIKFrame(const std::string& link) {
setIKFrame(Eigen::Affine3d::Identity(), link);
setIKFrame(Eigen::Isometry3d::Identity(), link);
}
/// set minimum / maximum distance to move

View File

@ -64,14 +64,14 @@ public:
void setIKFrame(const geometry_msgs::PoseStamped &pose) {
setProperty("ik_frame", pose);
}
void setIKFrame(const Eigen::Affine3d& pose, const std::string& link);
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T>
void setIKFrame(const T& p, const std::string& link) {
Eigen::Affine3d pose; pose = p;
Eigen::Isometry3d pose; pose = p;
setIKFrame(pose, link);
}
void setIKFrame(const std::string& link) {
setIKFrame(Eigen::Affine3d::Identity(), link);
setIKFrame(Eigen::Isometry3d::Identity(), link);
}
/// move link to given pose
@ -104,10 +104,10 @@ protected:
SubTrajectory &trajectory, Direction dir);
bool getJointStateGoal(const boost::any& goal, const core::JointModelGroup* jmg, moveit::core::RobotState& state);
bool getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStamped& ik_pose_msg,
const planning_scene::PlanningScenePtr& scene, Eigen::Affine3d& target_eigen,
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen,
decltype(std::declval<SolutionBase>().markers())& markers);
bool getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
const planning_scene::PlanningScenePtr& scene, Eigen::Affine3d& target_eigen,
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen,
decltype(std::declval<SolutionBase>().markers())&);
protected:

View File

@ -72,14 +72,14 @@ public:
/// set properties of IK solver
void setIKFrame(const geometry_msgs::PoseStamped &transform) { setProperty("ik_frame", transform); }
void setIKFrame(const Eigen::Affine3d& pose, const std::string& link);
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T>
void setIKFrame(const T& t, const std::string& link) {
Eigen::Affine3d transform; transform = t;
Eigen::Isometry3d transform; transform = t;
setIKFrame(transform, link);
}
void setIKFrame(const std::string& link) {
setIKFrame(Eigen::Affine3d::Identity(), link);
setIKFrame(Eigen::Isometry3d::Identity(), link);
}
void setMaxIKSolutions(uint32_t max_ik_solutions) { setProperty("max_ik_solutions", max_ik_solutions); }

View File

@ -74,7 +74,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link,
const Eigen::Affine3d &target,
const Eigen::Isometry3d &target,
const moveit::core::JointModelGroup *jmg,
double timeout,
robot_trajectory::RobotTrajectoryPtr &result,

View File

@ -99,7 +99,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link,
const Eigen::Affine3d& target_eigen,
const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup *jmg,
double timeout,
robot_trajectory::RobotTrajectoryPtr& result,

View File

@ -115,7 +115,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link,
const Eigen::Affine3d& target_eigen,
const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup *jmg,
double timeout,
robot_trajectory::RobotTrajectoryPtr& result,

View File

@ -68,7 +68,7 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
p.declare<geometry_msgs::PoseStamped>("target_pose", "goal pose for ik frame");
}
void ComputeIK::setIKFrame(const Eigen::Affine3d &pose, const std::string &link)
void ComputeIK::setIKFrame(const Eigen::Isometry3d &pose, const std::string &link)
{
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
@ -76,7 +76,7 @@ void ComputeIK::setIKFrame(const Eigen::Affine3d &pose, const std::string &link)
setIKFrame(pose_msg);
}
void ComputeIK::setTargetPose(const Eigen::Affine3d &pose, const std::string &frame)
void ComputeIK::setTargetPose(const Eigen::Isometry3d &pose, const std::string &frame)
{
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = frame;
@ -92,7 +92,7 @@ namespace {
// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though...
// TODO: move into MoveIt! core, lift active_components_only_ from fcl to common interface
bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene,
Eigen::Affine3d pose, const robot_model::LinkModel* link,
Eigen::Isometry3d pose, const robot_model::LinkModel* link,
collision_detection::CollisionResult* collision_result = nullptr)
{
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
@ -235,7 +235,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
if (target_pose_msg.header.frame_id.empty()) // if not provided, assume planning frame
target_pose_msg.header.frame_id = sandbox_scene->getPlanningFrame();
Eigen::Affine3d target_pose;
Eigen::Isometry3d target_pose;
tf::poseMsgToEigen(target_pose_msg.pose, target_pose);
if (target_pose_msg.header.frame_id != sandbox_scene->getPlanningFrame()) {
if (!sandbox_scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
@ -261,7 +261,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
ik_pose_msg.pose.orientation.w = 1.0;
} else {
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
Eigen::Affine3d ik_pose;
Eigen::Isometry3d ik_pose;
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown link: " << ik_pose_msg.header.frame_id);

View File

@ -147,7 +147,7 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &
tf::vectorMsgToEigen(boost::any_cast<geometry_msgs::Vector3>(dir), correction);
const std::string& name = c.body_type_1 == cd::BodyTypes::WORLD_OBJECT ? c.body_name_1 : c.body_name_2;
scene.getWorldNonConst()->moveObject(name, Eigen::Affine3d(Eigen::Translation3d(correction)));
scene.getWorldNonConst()->moveObject(name, Eigen::Isometry3d(Eigen::Translation3d(correction)));
}
}

View File

@ -131,7 +131,7 @@ void GenerateGraspPose::compute() {
double current_angle_ = 0.0;
while (current_angle_ < 2.*M_PI && current_angle_ > -2.*M_PI) {
// rotate object pose about z-axis
Eigen::Affine3d target_pose(Eigen::AngleAxisd(current_angle_, Eigen::Vector3d::UnitZ()));
Eigen::Isometry3d target_pose(Eigen::AngleAxisd(current_angle_, Eigen::Vector3d::UnitZ()));
current_angle_ += props.get<double>("angle_delta");
InterfaceState state(scene);

View File

@ -60,7 +60,7 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
"constraints to maintain during trajectory");
}
void MoveRelative::setIKFrame(const Eigen::Affine3d& pose, const std::string& link)
void MoveRelative::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link)
{
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
@ -154,12 +154,12 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
Eigen::Vector3d angular; // angular rotation
double linear_norm = 0.0, angular_norm = 0.0;
Eigen::Affine3d target_eigen;
Eigen::Affine3d link_pose = scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success
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);
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
tf::vectorMsgToEigen(target.twist.linear, linear);
tf::vectorMsgToEigen(target.twist.angular, angular);
@ -200,7 +200,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
try { // try to extract Vector
const geometry_msgs::Vector3Stamped& target = boost::any_cast<geometry_msgs::Vector3Stamped>(direction);
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
tf::vectorMsgToEigen(target.vector, linear);
// use max distance?
@ -225,7 +225,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
COMPUTE:
// transform target pose such that ik frame will reach there if link does
Eigen::Affine3d ik_pose;
Eigen::Isometry3d ik_pose;
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
target_eigen = target_eigen * ik_pose.inverse();
@ -237,7 +237,7 @@ COMPUTE:
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
reached_state->updateLinkTransforms();
const Eigen::Affine3d& reached_pose = reached_state->getGlobalLinkTransform(link);
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link);
if (use_rotation_distance) {
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
distance = rotation.angle();

View File

@ -58,7 +58,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
"constraints to maintain during trajectory");
}
void MoveTo::setIKFrame(const Eigen::Affine3d& pose, const std::string& link)
void MoveTo::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link)
{
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
@ -107,14 +107,14 @@ bool MoveTo::getJointStateGoal(const boost::any& goal,
bool MoveTo::getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStamped& ik_pose_msg,
const planning_scene::PlanningScenePtr& scene,
Eigen::Affine3d& target_eigen, decltype(std::declval<SolutionBase>().markers())& markers)
Eigen::Isometry3d& target_eigen, decltype(std::declval<SolutionBase>().markers())& markers)
{
try {
const geometry_msgs::PoseStamped& target = boost::any_cast<geometry_msgs::PoseStamped>(goal);
tf::poseMsgToEigen(target.pose, target_eigen);
// transform target into global frame
const Eigen::Affine3d& frame = scene->getFrameTransform(target.header.frame_id);
const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id);
target_eigen = frame * target_eigen;
// frame at target pose
@ -130,7 +130,7 @@ bool MoveTo::getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStampe
bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
const planning_scene::PlanningScenePtr& scene,
Eigen::Affine3d& target_eigen, decltype(std::declval<SolutionBase>().markers())&)
Eigen::Isometry3d& target_eigen, decltype(std::declval<SolutionBase>().markers())&)
{
try {
const geometry_msgs::PointStamped& target = boost::any_cast<geometry_msgs::PointStamped>(goal);
@ -138,7 +138,7 @@ bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel*
tf::pointMsgToEigen(target.point, target_point);
// transform target into global frame
const Eigen::Affine3d& frame = scene->getFrameTransform(target.header.frame_id);
const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id);
target_point = frame * target_point;
// retain link orientation
@ -179,7 +179,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
} else { // Cartesian goal
const moveit::core::LinkModel* link;
Eigen::Affine3d target_eigen;
Eigen::Isometry3d target_eigen;
// Cartesian targets require an IK reference frame
geometry_msgs::PoseStamped ik_pose_msg;
@ -207,7 +207,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
}
// transform target pose such that ik frame will reach there if link does
Eigen::Affine3d ik_pose;
Eigen::Isometry3d ik_pose;
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
target_eigen = target_eigen * ik_pose.inverse();

View File

@ -143,7 +143,7 @@ void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model)
SerialContainer::init(robot_model);
}
void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) {
void SimpleGraspBase::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
tf::poseEigenToMsg(pose, pose_msg.pose);

View File

@ -55,7 +55,7 @@ TEST(PR2, pick) {
grasp_generator->setMonitoredStage(initial_stage);
auto grasp = std::make_unique<stages::SimpleGrasp>(std::unique_ptr<MonitoringGenerator>(grasp_generator));
grasp->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
grasp->setIKFrame(Eigen::Isometry3d::Identity(), "l_gripper_tool_frame");
// pick stage
auto pick = std::make_unique<stages::Pick>(std::move(grasp));

View File

@ -33,8 +33,8 @@ std_msgs::ColorRGBA& interpolate(std_msgs::ColorRGBA& color, const std_msgs::Col
std_msgs::ColorRGBA& brighten(std_msgs::ColorRGBA& color, double fraction);
std_msgs::ColorRGBA& darken(std_msgs::ColorRGBA& color, double fraction);
geometry_msgs::Pose composePoses(const geometry_msgs::Pose& first, const Eigen::Affine3d& second);
geometry_msgs::Pose composePoses(const Eigen::Affine3d& first, const geometry_msgs::Pose& second);
geometry_msgs::Pose composePoses(const geometry_msgs::Pose& first, const Eigen::Isometry3d& second);
geometry_msgs::Pose composePoses(const Eigen::Isometry3d& first, const geometry_msgs::Pose& second);
/** All routines only touch the geometry part of the marker
* pose, color, namespace, id, etc need to be set externally
@ -91,7 +91,7 @@ void appendFrame(T& container, const geometry_msgs::PoseStamped& pose, double sc
container.push_back(m);
// z-axis
m.pose = composePoses(pose.pose, Eigen::Translation3d(0, 0, scale / 2.0) * Eigen::Affine3d::Identity());
m.pose = composePoses(pose.pose, Eigen::Translation3d(0, 0, scale / 2.0) * Eigen::Isometry3d::Identity());
setColor(m.color, BLUE);
container.push_back(m);
}

View File

@ -134,18 +134,18 @@ std_msgs::ColorRGBA getColor(Color color, double alpha) {
return result;
}
geometry_msgs::Pose composePoses(const geometry_msgs::Pose& first, const Eigen::Affine3d& second) {
geometry_msgs::Pose composePoses(const geometry_msgs::Pose& first, const Eigen::Isometry3d& second) {
geometry_msgs::Pose result;
Eigen::Affine3d result_eigen;
Eigen::Isometry3d result_eigen;
tf::poseMsgToEigen(first, result_eigen);
result_eigen = result_eigen * second;
tf::poseEigenToMsg(result_eigen, result);
return result;
}
geometry_msgs::Pose composePoses(const Eigen::Affine3d& first, const geometry_msgs::Pose& second) {
geometry_msgs::Pose composePoses(const Eigen::Isometry3d& first, const geometry_msgs::Pose& second) {
geometry_msgs::Pose result;
Eigen::Affine3d result_eigen;
Eigen::Isometry3d result_eigen;
tf::poseMsgToEigen(second, result_eigen);
result_eigen = first * result_eigen;
tf::poseEigenToMsg(result_eigen, result);
@ -277,7 +277,7 @@ vm::Marker& makeArrow(vm::Marker &m, double scale, bool tip_at_origin) {
m.scale.x = scale;
prepareMarker(m, vm::Marker::ARROW);
if (tip_at_origin)
m.pose = composePoses(m.pose, Eigen::Translation3d(-scale, 0, 0) * Eigen::Affine3d::Identity());
m.pose = composePoses(m.pose, Eigen::Translation3d(-scale, 0, 0) * Eigen::Isometry3d::Identity());
return m;
}