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, bool plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link, const moveit::core::LinkModel &link,
const Eigen::Affine3d& target, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup *jmg, const moveit::core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr& result, robot_trajectory::RobotTrajectoryPtr& result,

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -72,14 +72,14 @@ public:
/// set properties of IK solver /// set properties of IK solver
void setIKFrame(const geometry_msgs::PoseStamped &transform) { setProperty("ik_frame", transform); } 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> template <typename T>
void setIKFrame(const T& t, const std::string& link) { void setIKFrame(const T& t, const std::string& link) {
Eigen::Affine3d transform; transform = t; Eigen::Isometry3d transform; transform = t;
setIKFrame(transform, link); setIKFrame(transform, link);
} }
void setIKFrame(const std::string& 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); } 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, bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link, const moveit::core::LinkModel &link,
const Eigen::Affine3d &target, const Eigen::Isometry3d &target,
const moveit::core::JointModelGroup *jmg, const moveit::core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr &result, 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, bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link, const moveit::core::LinkModel &link,
const Eigen::Affine3d& target_eigen, const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup *jmg, const moveit::core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr& result, 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, bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link, const moveit::core::LinkModel &link,
const Eigen::Affine3d& target_eigen, const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup *jmg, const moveit::core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr& result, 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"); 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; geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link; pose_msg.header.frame_id = link;
@ -76,7 +76,7 @@ void ComputeIK::setIKFrame(const Eigen::Affine3d &pose, const std::string &link)
setIKFrame(pose_msg); 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; geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = frame; 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: 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 // TODO: move into MoveIt! core, lift active_components_only_ from fcl to common interface
bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, 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) collision_detection::CollisionResult* collision_result = nullptr)
{ {
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst(); 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 if (target_pose_msg.header.frame_id.empty()) // if not provided, assume planning frame
target_pose_msg.header.frame_id = sandbox_scene->getPlanningFrame(); 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); tf::poseMsgToEigen(target_pose_msg.pose, target_pose);
if (target_pose_msg.header.frame_id != sandbox_scene->getPlanningFrame()) { if (target_pose_msg.header.frame_id != sandbox_scene->getPlanningFrame()) {
if (!sandbox_scene->knowsFrameTransform(target_pose_msg.header.frame_id)) { 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; ik_pose_msg.pose.orientation.w = 1.0;
} else { } else {
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value); 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); tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) { if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown link: " << 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); 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; 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; double current_angle_ = 0.0;
while (current_angle_ < 2.*M_PI && current_angle_ > -2.*M_PI) { while (current_angle_ < 2.*M_PI && current_angle_ > -2.*M_PI) {
// rotate object pose about z-axis // 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"); current_angle_ += props.get<double>("angle_delta");
InterfaceState state(scene); InterfaceState state(scene);

View File

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

View File

@ -58,7 +58,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
"constraints to maintain during trajectory"); "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; geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link; 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, bool MoveTo::getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStamped& ik_pose_msg,
const planning_scene::PlanningScenePtr& scene, 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 { try {
const geometry_msgs::PoseStamped& target = boost::any_cast<geometry_msgs::PoseStamped>(goal); const geometry_msgs::PoseStamped& target = boost::any_cast<geometry_msgs::PoseStamped>(goal);
tf::poseMsgToEigen(target.pose, target_eigen); tf::poseMsgToEigen(target.pose, target_eigen);
// transform target into global frame // 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; target_eigen = frame * target_eigen;
// frame at target pose // 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, bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
const planning_scene::PlanningScenePtr& scene, const planning_scene::PlanningScenePtr& scene,
Eigen::Affine3d& target_eigen, decltype(std::declval<SolutionBase>().markers())&) Eigen::Isometry3d& target_eigen, decltype(std::declval<SolutionBase>().markers())&)
{ {
try { try {
const geometry_msgs::PointStamped& target = boost::any_cast<geometry_msgs::PointStamped>(goal); 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); tf::pointMsgToEigen(target.point, target_point);
// transform target into global frame // 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; target_point = frame * target_point;
// retain link orientation // 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); success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
} else { // Cartesian goal } else { // Cartesian goal
const moveit::core::LinkModel* link; const moveit::core::LinkModel* link;
Eigen::Affine3d target_eigen; Eigen::Isometry3d target_eigen;
// Cartesian targets require an IK reference frame // Cartesian targets require an IK reference frame
geometry_msgs::PoseStamped ik_pose_msg; 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 // 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); tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
target_eigen = target_eigen * ik_pose.inverse(); 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); 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; geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link; pose_msg.header.frame_id = link;
tf::poseEigenToMsg(pose, pose_msg.pose); tf::poseEigenToMsg(pose, pose_msg.pose);

View File

@ -55,7 +55,7 @@ TEST(PR2, pick) {
grasp_generator->setMonitoredStage(initial_stage); grasp_generator->setMonitoredStage(initial_stage);
auto grasp = std::make_unique<stages::SimpleGrasp>(std::unique_ptr<MonitoringGenerator>(grasp_generator)); 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 // pick stage
auto pick = std::make_unique<stages::Pick>(std::move(grasp)); 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& brighten(std_msgs::ColorRGBA& color, double fraction);
std_msgs::ColorRGBA& darken(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 geometry_msgs::Pose& first, const Eigen::Isometry3d& second);
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);
/** All routines only touch the geometry part of the marker /** All routines only touch the geometry part of the marker
* pose, color, namespace, id, etc need to be set externally * 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); container.push_back(m);
// z-axis // 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); setColor(m.color, BLUE);
container.push_back(m); container.push_back(m);
} }

View File

@ -134,18 +134,18 @@ std_msgs::ColorRGBA getColor(Color color, double alpha) {
return result; 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; geometry_msgs::Pose result;
Eigen::Affine3d result_eigen; Eigen::Isometry3d result_eigen;
tf::poseMsgToEigen(first, result_eigen); tf::poseMsgToEigen(first, result_eigen);
result_eigen = result_eigen * second; result_eigen = result_eigen * second;
tf::poseEigenToMsg(result_eigen, result); tf::poseEigenToMsg(result_eigen, result);
return 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; geometry_msgs::Pose result;
Eigen::Affine3d result_eigen; Eigen::Isometry3d result_eigen;
tf::poseMsgToEigen(second, result_eigen); tf::poseMsgToEigen(second, result_eigen);
result_eigen = first * result_eigen; result_eigen = first * result_eigen;
tf::poseEigenToMsg(result_eigen, result); 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; m.scale.x = scale;
prepareMarker(m, vm::Marker::ARROW); prepareMarker(m, vm::Marker::ARROW);
if (tip_at_origin) 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; return m;
} }