MoveTo: store goal as any type

This commit is contained in:
Robert Haschke 2018-06-01 06:56:03 +02:00
parent 941dee7fc1
commit 00260d62b5
2 changed files with 79 additions and 87 deletions

View File

@ -76,22 +76,22 @@ public:
/// move link to given pose
void setGoal(const geometry_msgs::PoseStamped& pose) {
setProperty("pose", pose);
setProperty("goal", pose);
}
/// move link to given point, keeping current orientation
void setGoal(const geometry_msgs::PointStamped& point) {
setProperty("point", point);
setProperty("goal", point);
}
/// move joint model group to given named pose
void setGoal(const std::string& named_joint_pose) {
setProperty("named_joint_pose", named_joint_pose);
setProperty("goal", named_joint_pose);
}
/// move joints specified in msg to their target values
void setGoal(const moveit_msgs::RobotState& robot_state) {
setProperty("joint_pose", robot_state);
setProperty("goal", robot_state);
}
void setPathConstraints(moveit_msgs::Constraints path_constraints){
@ -102,7 +102,13 @@ protected:
// return false if trajectory shouldn't be stored
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
SubTrajectory &trajectory, Direction dir);
bool getJointStateGoal(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,
const planning_scene::PlanningScenePtr& scene, Eigen::Affine3d& 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,
decltype(std::declval<SolutionBase>().markers())&);
protected:
solvers::PlannerInterfacePtr planner_;

View File

@ -52,11 +52,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
auto& p = properties();
p.declare<std::string>("group", "name of planning group");
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
p.declare<geometry_msgs::PoseStamped>("pose", "Cartesian target pose");
p.declare<geometry_msgs::PointStamped>("point", "Cartesian target point");
p.declare<std::string>("named_joint_pose", "named joint pose");
p.declare<moveit_msgs::RobotState>("joint_pose", "joint pose");
p.declare<boost::any>("goal", "goal specification");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
"constraints to maintain during trajectory");
@ -74,32 +70,19 @@ void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
{
PropagatingEitherWay::init(robot_model);
planner_->init(robot_model);
// Check if named_joint_pose is well-defined in JMG.
robot_state::RobotState state(robot_model);
getJointStateGoal(state);
}
bool MoveTo::getJointStateGoal(moveit::core::RobotState& state) {
const auto& props = properties();
if (props.countDefined({"named_joint_pose", "joint_pose"}) == 2)
throw InitStageException(*this, "Cannot define both, named_joint_pose and joint_pose");
const std::string& group = props.get<std::string>("group");
const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(group);
if (!jmg)
throw InitStageException(*this, "Unknown joint model group: " + group);
boost::any goal = props.get("named_joint_pose");
if (!goal.empty()) {
bool MoveTo::getJointStateGoal(const boost::any& goal,
const moveit::core::JointModelGroup* jmg,
moveit::core::RobotState& state) {
try {
// try named joint pose
const std::string& named_joint_pose = boost::any_cast<std::string>(goal);
if (!state.setToDefaultValues(jmg, named_joint_pose))
throw InitStageException(*this, "Unknown joint pose: " + named_joint_pose);
return true;
}
goal = props.get("joint_pose");
if (!goal.empty()) {
// try RobotState
const moveit_msgs::RobotState& msg = boost::any_cast<moveit_msgs::RobotState>(goal);
if (!msg.is_diff)
throw InitStageException(*this, "Expecting a diff state");
@ -108,17 +91,65 @@ bool MoveTo::getJointStateGoal(moveit::core::RobotState& state) {
const auto& accepted = jmg->getJointModelNames();
for (const auto &name : msg.joint_state.name)
if (std::find(accepted.begin(), accepted.end(), name) == accepted.end())
throw InitStageException(*this, "Joint '" + name + "' is not part of group '" + group + "'");
throw InitStageException(*this, "Joint '" + name + "' is not part of group '" + jmg->getName() + "'");
for (const auto &name : msg.multi_dof_joint_state.joint_names)
if (std::find(accepted.begin(), accepted.end(), name) == accepted.end())
throw InitStageException(*this, "Joint '" + name + "' is not part of group '" + group + "'");
throw InitStageException(*this, "Joint '" + name + "' is not part of group '" + jmg->getName() + "'");
moveit::core::robotStateMsgToRobotState(msg, state, false);
return true;
} catch (const boost::bad_any_cast&) {
return false;
}
return false;
}
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)
{
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);
target_eigen = frame * target_eigen;
// frame at target pose
rviz_marker_tools::appendFrame(markers, target, 0.1, "ik frame");
// frame at link
rviz_marker_tools::appendFrame(markers, ik_pose_msg, 0.1, "ik frame");
} catch (const boost::bad_any_cast&) {
return false;
}
return true;
}
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())&)
{
try {
const geometry_msgs::PointStamped& target = boost::any_cast<geometry_msgs::PointStamped>(goal);
Eigen::Vector3d target_point;
tf::pointMsgToEigen(target.point, target_point);
// transform target into global frame
const Eigen::Affine3d& frame = scene->getFrameTransform(target.header.frame_id);
target_point = frame * target_point;
// retain link orientation
target_eigen = scene->getCurrentState().getGlobalLinkTransform(link);
target_eigen.translation() = target_point;
} catch (const boost::bad_any_cast&) {
return false;
}
return true;
}
bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
SubTrajectory &solution, Direction dir) {
scene = state.scene()->diff();
@ -133,40 +164,20 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
ROS_WARN_STREAM_NAMED("MoveTo", "Invalid joint model group: " << group);
return false;
}
boost::any goal = props.get("goal");
if (goal.empty()) {
ROS_WARN_NAMED("MoveTo", "goal undefined");
return false;
}
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
bool success = false;
bool has_joint_state_goal;
try {
has_joint_state_goal = getJointStateGoal(scene->getCurrentStateNonConst());
} catch (const InitStageException &e) {
ROS_WARN_STREAM_NAMED("MoveTo", e.what());
return false;
}
size_t cartesian_goals = props.countDefined({"pose", "point"});
if (cartesian_goals > 1) {
ROS_WARN_NAMED("MoveTo", "Ambiguous goals: Multiple Cartesian goals defined");
return false;
}
if (cartesian_goals > 0 && has_joint_state_goal) {
ROS_WARN_NAMED("MoveTo", "Ambiguous goals: Cartesian goals and joint state goals defined");
return false;
}
if (cartesian_goals == 0 && !has_joint_state_goal) {
ROS_WARN_NAMED("MoveTo", "No goal defined");
return false;
}
if (has_joint_state_goal) {
if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
// plan to joint-space target
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
} else if (cartesian_goals == 1) {
} else { // Cartesian goal
const moveit::core::LinkModel* link;
Eigen::Affine3d target_eigen;
@ -189,35 +200,10 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
}
}
boost::any goal = props.get("pose");
if (!goal.empty()) {
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);
target_eigen = frame * target_eigen;
// frame at target pose
rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame");
// frame at link
rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
}
goal = props.get("point");
if (!goal.empty()) {
const geometry_msgs::PointStamped& target = boost::any_cast<geometry_msgs::PointStamped>(goal);
Eigen::Vector3d target_point;
tf::pointMsgToEigen(target.point, target_point);
// transform target into global frame
const Eigen::Affine3d& frame = scene->getFrameTransform(target.header.frame_id);
target_point = frame * target_point;
// retain link orientation
target_eigen = scene->getCurrentState().getGlobalLinkTransform(link);
target_eigen.translation() = target_point;
if (!getPoseGoal(goal, ik_pose_msg, scene, target_eigen, solution.markers()) &&
!getPointGoal(goal, link, scene, target_eigen, solution.markers())) {
ROS_ERROR_STREAM_NAMED("MoveTo", "Invalid type for goal: " << goal.type().name());
return false;
}
// transform target pose such that ik frame will reach there if link does