mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
establish utils namespace
leaves us a place to put free helper functions
This commit is contained in:
parent
48959c6806
commit
f1fc447e3b
@ -57,18 +57,3 @@
|
||||
#define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6)
|
||||
|
||||
#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6)
|
||||
|
||||
#if !MOVEIT_HAS_STATE_RIGID_PARENT_LINK
|
||||
#include <moveit/robot_state/robot_state.h>
|
||||
inline const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
|
||||
std::string frame) {
|
||||
const moveit::core::LinkModel* link{ nullptr };
|
||||
|
||||
if (state.hasAttachedBody(frame)) {
|
||||
link = state.getAttachedBody(frame)->getAttachedLink();
|
||||
} else if (state.getRobotModel()->hasLinkModel(frame))
|
||||
link = state.getLinkModel(frame);
|
||||
|
||||
return state.getRobotModel()->getRigidlyConnectedParentLinkModel(link);
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -84,7 +84,7 @@ enum InterfaceFlag
|
||||
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
|
||||
};
|
||||
|
||||
using InterfaceFlags = Flags<InterfaceFlag>;
|
||||
using InterfaceFlags = utils::Flags<InterfaceFlag>;
|
||||
|
||||
/** invert interface such that
|
||||
* - new end can connect to old start
|
||||
|
||||
@ -32,15 +32,27 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Authors: Robert Haschke
|
||||
Desc: Project-agnostic utility classes
|
||||
/* Authors: Robert Haschke, Michael 'v4hn' Goerner
|
||||
Desc: Miscellaneous utilities
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <type_traits>
|
||||
#include <initializer_list>
|
||||
|
||||
namespace moveit {
|
||||
|
||||
namespace core {
|
||||
class LinkModel;
|
||||
class RobotState;
|
||||
} // namespace core
|
||||
|
||||
namespace task_constructor {
|
||||
|
||||
namespace utils {
|
||||
|
||||
/** template class to compose flags from enums in a type-safe fashion */
|
||||
template <typename Enum>
|
||||
class Flags
|
||||
@ -118,3 +130,9 @@ private:
|
||||
};
|
||||
|
||||
#define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags<Enum>;
|
||||
|
||||
const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
|
||||
std::string frame);
|
||||
} // namespace utils
|
||||
} // namespace task_constructor
|
||||
} // namespace moveit
|
||||
|
||||
@ -28,6 +28,7 @@ add_library(${PROJECT_NAME}
|
||||
stage.cpp
|
||||
storage.cpp
|
||||
task.cpp
|
||||
utils.cpp
|
||||
|
||||
solvers/planner_interface.cpp
|
||||
solvers/cartesian_path.cpp
|
||||
|
||||
@ -294,11 +294,7 @@ void ComputeIK::compute() {
|
||||
}
|
||||
ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose;
|
||||
|
||||
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
|
||||
link = scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id);
|
||||
#else
|
||||
link = getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id);
|
||||
#endif
|
||||
link = utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id);
|
||||
|
||||
// transform target pose such that ik frame will reach there if link does
|
||||
target_pose = target_pose * ik_pose.inverse() * scene->getCurrentState().getFrameTransform(link->getName());
|
||||
|
||||
@ -43,6 +43,7 @@
|
||||
#include <moveit/task_constructor/stages/move_to.h>
|
||||
#include <moveit/task_constructor/cost_terms.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
#include <moveit/task_constructor/utils.h>
|
||||
|
||||
#include <rviz_marker_tools/marker_creation.h>
|
||||
|
||||
@ -262,13 +263,8 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
||||
add_frame(target, "target frame");
|
||||
add_frame(ik_pose_world, "ik frame");
|
||||
|
||||
const moveit::core::LinkModel* parent {
|
||||
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
|
||||
scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id)
|
||||
#else
|
||||
getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id)
|
||||
#endif
|
||||
};
|
||||
const moveit::core::LinkModel* parent{ utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(),
|
||||
ik_pose_msg.header.frame_id) };
|
||||
|
||||
// transform target pose such that ik frame will reach there if link does
|
||||
Eigen::Isometry3d ik_pose;
|
||||
|
||||
64
core/src/utils.cpp
Normal file
64
core/src/utils.cpp
Normal file
@ -0,0 +1,64 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Hamburg University
|
||||
* Copyright (c) 2017, Bielefeld University
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Bielefeld University nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Authors: Michael Goerner, Robert Haschke */
|
||||
|
||||
#include <moveit/robot_state/robot_state.h>
|
||||
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace utils {
|
||||
|
||||
const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
|
||||
std::string frame) {
|
||||
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
|
||||
return state.getRigidlyConnectedParentLinkModel(frame);
|
||||
#else
|
||||
const moveit::core::LinkModel* link{ nullptr };
|
||||
|
||||
if (state.hasAttachedBody(frame)) {
|
||||
link = state.getAttachedBody(frame)->getAttachedLink();
|
||||
} else if (state.getRobotModel()->hasLinkModel(frame))
|
||||
link = state.getLinkModel(frame);
|
||||
|
||||
return state.getRobotModel()->getRigidlyConnectedParentLinkModel(link);
|
||||
#endif
|
||||
}
|
||||
|
||||
} // namespace utils
|
||||
} // namespace task_constructor
|
||||
} // namespace moveit
|
||||
Loading…
Reference in New Issue
Block a user