mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Replace namespace robot_[model|state] with moveit::core
This commit is contained in:
parent
6f7282423d
commit
c605a0059a
@ -130,9 +130,9 @@ void ExecuteTaskSolutionCapability::preemptCallback() {
|
||||
|
||||
bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution,
|
||||
plan_execution::ExecutableMotionPlan& plan) {
|
||||
robot_model::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
|
||||
moveit::core::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
|
||||
|
||||
robot_state::RobotState state(model);
|
||||
moveit::core::RobotState state(model);
|
||||
{
|
||||
planning_scene_monitor::LockedPlanningSceneRO scene(context_->planning_scene_monitor_);
|
||||
state = scene->getCurrentState();
|
||||
|
||||
@ -105,7 +105,7 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
|
||||
|
||||
robot_trajectory::RobotTrajectoryPtr
|
||||
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||
const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group,
|
||||
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group,
|
||||
const trajectory_processing::TimeParameterization& time_parameterization) {
|
||||
if (sub_trajectories.size() <= 1)
|
||||
throw std::runtime_error("Expected multiple sub solutions");
|
||||
@ -141,7 +141,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
|
||||
std::vector<double> values;
|
||||
values.reserve(max_num_vars);
|
||||
|
||||
auto merged_state = std::make_shared<robot_state::RobotState>(base_state);
|
||||
auto merged_state = std::make_shared<moveit::core::RobotState>(base_state);
|
||||
while (true) {
|
||||
bool finished = true;
|
||||
size_t index = merged_traj->getWayPointCount();
|
||||
@ -151,7 +151,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
|
||||
continue; // no more waypoints in this sub solution
|
||||
|
||||
finished = false; // there was a waypoint, continue while loop
|
||||
const robot_state::RobotState& sub_state = sub->getWayPoint(index);
|
||||
const moveit::core::RobotState& sub_state = sub->getWayPoint(index);
|
||||
sub_state.copyJointGroupPositions(sub->getGroup(), values);
|
||||
merged_state->setJointGroupPositions(sub->getGroup(), values);
|
||||
}
|
||||
@ -162,7 +162,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
|
||||
// add waypoint without timing
|
||||
merged_traj->addSuffixWayPoint(merged_state, 0.0);
|
||||
// create new RobotState for next waypoint
|
||||
merged_state = std::make_shared<robot_state::RobotState>(*merged_state);
|
||||
merged_state = std::make_shared<moveit::core::RobotState>(*merged_state);
|
||||
}
|
||||
|
||||
// add timing
|
||||
|
||||
@ -89,7 +89,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
|
||||
const double* joint_positions) {
|
||||
state->setJointGroupPositions(jmg, joint_positions);
|
||||
state->update();
|
||||
return !sandbox_scene->isStateColliding(const_cast<const robot_state::RobotState&>(*state), jmg->getName()) &&
|
||||
return !sandbox_scene->isStateColliding(const_cast<const moveit::core::RobotState&>(*state), jmg->getName()) &&
|
||||
kcs.decide(*state).satisfied;
|
||||
};
|
||||
|
||||
|
||||
@ -52,10 +52,10 @@ struct PlannerCache
|
||||
{
|
||||
using PlannerID = std::tuple<std::string, std::string>;
|
||||
using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline> >;
|
||||
using ModelList = std::list<std::pair<std::weak_ptr<const robot_model::RobotModel>, PlannerMap> >;
|
||||
using ModelList = std::list<std::pair<std::weak_ptr<const moveit::core::RobotModel>, PlannerMap> >;
|
||||
ModelList cache_;
|
||||
|
||||
PlannerMap::mapped_type& retrieve(const robot_model::RobotModelConstPtr& model, const PlannerID& id) {
|
||||
PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& id) {
|
||||
// find model in cache_ and remove expired entries while doing so
|
||||
ModelList::iterator model_it = cache_.begin();
|
||||
while (model_it != cache_.end()) {
|
||||
|
||||
@ -98,11 +98,11 @@ namespace {
|
||||
// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though...
|
||||
// TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface
|
||||
bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene,
|
||||
robot_state::RobotState& robot_state, Eigen::Isometry3d pose,
|
||||
const robot_model::LinkModel* link, const robot_model::JointModelGroup* jmg = nullptr,
|
||||
moveit::core::RobotState& robot_state, Eigen::Isometry3d pose,
|
||||
const moveit::core::LinkModel* link, const moveit::core::JointModelGroup* jmg = nullptr,
|
||||
collision_detection::CollisionResult* collision_result = nullptr) {
|
||||
// consider all rigidly connected parent links as well
|
||||
const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link);
|
||||
const moveit::core::LinkModel* parent = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link);
|
||||
if (parent != link) // transform pose into pose suitable to place parent
|
||||
pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent);
|
||||
|
||||
@ -116,10 +116,10 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce
|
||||
while (parent) {
|
||||
pending_links.push_back(&parent->getName());
|
||||
link = parent;
|
||||
const robot_model::JointModel* joint = link->getParentJointModel();
|
||||
const moveit::core::JointModel* joint = link->getParentJointModel();
|
||||
parent = joint->getParentLinkModel();
|
||||
|
||||
if (joint->getType() != robot_model::JointModel::FIXED) {
|
||||
if (joint->getType() != moveit::core::JointModel::FIXED) {
|
||||
for (const std::string* name : pending_links)
|
||||
acm.setDefaultEntry(*name, true);
|
||||
pending_links.clear();
|
||||
@ -280,7 +280,7 @@ void ComputeIK::compute() {
|
||||
}
|
||||
|
||||
// determine IK link from ik_frame
|
||||
const robot_model::LinkModel* link = nullptr;
|
||||
const moveit::core::LinkModel* link = nullptr;
|
||||
geometry_msgs::PoseStamped ik_pose_msg;
|
||||
const boost::any& value = props.get("ik_frame");
|
||||
if (value.empty()) { // property undefined
|
||||
@ -311,7 +311,7 @@ void ComputeIK::compute() {
|
||||
|
||||
// validate placed link for collisions
|
||||
collision_detection::CollisionResult collisions;
|
||||
robot_state::RobotState sandbox_state{ scene->getCurrentState() };
|
||||
moveit::core::RobotState sandbox_state{ scene->getCurrentState() };
|
||||
bool colliding =
|
||||
!ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, jmg, &collisions);
|
||||
|
||||
@ -349,7 +349,7 @@ void ComputeIK::compute() {
|
||||
std::vector<double> compare_pose;
|
||||
const std::string& compare_pose_name = props.get<std::string>("default_pose");
|
||||
if (!compare_pose_name.empty()) {
|
||||
robot_state::RobotState compare_state(robot_model);
|
||||
moveit::core::RobotState compare_state(robot_model);
|
||||
compare_state.setToDefaultValues(jmg, compare_pose_name);
|
||||
compare_state.copyJointGroupPositions(jmg, compare_pose);
|
||||
} else
|
||||
@ -359,7 +359,7 @@ void ComputeIK::compute() {
|
||||
|
||||
IKSolutions ik_solutions;
|
||||
auto is_valid = [scene, ignore_collisions, min_solution_distance,
|
||||
&ik_solutions](robot_state::RobotState* state, const robot_model::JointModelGroup* jmg,
|
||||
&ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg,
|
||||
const double* joint_positions) {
|
||||
for (const auto& sol : ik_solutions) {
|
||||
if (jmg->distance(joint_positions, sol.joint_positions.data()) < min_solution_distance)
|
||||
@ -419,7 +419,7 @@ void ComputeIK::compute() {
|
||||
solution.markAsFailure(ss.str());
|
||||
}
|
||||
// set scene's robot state
|
||||
robot_state::RobotState& solution_state = solution_scene->getCurrentStateNonConst();
|
||||
moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst();
|
||||
solution_state.setJointGroupPositions(jmg, ik_solutions[i].joint_positions.data());
|
||||
solution_state.update();
|
||||
|
||||
|
||||
@ -152,7 +152,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
planning_scene::PlanningScenePtr end = start->diff();
|
||||
const moveit::core::JointModelGroup* jmg = final_goal_state.getJointModelGroup(pair.first);
|
||||
final_goal_state.copyJointGroupPositions(jmg, positions);
|
||||
robot_state::RobotState& goal_state = end->getCurrentStateNonConst();
|
||||
moveit::core::RobotState& goal_state = end->getCurrentStateNonConst();
|
||||
goal_state.setJointGroupPositions(jmg, positions);
|
||||
goal_state.update();
|
||||
intermediate_scenes.push_back(end);
|
||||
|
||||
@ -150,7 +150,7 @@ void GenerateGraspPose::compute() {
|
||||
const std::string& eef = props.get<std::string>("eef");
|
||||
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef);
|
||||
|
||||
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
|
||||
moveit::core::RobotState& robot_state = scene->getCurrentStateNonConst();
|
||||
try {
|
||||
applyPreGrasp(robot_state, jmg, props.property("pregrasp"));
|
||||
} catch (const moveit::Exception& e) {
|
||||
|
||||
@ -166,7 +166,7 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Inter
|
||||
bool MoveRelative::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene,
|
||||
SubTrajectory& solution, Interface::Direction dir) {
|
||||
scene = state.scene()->diff();
|
||||
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
|
||||
const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
|
||||
assert(robot_model);
|
||||
|
||||
const auto& props = properties();
|
||||
@ -282,7 +282,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||
|
||||
if (robot_trajectory) { // the following requires a robot_trajectory returned from planning
|
||||
robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
||||
moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
||||
reached_state->updateLinkTransforms();
|
||||
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset;
|
||||
|
||||
|
||||
@ -177,7 +177,7 @@ bool MoveTo::getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_po
|
||||
bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& solution,
|
||||
Interface::Direction dir) {
|
||||
scene = state.scene()->diff();
|
||||
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
|
||||
const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
|
||||
assert(robot_model);
|
||||
|
||||
const auto& props = properties();
|
||||
|
||||
@ -120,7 +120,7 @@ void TaskDisplay::loadRobotModel() {
|
||||
|
||||
const srdf::ModelSharedPtr& srdf =
|
||||
rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model());
|
||||
robot_model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf));
|
||||
robot_model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf));
|
||||
|
||||
// Send to child class
|
||||
trajectory_visual_->onRobotModelLoaded(robot_model_);
|
||||
|
||||
@ -65,7 +65,7 @@ float DisplaySolution::getWayPointDurationFromPrevious(const IndexPair& idx_pair
|
||||
return data_[idx_pair.first].trajectory_->getWayPointDurationFromPrevious(idx_pair.second);
|
||||
}
|
||||
|
||||
const robot_state::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPair& idx_pair) const {
|
||||
const moveit::core::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPair& idx_pair) const {
|
||||
return data_[idx_pair.first].trajectory_->getWayPointPtr(idx_pair.second);
|
||||
}
|
||||
|
||||
|
||||
@ -209,7 +209,7 @@ void TaskSolutionVisualization::setName(const QString& name) {
|
||||
slider_dock_panel_->setWindowTitle(name + " - Slider");
|
||||
}
|
||||
|
||||
void TaskSolutionVisualization::onRobotModelLoaded(const robot_model::RobotModelConstPtr& robot_model) {
|
||||
void TaskSolutionVisualization::onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model) {
|
||||
// Error check
|
||||
if (!robot_model) {
|
||||
ROS_ERROR_STREAM_NAMED("task_solution_visualization", "No robot model found");
|
||||
|
||||
Loading…
Reference in New Issue
Block a user