mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
MoveRelative: store goal as any type
This commit is contained in:
parent
00260d62b5
commit
544f574166
@ -92,7 +92,7 @@ int main(int argc, char** argv){
|
|||||||
geometry_msgs::Vector3Stamped direction;
|
geometry_msgs::Vector3Stamped direction;
|
||||||
direction.header.frame_id = "lh_tool_frame";
|
direction.header.frame_id = "lh_tool_frame";
|
||||||
direction.vector.z = 1;
|
direction.vector.z = 1;
|
||||||
move->along(direction);
|
move->setGoal(direction);
|
||||||
t.add(std::move(move));
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -146,7 +146,7 @@ int main(int argc, char** argv){
|
|||||||
geometry_msgs::Vector3Stamped direction;
|
geometry_msgs::Vector3Stamped direction;
|
||||||
direction.header.frame_id = "world";
|
direction.header.frame_id = "world";
|
||||||
direction.vector.z = 1;
|
direction.vector.z = 1;
|
||||||
move->along(direction);
|
move->setGoal(direction);
|
||||||
t.add(std::move(move));
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -161,7 +161,7 @@ int main(int argc, char** argv){
|
|||||||
twist.header.frame_id = "object";
|
twist.header.frame_id = "object";
|
||||||
twist.twist.linear.y = 1;
|
twist.twist.linear.y = 1;
|
||||||
twist.twist.angular.y = 2;
|
twist.twist.angular.y = 2;
|
||||||
move->along(twist);
|
move->setGoal(twist);
|
||||||
t.add(std::move(move));
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -44,6 +44,9 @@
|
|||||||
#include <geometry_msgs/TwistStamped.h>
|
#include <geometry_msgs/TwistStamped.h>
|
||||||
#include <geometry_msgs/Vector3Stamped.h>
|
#include <geometry_msgs/Vector3Stamped.h>
|
||||||
|
|
||||||
|
namespace moveit { namespace core {
|
||||||
|
class RobotState;
|
||||||
|
} }
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
/** Perform a Cartesian motion relative to some link */
|
/** Perform a Cartesian motion relative to some link */
|
||||||
@ -89,23 +92,24 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// perform twist motion on specified link
|
/// perform twist motion on specified link
|
||||||
void along(const geometry_msgs::TwistStamped& twist) {
|
void setGoal(const geometry_msgs::TwistStamped& twist) {
|
||||||
setProperty("twist", twist);
|
setProperty("goal", twist);
|
||||||
}
|
}
|
||||||
/// translate link along given direction
|
/// translate link along given direction
|
||||||
void along(const geometry_msgs::Vector3Stamped& direction) {
|
void setGoal(const geometry_msgs::Vector3Stamped& direction) {
|
||||||
setProperty("direction", direction);
|
setProperty("goal", direction);
|
||||||
}
|
}
|
||||||
/// move specified joint variables by given amount
|
/// move specified joint variables by given amount
|
||||||
void about(const std::map<std::string, double>& goal) {
|
void setGoal(const std::map<std::string, double>& joint_deltas) {
|
||||||
setProperty("joints", goal);
|
setProperty("goal", joint_deltas);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// return false if trajectory shouldn't be stored
|
// return false if trajectory shouldn't be stored
|
||||||
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
|
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
|
||||||
SubTrajectory &trajectory, Direction dir);
|
SubTrajectory &trajectory, Direction dir);
|
||||||
|
bool getJointStateGoal(const boost::any& goal, const moveit::core::JointModelGroup* jmg,
|
||||||
|
moveit::core::RobotState& robot_state);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
solvers::PlannerInterfacePtr planner_;
|
solvers::PlannerInterfacePtr planner_;
|
||||||
|
@ -51,13 +51,11 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
|
|||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<std::string>("group", "name of planning group");
|
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>("ik_frame", "frame to be moved towards goal pose");
|
||||||
|
|
||||||
|
p.declare<boost::any>("goal", "motion specification");
|
||||||
p.declare<double>("min_distance", -1.0, "minimum distance to move");
|
p.declare<double>("min_distance", -1.0, "minimum distance to move");
|
||||||
p.declare<double>("max_distance", 0.0, "maximum distance to move");
|
p.declare<double>("max_distance", 0.0, "maximum distance to move");
|
||||||
|
|
||||||
p.declare<geometry_msgs::TwistStamped>("twist", "Cartesian twist transform");
|
|
||||||
p.declare<geometry_msgs::Vector3Stamped>("direction", "Cartesian translation direction");
|
|
||||||
p.declare<std::map<std::string, double>>("joints", "Relative joint space goal");
|
|
||||||
|
|
||||||
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
|
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
|
||||||
"constraints to maintain during trajectory");
|
"constraints to maintain during trajectory");
|
||||||
}
|
}
|
||||||
@ -76,6 +74,30 @@ void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
|
|||||||
planner_->init(robot_model);
|
planner_->init(robot_model);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool MoveRelative::getJointStateGoal(const boost::any& goal,
|
||||||
|
const moveit::core::JointModelGroup* jmg,
|
||||||
|
moveit::core::RobotState& robot_state) {
|
||||||
|
try {
|
||||||
|
const auto& accepted = jmg->getJointModelNames();
|
||||||
|
const auto& joints = boost::any_cast<std::map<std::string, double>>(goal);
|
||||||
|
for (const auto& j : joints) {
|
||||||
|
int index = robot_state.getRobotModel()->getVariableIndex(j.first);
|
||||||
|
auto jm = robot_state.getRobotModel()->getJointModel(index);
|
||||||
|
if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end())
|
||||||
|
throw std::runtime_error("Cannot plan for joint '" + j.first + "' that is not part of group '" + jmg->getName() + "'");
|
||||||
|
|
||||||
|
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
|
||||||
|
robot_state.enforceBounds(jm);
|
||||||
|
}
|
||||||
|
robot_state.update();
|
||||||
|
return true;
|
||||||
|
} catch (const boost::bad_any_cast&) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
|
bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
|
||||||
SubTrajectory &solution, Direction dir) {
|
SubTrajectory &solution, Direction dir) {
|
||||||
scene = state.scene()->diff();
|
scene = state.scene()->diff();
|
||||||
@ -90,12 +112,9 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
ROS_WARN_STREAM_NAMED("MoveRelative", "Invalid joint model group: " << group);
|
ROS_WARN_STREAM_NAMED("MoveRelative", "Invalid joint model group: " << group);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
boost::any goal = props.get("goal");
|
||||||
// only allow single target
|
if (goal.empty()) {
|
||||||
size_t count_goals = props.countDefined({"twist", "direction", "joints"});
|
ROS_WARN_NAMED("MoveRelative", "goal undefined");
|
||||||
if (count_goals != 1) {
|
|
||||||
if (count_goals == 0) ROS_WARN_NAMED("MoveRelative", "No goal defined");
|
|
||||||
else ROS_WARN_NAMED("MoveRelative", "Cannot plan to multiple goals");
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -106,22 +125,8 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
||||||
bool success = false;
|
bool success = false;
|
||||||
|
|
||||||
boost::any goal = props.get("joints");
|
if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
|
||||||
if (!goal.empty()) {
|
// plan to joint-space target
|
||||||
const auto& accepted = jmg->getJointModelNames();
|
|
||||||
auto& robot_state = scene->getCurrentStateNonConst();
|
|
||||||
const auto& joints = boost::any_cast<std::map<std::string, double>>(goal);
|
|
||||||
for (const auto& j : joints) {
|
|
||||||
int index = robot_state.getRobotModel()->getVariableIndex(j.first);
|
|
||||||
auto jm = robot_model->getJointModel(index);
|
|
||||||
if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end()) {
|
|
||||||
ROS_WARN_STREAM_NAMED("MoveRelative", "Cannot plan joint target for joint '" << j.first << "' that is not part of group '" << group << "'");
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
|
|
||||||
robot_state.enforceBounds(jm);
|
|
||||||
}
|
|
||||||
robot_state.update();
|
|
||||||
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 {
|
} else {
|
||||||
// Cartesian targets require an IK reference frame
|
// Cartesian targets require an IK reference frame
|
||||||
@ -152,8 +157,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
Eigen::Affine3d target_eigen;
|
Eigen::Affine3d target_eigen;
|
||||||
Eigen::Affine3d link_pose = scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success
|
Eigen::Affine3d link_pose = scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success
|
||||||
|
|
||||||
boost::any goal = props.get("twist");
|
try { // try to extract Twist goal
|
||||||
if (!goal.empty()) {
|
|
||||||
const geometry_msgs::TwistStamped& target = boost::any_cast<geometry_msgs::TwistStamped>(goal);
|
const geometry_msgs::TwistStamped& target = boost::any_cast<geometry_msgs::TwistStamped>(goal);
|
||||||
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
|
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
|
||||||
tf::vectorMsgToEigen(target.twist.linear, linear);
|
tf::vectorMsgToEigen(target.twist.linear, linear);
|
||||||
@ -189,10 +193,10 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
target_eigen = link_pose;
|
target_eigen = link_pose;
|
||||||
target_eigen.linear() = target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular);
|
target_eigen.linear() = target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular);
|
||||||
target_eigen.translation() += linear;
|
target_eigen.translation() += linear;
|
||||||
}
|
goto COMPUTE;
|
||||||
|
} catch (const boost::bad_any_cast&) { /* continue with Vector goal */ }
|
||||||
|
|
||||||
goal = props.get("direction");
|
try { // try to extract Vector goal
|
||||||
if (!goal.empty()) {
|
|
||||||
const geometry_msgs::Vector3Stamped& target = boost::any_cast<geometry_msgs::Vector3Stamped>(goal);
|
const geometry_msgs::Vector3Stamped& target = boost::any_cast<geometry_msgs::Vector3Stamped>(goal);
|
||||||
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
|
const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
|
||||||
tf::vectorMsgToEigen(target.vector, linear);
|
tf::vectorMsgToEigen(target.vector, linear);
|
||||||
@ -212,8 +216,12 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
linear = frame_pose.linear() * linear;
|
linear = frame_pose.linear() * linear;
|
||||||
target_eigen = link_pose;
|
target_eigen = link_pose;
|
||||||
target_eigen.translation() += linear;
|
target_eigen.translation() += linear;
|
||||||
|
} catch (const boost::bad_any_cast&) {
|
||||||
|
ROS_ERROR_STREAM_NAMED("MoveRelative", "Invalid type for goal: " << goal.type().name());
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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::Affine3d ik_pose;
|
||||||
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
|
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
|
||||||
|
@ -81,7 +81,7 @@ void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model)
|
|||||||
void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
|
void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
|
||||||
{
|
{
|
||||||
auto& p = approach_stage_->properties();
|
auto& p = approach_stage_->properties();
|
||||||
p.set("twist", motion);
|
p.set("goal", motion);
|
||||||
p.set("min_distance", min_distance);
|
p.set("min_distance", min_distance);
|
||||||
p.set("max_distance", max_distance);
|
p.set("max_distance", max_distance);
|
||||||
}
|
}
|
||||||
@ -89,7 +89,7 @@ void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion
|
|||||||
void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
|
void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
|
||||||
{
|
{
|
||||||
auto& p = lift_stage_->properties();
|
auto& p = lift_stage_->properties();
|
||||||
p.set("twist", motion);
|
p.set("goal", motion);
|
||||||
p.set("min_distance", min_distance);
|
p.set("min_distance", min_distance);
|
||||||
p.set("max_distance", max_distance);
|
p.set("max_distance", max_distance);
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user