pyrobot_state.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/numpy.h>
39 #include <pybind11/stl.h>
41 
44 
45 namespace py = pybind11;
46 using namespace robot_state;
47 
48 void def_robot_state_bindings(py::module& m)
49 {
50  m.doc() = "Representation of a robot's state. This includes position, velocity, acceleration and effort.";
51  py::class_<RobotState, RobotStatePtr>(m, "RobotState")
52  .def(py::init<const robot_model::RobotModelConstPtr&>(), py::arg("robot_model"))
53  .def("setToRandomPositions", py::overload_cast<>(&RobotState::setToRandomPositions))
54  .def("setToRandomPositions", py::overload_cast<const JointModelGroup*>(&RobotState::setToRandomPositions))
55  .def("getJointModelGroup", &RobotState::getJointModelGroup, py::return_value_policy::reference)
56  .def("getJointModel", &RobotState::getJointModel, py::return_value_policy::reference)
57  .def("getLinkModel", &RobotState::getLinkModel, py::return_value_policy::reference)
58  .def("getVariableNames", &RobotState::getVariableNames)
59  .def("getVariablePositions", py::overload_cast<>(&RobotState::getVariablePositions, py::const_),
60  py::return_value_policy::reference)
61  .def("getVariableCount", &RobotState::getVariableCount)
62  .def("hasVelocities", &RobotState::hasVelocities)
63  .def("setJointGroupPositions",
64  py::overload_cast<const std::string&, const std::vector<double>&>(&RobotState::setJointGroupPositions))
65  .def("setJointGroupPositions",
66  py::overload_cast<const JointModelGroup*, const std::vector<double>&>(&RobotState::setJointGroupPositions))
67  .def("satisfiesBounds",
68  py::overload_cast<const JointModelGroup*, double>(&RobotState::satisfiesBounds, py::const_),
69  py::arg("joint_model_group"), py::arg("margin") = 0.0)
70  .def("update", &RobotState::update, py::arg("force") = false)
71  .def("printStateInfo",
72  [](const RobotState& s) {
73  std::stringstream ss;
74  s.printStateInfo(ss);
75  return ss.str();
76  })
77  .def("printStatePositions",
78  [](const RobotState& s) {
79  std::stringstream ss;
80  s.printStatePositions(ss);
81  return ss.str();
82  })
83  .def("__repr__",
84  [](const RobotState& s) {
85  std::stringstream ss;
86  s.printStatePositions(ss);
87  return ss.str();
88  })
89  //
90  ;
91 
92  m.def("jointStateToRobotState", &jointStateToRobotState);
93  m.def(
94  "robotStateToRobotStateMsg",
95  [](const RobotState& state, bool copy_attached_bodies) {
96  moveit_msgs::RobotState state_msg;
97  robotStateToRobotStateMsg(state, state_msg, copy_attached_bodies);
98  return state_msg;
99  },
100  py::arg("state"), py::arg("copy_attached_bodies") = true);
101 }
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
moveit::core::RobotState::getJointModel
const JointModel * getJointModel(const std::string &joint) const
Get the model of a particular joint.
Definition: robot_state.h:194
moveit::core::RobotState::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:176
pybind_rosmsg_typecasters.h
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
s
XmlRpcServer s
moveit::core::RobotState::getLinkModel
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:188
moveit::core::RobotState::hasVelocities
bool hasVelocities() const
By default, if velocities are never set or initialized, the state remembers that there are no velocit...
Definition: robot_state.h:295
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
moveit::core::RobotState::setToRandomPositions
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
Definition: robot_state.cpp:348
conversions.h
moveit::core::RobotState::getVariableNames
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up this state, in the order they are stored in memory.
Definition: robot_state.h:182
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:720
moveit::core::RobotState::getVariablePositions
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully....
Definition: robot_state.h:213
def_robot_state_bindings
void def_robot_state_bindings(py::module &m)
Definition: pyrobot_state.cpp:48
moveit::core::RobotState::satisfiesBounds
bool satisfiesBounds(double margin=0.0) const
Definition: robot_state.cpp:891
pybind11
Definition: pybind_rosmsg_typecasters.h:55
moveit::core::RobotState::setJointGroupPositions
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition: robot_state.h:671
moveit::core::RobotState::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
Definition: robot_state.h:200
moveit::core::jointStateToRobotState
bool jointStateToRobotState(const sensor_msgs::JointState &joint_state, RobotState &state)
Convert a joint state to a MoveIt robot state.
Definition: conversions.cpp:451
moveit::core::robotStateToRobotStateMsg
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
Definition: conversions.cpp:473
robot_state.h


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Nov 24 2022 03:32:15