moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
robot_poses.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2021, PickNik Robotics
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 * * Neither the name of PickNik Robotics nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior 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: David V. Lu!! */
36
39
40namespace moveit_setup
41{
42namespace srdf_setup
43{
45{
47
48 // Create scene publisher for later use
49 pub_robot_state_ = parent_node_->create_publisher<moveit_msgs::msg::DisplayRobotState>("moveit_robot_state", 1);
50
51 // Set the planning scene
52 // srdf_config_->getPlanningScene()->setName("MoveIt Planning Scene");
53
54 // Collision Detection initialization -------------------------------
55
56 // Setup the request
57 request_.contacts = true;
60 request_.verbose = false;
61}
62
63// ******************************************************************************************
64// Find the associated data by name
65// ******************************************************************************************
66srdf::Model::GroupState* RobotPoses::findPoseByName(const std::string& name, const std::string& group)
67{
68 // Find the group state we are editing based on the pose name
69 srdf::Model::GroupState* searched_state = nullptr; // used for holding our search results
70
71 for (srdf::Model::GroupState& state : srdf_config_->getGroupStates())
72 {
73 if (state.name_ == name && state.group_ == group) // match
74 {
75 searched_state = &state;
76 break;
77 }
78 }
79
80 return searched_state;
81}
82
83// ******************************************************************************************
84// Load the allowed collision matrix from the SRDF's list of link pairs
85// ******************************************************************************************
87{
88 // Clear the allowed collision matrix
90
91 // Update the allowed collision matrix, in case there has been a change
92 for (const auto& disabled_collision : srdf_config_->getDisabledCollisions())
93 {
94 allowed_collision_matrix_.setEntry(disabled_collision.link1_, disabled_collision.link2_, true);
95 }
96}
97
98// ******************************************************************************************
99// Publish the current RobotState to Rviz
100// ******************************************************************************************
102{
103 // Create a planning scene message
104 moveit_msgs::msg::DisplayRobotState msg;
105 moveit::core::robotStateToRobotStateMsg(robot_state, msg.state);
106
107 // Publish!
108 pub_robot_state_->publish(msg);
109}
110
112{
113 // Decide if current state is in collision
115 srdf_config_->getPlanningScene()->checkSelfCollision(request_, result, robot_state, allowed_collision_matrix_);
116 return !result.contacts.empty();
117}
118
119std::vector<const moveit::core::JointModel*> RobotPoses::getSimpleJointModels(const std::string& group_name) const
120{
121 moveit::core::RobotModelPtr robot_model = srdf_config_->getRobotModel();
122 if (!robot_model->hasJointModelGroup(group_name))
123 {
124 throw std::runtime_error(std::string("Unable to find joint model group for group: ") + group_name +
125 ". Are you sure this group has associated joints/links?");
126 }
127
128 const moveit::core::JointModelGroup* joint_model_group = robot_model->getJointModelGroup(group_name);
129
130 std::vector<const moveit::core::JointModel*> joint_models;
131 for (const moveit::core::JointModel* joint_model : joint_model_group->getJointModels())
132 {
133 if (joint_model->getVariableCount() != 1 || // only consider 1-variable joints
134 joint_model->isPassive() || // ignore passive
135 joint_model->getMimic()) // and mimic joints
136 continue;
137
138 joint_models.push_back(joint_model);
139 }
140 return joint_models;
141}
142
143void RobotPoses::setToCurrentValues(srdf::Model::GroupState& group_state)
144{
145 // Clear the old values (if any)
146 group_state.joint_values_.clear();
147
148 const auto& robot_state = srdf_config_->getPlanningScene()->getCurrentState();
149 for (const moveit::core::JointModel* joint_model : getSimpleJointModels(group_state.group_))
150 {
151 // Create vector for new joint values
152 std::vector<double> joint_values(joint_model->getVariableCount());
153 const double* const first_variable = robot_state.getVariablePositions() + joint_model->getFirstVariableIndex();
154 std::copy(first_variable, first_variable + joint_values.size(), joint_values.begin());
155
156 // Add joint vector to SRDF
157 group_state.joint_values_[joint_model->getName()] = std::move(joint_values);
158 }
159 srdf_config_->updateRobotModel(POSES);
160}
161
162} // namespace srdf_setup
163} // namespace moveit_setup
void clear()
Clear the allowed collision matrix.
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
Set an entry corresponding to a pair of elements.
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition robot_state.h:90
rclcpp::Node::SharedPtr parent_node_
void setToCurrentValues(srdf::Model::GroupState &group_state)
void loadAllowedCollisionMatrix()
Load the allowed collision matrix from the SRDF's list of link pairs.
std::vector< const moveit::core::JointModel * > getSimpleJointModels(const std::string &group_name) const
Returns a vector of joint models for the given group name.
void onInit() override
Overridable initialization method.
void publishState(const moveit::core::RobotState &robot_state)
Publish the given state on the moveit_robot_state topic.
collision_detection::AllowedCollisionMatrix allowed_collision_matrix_
Allowed collision matrix for robot poses.
srdf::Model::GroupState * findPoseByName(const std::string &name, const std::string &group)
collision_detection::CollisionRequest request_
rclcpp::Publisher< moveit_msgs::msg::DisplayRobotState >::SharedPtr pub_robot_state_
Remember the publisher for quick publishing later.
bool checkSelfCollision(const moveit::core::RobotState &robot_state)
Check if the given robot state is in collision.
void onInit() override
Overridable initialization method.
Definition srdf_step.hpp:51
std::shared_ptr< SRDFConfig > srdf_config_
Definition srdf_step.hpp:67
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
bool verbose
Flag indicating whether information about detected collisions should be reported.
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
std::size_t max_contacts
Overall maximum number of contacts to compute.
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
Representation of a collision checking result.