moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
common.hpp
Go to the documentation of this file.
1/*******************************************************************************
2 * BSD 3-Clause License
3 *
4 * Copyright (c) 2019, Los Alamos National Security, LLC
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 are met:
9 *
10 * * Redistributions of source code must retain the above copyright notice, this
11 * list of conditions and the following disclaimer.
12 *
13 * * Redistributions in binary form must reproduce the above copyright notice,
14 * this list of conditions and the following disclaimer in the documentation
15 * and/or other materials provided with the distribution.
16 *
17 * * Neither the name of the copyright holder nor the names of its
18 * contributors may be used to endorse or promote products derived from
19 * this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24 * ARE
25 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
26 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
30 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32 *******************************************************************************/
33
34/* Title : utils.hpp
35 * Project : moveit_servo
36 * Created : 05/17/2023
37 * Author : Andy Zelenak, V Mohammed Ibrahim
38 *
39 * Description : The utility functions used by MoveIt Servo.
40 */
41
42#pragma once
43
44#include <moveit_servo_lib_parameters.hpp>
49#include <sensor_msgs/msg/joint_state.hpp>
50#include <std_msgs/msg/float64_multi_array.hpp>
51#include <tf2_eigen/tf2_eigen.hpp>
52#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
53#include <trajectory_msgs/msg/joint_trajectory.hpp>
54
55namespace moveit_servo
56{
57// A minimum of 3 points are used to help with interpolation when creating trajectory messages.
58constexpr int MIN_POINTS_FOR_TRAJ_MSG = 3;
59
66std::optional<std::string> getIKSolverBaseFrame(const moveit::core::RobotStatePtr& robot_state,
67 const std::string& group_name);
68
75std::optional<std::string> getIKSolverTipFrame(const moveit::core::RobotStatePtr& robot_state,
76 const std::string& group_name);
77
83bool isValidCommand(const Eigen::VectorXd& command);
84
90bool isValidCommand(const Eigen::Isometry3d& command);
91
97bool isValidCommand(const TwistCommand& command);
98
104bool isValidCommand(const PoseCommand& command);
105
112geometry_msgs::msg::Pose poseFromCartesianDelta(const Eigen::VectorXd& delta_x,
113 const Eigen::Isometry3d& base_to_tip_frame_transform);
114
122std::optional<trajectory_msgs::msg::JointTrajectory>
123composeTrajectoryMessage(const servo::Params& servo_params, const std::deque<KinematicState>& joint_cmd_rolling_window);
124
134void updateSlidingWindow(KinematicState& next_joint_state, std::deque<KinematicState>& joint_cmd_rolling_window,
135 double max_expected_latency, const rclcpp::Time& cur_time);
136
143std_msgs::msg::Float64MultiArray composeMultiArrayMessage(const servo::Params& servo_params,
144 const KinematicState& joint_state);
145
153std::pair<double, StatusCode> velocityScalingFactorForSingularity(const moveit::core::RobotStatePtr& robot_state,
154 const Eigen::VectorXd& target_delta_x,
155 const servo::Params& servo_params);
156
165double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities,
166 const moveit::core::JointBoundsVector& joint_bounds, double scaling_override);
167
176std::vector<size_t> jointVariablesToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
177 const moveit::core::JointBoundsVector& joint_bounds,
178 const std::vector<double>& margins);
179
187geometry_msgs::msg::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf,
188 const std::string& parent_frame,
189 const std::string& child_frame);
190
196PoseCommand poseFromPoseStamped(const geometry_msgs::msg::PoseStamped& msg);
197
201planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const rclcpp::Node::SharedPtr& node,
202 const servo::Params& servo_params);
203
210KinematicState extractRobotState(const moveit::core::RobotStatePtr& robot_state, const std::string& move_group_name);
211
212} // namespace moveit_servo
std::vector< const JointModel::Bounds * > JointBoundsVector
std::optional< std::string > getIKSolverTipFrame(const moveit::core::RobotStatePtr &robot_state, const std::string &group_name)
Get the tip (end-effector) frame of the current active joint group or subgroup's IK solver.
Definition common.cpp:72
std::optional< trajectory_msgs::msg::JointTrajectory > composeTrajectoryMessage(const servo::Params &servo_params, const std::deque< KinematicState > &joint_cmd_rolling_window)
Create a trajectory message from a rolling window queue of joint state commands. Method optionally re...
Definition common.cpp:151
geometry_msgs::msg::Pose poseFromCartesianDelta(const Eigen::VectorXd &delta_x, const Eigen::Isometry3d &base_to_tip_frame_transform)
Create a pose message for the provided change in Cartesian position.
Definition common.cpp:115
std::pair< double, StatusCode > velocityScalingFactorForSingularity(const moveit::core::RobotStatePtr &robot_state, const Eigen::VectorXd &target_delta_x, const servo::Params &servo_params)
Computes scaling factor for velocity when the robot is near a singularity.
Definition common.cpp:282
std::optional< std::string > getIKSolverBaseFrame(const moveit::core::RobotStatePtr &robot_state, const std::string &group_name)
Get the base frame of the current active joint group or subgroup's IK solver.
Definition common.cpp:57
double jointLimitVelocityScalingFactor(const Eigen::VectorXd &velocities, const moveit::core::JointBoundsVector &joint_bounds, double scaling_override)
Apply velocity scaling based on joint limits. If the robot model does not have velocity limits define...
Definition common.cpp:380
std_msgs::msg::Float64MultiArray composeMultiArrayMessage(const servo::Params &servo_params, const KinematicState &joint_state)
Create a Float64MultiArray message from given joint state.
Definition common.cpp:258
bool isValidCommand(const Eigen::VectorXd &command)
Checks if a given VectorXd is a valid command.
Definition common.cpp:87
geometry_msgs::msg::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d &eigen_tf, const std::string &parent_frame, const std::string &child_frame)
Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped.
Definition common.cpp:454
void updateSlidingWindow(KinematicState &next_joint_state, std::deque< KinematicState > &joint_cmd_rolling_window, double max_expected_latency, const rclcpp::Time &cur_time)
Adds a new joint state command to a queue containing commands over a time window. Also modifies the v...
Definition common.cpp:203
constexpr int MIN_POINTS_FOR_TRAJ_MSG
Definition common.hpp:58
std::vector< size_t > jointVariablesToHalt(const Eigen::VectorXd &positions, const Eigen::VectorXd &velocities, const moveit::core::JointBoundsVector &joint_bounds, const std::vector< double > &margins)
Finds the joint variable indices corresponding to joints exceeding allowable position limits.
Definition common.cpp:411
KinematicState extractRobotState(const moveit::core::RobotStatePtr &robot_state, const std::string &move_group_name)
Extract the state from a RobotStatePtr instance.
Definition common.cpp:516
PoseCommand poseFromPoseStamped(const geometry_msgs::msg::PoseStamped &msg)
Convert a PoseStamped message to a Servo Pose.
Definition common.cpp:464
planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const rclcpp::Node::SharedPtr &node, const servo::Params &servo_params)
Creates the planning scene monitor used by servo.
Definition common.cpp:480