MoveIt Pro Behavior
Core Behaviors for MoveIt Pro
Loading...
Searching...
No Matches
compute_velocity_to_align_with_target.hpp File Reference
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include "behaviortree_cpp/action_node.h"
#include "moveit_pro_types/types.hpp"
#include "moveit_studio_behavior_interface/shared_resources_node.hpp"
Include dependency graph for compute_velocity_to_align_with_target.hpp:
This graph shows which files directly or indirectly include this file:

Classes

class  moveit_studio::behaviors::ComputeVelocityToAlignWithTarget
 calculates a control velocity that is a sum of the target velocity and a catchup velocity. The target motion state can be offset from the incoming motion state message. The returned control velocity is designed to be sent to the velocity_force_controller. More...
 

Namespaces

namespace  moveit_studio
 
namespace  moveit_studio::behaviors
 
namespace  moveit_studio::behaviors::compute_velocity_to_align_with_target
 
namespace  moveit_studio::behaviors::compute_velocity_to_align_with_target::internal
 

Functions

nav_msgs::msg::Odometry moveit_studio::behaviors::compute_velocity_to_align_with_target::internal::offsetPoseAndTwist (const nav_msgs::msg::Odometry &target_motion_state, const geometry_msgs::msg::PoseStamped &target_pose_offset)
 Calculate the offset target motion_state based on the target motion_state and target pose offset.
 
tl::expected< geometry_msgs::msg::PoseStamped, std::string > moveit_studio::behaviors::compute_velocity_to_align_with_target::internal::calculatePoseError (const nav_msgs::msg::Odometry &target_motion_state, tf2_ros::Buffer &tf_buffer, const std::string &end_effector_frame)
 Calculate the pose error between the target motion_state and the current end effector frame.
 
geometry_msgs::msg::TwistStamped moveit_studio::behaviors::compute_velocity_to_align_with_target::internal::calculateControlVelocity (const geometry_msgs::msg::PoseStamped &pose_error, double proportional_gain_linear, double proportional_gain_angular)
 Calculate the proportional control velocity based on the pose error.
 
tl::expected< geometry_msgs::msg::TwistStamped, std::string > moveit_studio::behaviors::compute_velocity_to_align_with_target::internal::addTargetAndCatchupVelocity (const nav_msgs::msg::Odometry &target_motion_state, const tf2_ros::Buffer &tf_buffer, const std::string &end_effector_frame, const geometry_msgs::msg::TwistStamped &catchup_velocity)
 Add the target and catchup velocities in the end effector frame.