moveit2
The MoveIt Motion Planning Framework for ROS 2.
pytransforms.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2021, Peter Mitrano
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * The name of Peter Mitrano may not be used to endorse or promote
18  * products derived from this software without specific prior
19  * written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Peter Mitrano */
36 
37 #include <pybind11/pybind11.h>
38 #include <pybind11/stl.h>
40 
42 
43 namespace py = pybind11;
44 using namespace moveit::core;
45 
46 void def_transforms_bindings(py::module& m)
47 {
48  m.doc() = "Provides an implementation of a snapshot of a transform tree that can be easily queried for transforming "
49  "different quantities. Transforms are maintained as a list of transforms to a particular frame. All stored "
50  "transforms are considered fixed.";
51  py::class_<Transforms, TransformsPtr>(m, "Transforms")
52  .def(py::init<const std::string&>())
53  .def("canTransform", &Transforms::canTransform)
54  .def("getTargetFrame", &Transforms::getTargetFrame)
55  .def("getTransform", &Transforms::getTransform)
56  .def("isFixedFrame", &Transforms::isFixedFrame)
57  .def("getAllTransforms", &Transforms::getAllTransforms)
58  .def("setTransform", py::overload_cast<const Eigen::Isometry3d&, const std::string&>(&Transforms::setTransform))
59  .def("setTransform", py::overload_cast<const geometry_msgs::TransformStamped&>(&Transforms::setTransform))
60  .def("setTransforms", &Transforms::setTransforms)
61  .def("setAllTransforms", &Transforms::setAllTransforms)
62  .def("transformVector3", &Transforms::transformVector3)
63  .def("transformQuaternion", &Transforms::transformQuaternion)
64  .def("transformRotationMatrix", &Transforms::transformRotationMatrix)
65  .def("transformPose", &Transforms::transformPose)
66  //
67  ;
68 }
void setTransforms(const std::vector< geometry_msgs::msg::TransformStamped > &transforms)
Set a transform in the transform tree (adding it if necessary)
Definition: transforms.cpp:158
const std::string & getTargetFrame() const
Get the planning frame corresponding to this set of transforms.
Definition: transforms.cpp:73
void transformRotationMatrix(const std::string &from_frame, const Eigen::Matrix3d &m_in, Eigen::Matrix3d &m_out) const
Transform a rotation matrix in from_frame to the target_frame.
Definition: transforms.h:172
void transformPose(const std::string &from_frame, const Eigen::Isometry3d &t_in, Eigen::Isometry3d &t_out) const
Transform a pose in from_frame to the target_frame.
Definition: transforms.h:184
void setTransform(const Eigen::Isometry3d &t, const std::string &from_frame)
Set a transform in the transform tree (adding it if necessary)
Definition: transforms.cpp:126
const FixedTransformsMap & getAllTransforms() const
Return all the transforms.
Definition: transforms.cpp:78
void transformQuaternion(const std::string &from_frame, const Eigen::Quaterniond &q_in, Eigen::Quaterniond &q_out) const
Transform a quaternion in from_frame to the target_frame.
Definition: transforms.h:159
virtual const Eigen::Isometry3d & getTransform(const std::string &from_frame) const
Get transform for from_frame (w.r.t target frame)
Definition: transforms.cpp:100
void setAllTransforms(const FixedTransformsMap &transforms)
Set all the transforms: a map from string names of frames to corresponding Eigen::Isometry3d (w....
Definition: transforms.cpp:83
virtual bool isFixedFrame(const std::string &frame) const
Check whether a frame stays constant as the state of the robot model changes. This is true for any tr...
Definition: transforms.cpp:92
void transformVector3(const std::string &from_frame, const Eigen::Vector3d &v_in, Eigen::Vector3d &v_out) const
Transform a vector in from_frame to the target_frame Note: assumes that v_in and v_out are "free" vec...
Definition: transforms.h:147
virtual bool canTransform(const std::string &from_frame) const
Check whether data can be transformed from a particular frame.
Definition: transforms.cpp:118
Core components of MoveIt.
void def_transforms_bindings(py::module &m)