mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Overload setGoal to accept map of joint values as argument (#87)
This commit is contained in:
parent
fee73bcd78
commit
267c214288
@ -95,6 +95,9 @@ public:
|
|||||||
setProperty("goal", robot_state);
|
setProperty("goal", robot_state);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// move joints by name to their mapped target value
|
||||||
|
void setGoal(const std::map<std::string, double>& joints);
|
||||||
|
|
||||||
void setPathConstraints(moveit_msgs::Constraints path_constraints){
|
void setPathConstraints(moveit_msgs::Constraints path_constraints){
|
||||||
setProperty("path_constraints", std::move(path_constraints));
|
setProperty("path_constraints", std::move(path_constraints));
|
||||||
}
|
}
|
||||||
|
|||||||
@ -71,6 +71,20 @@ void MoveTo::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link)
|
|||||||
setIKFrame(pose_msg);
|
setIKFrame(pose_msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void MoveTo::setGoal(const std::map<std::string, double>& joints) {
|
||||||
|
moveit_msgs::RobotState robot_state;
|
||||||
|
robot_state.joint_state.name.reserve(joints.size());
|
||||||
|
robot_state.joint_state.position.reserve(joints.size());
|
||||||
|
|
||||||
|
for (auto& joint : joints)
|
||||||
|
{
|
||||||
|
robot_state.joint_state.name.push_back(joint.first);
|
||||||
|
robot_state.joint_state.position.push_back(joint.second);
|
||||||
|
}
|
||||||
|
robot_state.is_diff = true;
|
||||||
|
setProperty("goal", robot_state);
|
||||||
|
}
|
||||||
|
|
||||||
void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
|
void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
PropagatingEitherWay::init(robot_model);
|
PropagatingEitherWay::init(robot_model);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user