mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
MoveTo: store goal as any type
This commit is contained in:
parent
941dee7fc1
commit
00260d62b5
@ -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_;
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user