moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
#include <moveit_servo_lib_parameters.hpp>
#include <moveit_servo/utils/datatypes.hpp>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/robot_model/joint_model_group.h>
#include <moveit/robot_state/robot_state.h>
#include <sensor_msgs/msg/joint_state.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <trajectory_msgs/msg/joint_trajectory.hpp>
Go to the source code of this file.
Namespaces | |
moveit_servo | |
Functions | |
std::optional< std::string > | moveit_servo::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. More... | |
std::optional< std::string > | moveit_servo::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. More... | |
bool | moveit_servo::isValidCommand (const Eigen::VectorXd &command) |
Checks if a given VectorXd is a valid command. More... | |
bool | moveit_servo::isValidCommand (const Eigen::Isometry3d &command) |
Checks if a given Isometry3d (pose) is a valid command. More... | |
bool | moveit_servo::isValidCommand (const TwistCommand &command) |
Checks if a given Twist command is valid. More... | |
bool | moveit_servo::isValidCommand (const PoseCommand &command) |
Checks if a given Pose command is valid. More... | |
geometry_msgs::msg::Pose | moveit_servo::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. More... | |
trajectory_msgs::msg::JointTrajectory | moveit_servo::composeTrajectoryMessage (const servo::Params &servo_params, const KinematicState &joint_state) |
Create a trajectory message from given joint state. More... | |
std_msgs::msg::Float64MultiArray | moveit_servo::composeMultiArrayMessage (const servo::Params &servo_params, const KinematicState &joint_state) |
Create a Float64MultiArray message from given joint state. More... | |
std::pair< double, StatusCode > | moveit_servo::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. More... | |
double | moveit_servo::jointLimitVelocityScalingFactor (const Eigen::VectorXd &velocities, const moveit::core::JointBoundsVector &joint_bounds, double scaling_override) |
Apply velocity scaling based on joint limits. More... | |
std::vector< int > | moveit_servo::jointsToHalt (const Eigen::VectorXd &positions, const Eigen::VectorXd &velocities, const moveit::core::JointBoundsVector &joint_bounds, double margin) |
Finds the joints that are exceeding allowable position limits. More... | |
geometry_msgs::msg::TransformStamped | moveit_servo::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. More... | |
PoseCommand | moveit_servo::poseFromPoseStamped (const geometry_msgs::msg::PoseStamped &msg) |
Convert a PoseStamped message to a Servo Pose. More... | |
planning_scene_monitor::PlanningSceneMonitorPtr | moveit_servo::createPlanningSceneMonitor (const rclcpp::Node::SharedPtr &node, const servo::Params &servo_params) |
Creates the planning scene monitor used by servo. More... | |