mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
MoveTo: simplify getJointStateGoal
This commit is contained in:
parent
9e19f2187b
commit
bbd274da96
@ -91,7 +91,7 @@ public:
|
|||||||
protected:
|
protected:
|
||||||
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(moveit::core::RobotState& state, const core::RobotModelConstPtr& robot_model);
|
bool getJointStateGoal(moveit::core::RobotState& state);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
solvers::PlannerInterfacePtr planner_;
|
solvers::PlannerInterfacePtr planner_;
|
||||||
|
|||||||
@ -69,17 +69,16 @@ void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
|
|||||||
|
|
||||||
// Check if named_joint_pose is well-defined in JMG.
|
// Check if named_joint_pose is well-defined in JMG.
|
||||||
robot_state::RobotState state(robot_model);
|
robot_state::RobotState state(robot_model);
|
||||||
getJointStateGoal(state, robot_model);
|
getJointStateGoal(state);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MoveTo::getJointStateGoal(moveit::core::RobotState& state,
|
bool MoveTo::getJointStateGoal(moveit::core::RobotState& state) {
|
||||||
const robot_model::RobotModelConstPtr& robot_model) {
|
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
if (props.countDefined({"named_joint_pose", "joint_pose"}) == 2)
|
if (props.countDefined({"named_joint_pose", "joint_pose"}) == 2)
|
||||||
throw InitStageException(*this, "Cannot define both, named_joint_pose and joint_pose");
|
throw InitStageException(*this, "Cannot define both, named_joint_pose and joint_pose");
|
||||||
|
|
||||||
const std::string& group = props.get<std::string>("group");
|
const std::string& group = props.get<std::string>("group");
|
||||||
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
|
const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(group);
|
||||||
if (!jmg)
|
if (!jmg)
|
||||||
throw InitStageException(*this, "Unknown joint model group: " + group);
|
throw InitStageException(*this, "Unknown joint model group: " + group);
|
||||||
|
|
||||||
@ -132,7 +131,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
|
|||||||
|
|
||||||
try {
|
try {
|
||||||
size_t cartesian_goals = props.countDefined({"pose", "point"});
|
size_t cartesian_goals = props.countDefined({"pose", "point"});
|
||||||
if (getJointStateGoal(scene->getCurrentStateNonConst(), scene->getRobotModel())) {
|
if (getJointStateGoal(scene->getCurrentStateNonConst())) {
|
||||||
if (cartesian_goals > 0)
|
if (cartesian_goals > 0)
|
||||||
ROS_WARN_NAMED("MoveTo", "Ignoring Cartesian goals in favour of joint space goal");
|
ROS_WARN_NAMED("MoveTo", "Ignoring Cartesian goals in favour of joint space goal");
|
||||||
// plan to joint-space target
|
// plan to joint-space target
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user