mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Eigen::Affine3 -> Eigen::Isometry3
This commit is contained in:
parent
5690d68311
commit
ae75c3aa7f
@ -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,
|
||||||
|
@ -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,
|
||||||
|
@ -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,
|
||||||
|
@ -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,
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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
|
||||||
|
@ -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:
|
||||||
|
@ -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); }
|
||||||
|
@ -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,
|
||||||
|
@ -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,
|
||||||
|
@ -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,
|
||||||
|
@ -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);
|
||||||
|
@ -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)));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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);
|
||||||
|
@ -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();
|
||||||
|
@ -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();
|
||||||
|
|
||||||
|
@ -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);
|
||||||
|
@ -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));
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user