moveit
moveit copied to clipboard
ros-planning
moveit_servo for fiducial markers
Hi, I'm trying to use moveit_servo to track fiducial markers and would really appreciate some help. I've been at my wits end and any leads would give me the opportunity to explore.
Thanks
Please precisely describe your issue. What did you achieved? What does fail? Did you have a look at existing issues, e.g. https://github.com/moveit/moveit/issues/2617?
Hi, thank you so much for getting back to me. I appreciate your help. I'm going to share a detailed overview of my current setup and what's functioning properly so far. Please bear with me as I provide the full context.
Enviroment: ROS Distro: Noetic OS Version: Ubuntu 20.04 Robot: UR5 - real robot
Steps to reproduce:
roslaunch ur_robot_driver ur5_bringup.launch
(The robot works perfectly with moveit)
Controller list:
To check the status of the controllers, I ran the following command:
rosservice call /controller_manager/list_controllers
Here is the output I received:
controller:
-
name: "joint_state_controller"
state: "running"
type: "joint_state_controller/JointStateController"
claimed_resources:
-
hardware_interface: "hardware_interface::JointStateInterface"
resources: []
-
name: "pos_joint_traj_controller"
state: "initialized"
type: "position_controllers/JointTrajectoryController"
claimed_resources:
-
hardware_interface: "hardware_interface::PositionJointInterface"
resources:
- elbow_joint
- shoulder_lift_joint
- shoulder_pan_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
-
name: "scaled_pos_joint_traj_controller"
state: "stopped"
type: "position_controllers/ScaledJointTrajectoryController"
claimed_resources:
-
hardware_interface: "scaled_controllers::ScaledPositionJointInterface"
resources:
- elbow_joint
- shoulder_lift_joint
- shoulder_pan_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
-
name: "joint_group_vel_controller"
state: "running"
type: "velocity_controllers/JointGroupVelocityController"
claimed_resources:
-
hardware_interface: "hardware_interface::VelocityJointInterface"
resources:
- elbow_joint
- shoulder_lift_joint
- shoulder_pan_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
-
name: "speed_scaling_state_controller"
state: "running"
type: "scaled_controllers/SpeedScalingStateController"
claimed_resources:
-
hardware_interface: "scaled_controllers::SpeedScalingInterface"
resources: []
-
name: "joint_group_position_controller"
state: "initialized"
type: "position_controllers/JointGroupPositionController"
claimed_resources:
-
hardware_interface: "hardware_interface::PositionJointInterface"
resources:
- elbow_joint
- shoulder_lift_joint
- shoulder_pan_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
Configuration File:
Below is my configuration file that defines all parameters related to servoing:
###############################################
# Modify all parameters related to servoing here
###############################################
use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
## Properties of incoming commands
command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
scale:
# Scale parameters are only used if command_in_type=="unitless"
linear: 0.02 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
rotational: 0.01 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
# Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
joint: 0.005
low_pass_filter_coeff: 2.0 # Larger --> trust the filtered data more, trust the measurements less.
## Properties of outgoing commands
publish_period: 0.008 # 1/Nominal publish rate [seconds]
low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
# What type of topic does your robot driver expect?
# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController)
# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots)
command_out_type: std_msgs/Float64MultiArray
# What to publish? Can save some bandwidth as most robots only require positions or velocities
publish_joint_positions: false
publish_joint_velocities: true
publish_joint_accelerations: false
## MoveIt properties
move_group_name: manipulator # Often 'manipulator' or 'arm'
planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world'
## Other frames
ee_frame_name: wrist_3_link # The name of the end effector link, used to return the EE pose
robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector
## Stopping behaviour
incoming_command_timeout: 10 # Stop servoing if X seconds elapse without a new command
# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
# Important because ROS may drop some messages and we need the robot to halt reliably.
num_outgoing_halt_msgs_to_publish: 4
## Configure handling of singularities and joint limits
lower_singularity_threshold: 17 # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 30 # Stop when the condition number hits this
joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
## Topic names
cartesian_command_in_topic: delta_twist_cmds # Topic for incoming Cartesian twist commands
joint_command_in_topic: delta_joint_cmds # Topic for incoming joint angle commands
joint_topic: joint_states
status_topic: status # Publish status to this topic
command_out_topic: /joint_group_vel_controller/command # Publish outgoing commands here
## Collision checking for the entire robot body
check_collisions: true # Check collisions?
collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often.
# Two collision check algorithms are available:
# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
collision_check_type: threshold_distance
# Parameters for "threshold_distance"-type collision checking
self_collision_proximity_threshold: 0.001 # Start decelerating when a self-collision is this far [m]
scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m]
# Parameters for "stop_distance"-type collision checking
collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency
min_allowable_collision_distance: 0.000001 # Stop if a collision is closer than this [m]
When publishing twist commands for the robot, it works as expected and moves in the y direction according to the velocity specified:
rostopic pub -r 100 -s /servo_server/delta_twist_cmds geometry_msgs/TwistStamped "header: auto
> twist:
> linear:
> x: 0.0
> y: 0.1
> z: 0.0
> angular:
> x: 0.0
> y: 0.0
> z: 0.0"
Log messages:
* /rosdistro: noetic
* /rosversion: 1.16.0
* /servo_server/angular_derivative_gain: 0.0
* /servo_server/angular_integral_gain: 0.0
* /servo_server/angular_proportional_gain: 0.5
* /servo_server/cartesian_command_in_topic: delta_twist_cmds
* /servo_server/check_collisions: True
* /servo_server/collision_check_rate: 50
* /servo_server/collision_check_type: threshold_distance
* /servo_server/collision_distance_safety_factor: 1000
* /servo_server/command_in_type: speed_units
* /servo_server/command_out_topic: /joint_group_vel_...
* /servo_server/command_out_type: std_msgs/Float64M...
* /servo_server/ee_frame_name: wrist_3_link
* /servo_server/hard_stop_singularity_threshold: 30
* /servo_server/incoming_command_timeout: 10
* /servo_server/joint_command_in_topic: delta_joint_cmds
* /servo_server/joint_limit_margin: 0.1
* /servo_server/joint_topic: joint_states
* /servo_server/low_latency_mode: False
* /servo_server/low_pass_filter_coeff: 2.0
* /servo_server/lower_singularity_threshold: 17
* /servo_server/min_allowable_collision_distance: 1e-06
* /servo_server/move_group_name: manipulator
* /servo_server/num_outgoing_halt_msgs_to_publish: 4
* /servo_server/planning_frame: base_link
* /servo_server/publish_joint_accelerations: False
* /servo_server/publish_joint_positions: False
* /servo_server/publish_joint_velocities: True
* /servo_server/publish_period: 0.008
* /servo_server/robot_link_command_frame: base_link
* /servo_server/scale/joint: 0.005
* /servo_server/scale/linear: 0.02
* /servo_server/scale/rotational: 0.01
* /servo_server/scene_collision_proximity_threshold: 0.05
* /servo_server/self_collision_proximity_threshold: 0.001
* /servo_server/status_topic: status
* /servo_server/use_gazebo: False
* /servo_server/windup_limit: 0.05
* /servo_server/x_derivative_gain: 0.0
* /servo_server/x_integral_gain: 0.0
* /servo_server/x_proportional_gain: 1.5
* /servo_server/y_derivative_gain: 0.0
* /servo_server/y_integral_gain: 0.0
* /servo_server/y_proportional_gain: 1.5
* /servo_server/z_derivative_gain: 0.0
* /servo_server/z_integral_gain: 0.0
* /servo_server/z_proportional_gain: 1.5
The pose_tracker_example.cpp, according to my understanding of what's happening should move the eef 0.0004 in the +z direction during each cycle. This happens as well. Here's the log file :
```yaml
PARAMETERS
* /rosdistro: noetic
* /rosversion: 1.16.0
* /servo_server/angular_derivative_gain: 0.0
* /servo_server/angular_integral_gain: 0.0
* /servo_server/angular_proportional_gain: 0.5
* /servo_server/cartesian_command_in_topic: delta_twist_cmds
* /servo_server/check_collisions: True
* /servo_server/collision_check_rate: 50
* /servo_server/collision_check_type: threshold_distance
* /servo_server/collision_distance_safety_factor: 1000
* /servo_server/command_in_type: speed_units
* /servo_server/command_out_topic: /joint_group_vel_...
* /servo_server/command_out_type: std_msgs/Float64M...
* /servo_server/ee_frame_name: wrist_3_link
* /servo_server/hard_stop_singularity_threshold: 30
* /servo_server/incoming_command_timeout: 10
* /servo_server/joint_command_in_topic: delta_joint_cmds
* /servo_server/joint_limit_margin: 0.1
* /servo_server/joint_topic: joint_states
* /servo_server/low_latency_mode: False
* /servo_server/low_pass_filter_coeff: 2.0
* /servo_server/lower_singularity_threshold: 17
* /servo_server/min_allowable_collision_distance: 1e-06
* /servo_server/move_group_name: manipulator
* /servo_server/num_outgoing_halt_msgs_to_publish: 4
* /servo_server/planning_frame: base_link
* /servo_server/publish_joint_accelerations: False
* /servo_server/publish_joint_positions: False
* /servo_server/publish_joint_velocities: True
* /servo_server/publish_period: 0.008
* /servo_server/robot_link_command_frame: base_link
* /servo_server/scale/joint: 0.005
* /servo_server/scale/linear: 0.02
* /servo_server/scale/rotational: 0.01
* /servo_server/scene_collision_proximity_threshold: 0.05
* /servo_server/self_collision_proximity_threshold: 0.001
* /servo_server/status_topic: status
* /servo_server/use_gazebo: False
* /servo_server/windup_limit: 0.05
* /servo_server/x_derivative_gain: 0.0
* /servo_server/x_integral_gain: 0.0
* /servo_server/x_proportional_gain: 1.5
* /servo_server/y_derivative_gain: 0.0
* /servo_server/y_integral_gain: 0.0
* /servo_server/y_proportional_gain: 1.5
* /servo_server/z_derivative_gain: 0.0
* /servo_server/z_integral_gain: 0.0
* /servo_server/z_proportional_gain: 1.5
NODES
/
servo_server (moveit_servo/servo_server)
ROS_MASTER_URI=http://localhost:11311
process[servo_server-1]: started with pid [1289987]
[ INFO] [1723397262.516808323]: Loading robot model 'ur5_robot'...
[ WARN] [1723397262.582698169]: IK plugin for group 'manipulator' relies on deprecated API. Please implement initialize(RobotModel, ...).
[ INFO] [1723397262.584858605]: IK Using joint shoulder_link -6.28319 6.28319
[ INFO] [1723397262.584900231]: IK Using joint upper_arm_link -6.28319 6.28319
[ INFO] [1723397262.584918535]: IK Using joint forearm_link -3.14159 3.14159
[ INFO] [1723397262.584936079]: IK Using joint wrist_1_link -6.28319 6.28319
[ INFO] [1723397262.584954520]: IK Using joint wrist_2_link -6.28319 6.28319
[ INFO] [1723397262.584969879]: IK Using joint wrist_3_link -6.28319 6.28319
[ INFO] [1723397262.584987788]: Looking in common namespaces for param name: manipulator/position_only_ik
[ INFO] [1723397262.586091824]: Looking in common namespaces for param name: manipulator/solve_type
[ INFO] [1723397262.587947607]: Using solve type Distance
[ INFO] [1723397262.620820992]: Starting planning scene monitor
[ INFO] [1723397262.623133049]: Listening to '/planning_scene'
[ INFO] [1723397262.623163396]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1723397262.624735061]: Listening to '/collision_object'
[ INFO] [1723397262.626345028]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1723397262.630409568]: Listening to '/attached_collision_object' for attached collision objects
[ WARN] [1723397262.847547708]: An acceleration limit is not defined for this joint; minimum stop distance should not be used for collision checking
[ WARN] [1723397276.119675775]: Close to a singularity, decelerating
[ WARN] [1723397288.599578438]: Stale command. Try a larger 'incoming_command_timeout' parameter?
Finally this is my code for tracking an aruco marker based on the pose_tracker_example.cpp. However, the UR5 doesn't move towards the marker and moves upward instead. When testing with a traditional OMPL, the manipulator works as expected.
#include <std_msgs/Int8.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <moveit_servo/servo.h>
#include <moveit_servo/pose_tracking.h>
#include <moveit_servo/status_codes.h>
#include <moveit_servo/make_shared_from_pool.h>
#include <thread>
static const std::string LOGNAME = "pose_tracking_test";
// Class for monitoring status of moveit_servo
class StatusMonitor
{
public:
StatusMonitor(ros::NodeHandle& nh, const std::string& topic)
{
sub_ = nh.subscribe(topic, 1, &StatusMonitor::statusCB, this);
}
private:
void statusCB(const std_msgs::Int8ConstPtr& msg)
{
moveit_servo::StatusCode latest_status = static_cast<moveit_servo::StatusCode>(msg->data);
if (latest_status != status_)
{
status_ = latest_status;
const auto& status_str = moveit_servo::SERVO_STATUS_CODE_MAP.at(status_);
ROS_INFO_STREAM_NAMED(LOGNAME, "Servo status: " << status_str);
}
}
moveit_servo::StatusCode status_ = moveit_servo::StatusCode::INVALID;
ros::Subscriber sub_;
};
// Global variable to store the latest ArUco marker pose
geometry_msgs::PoseStamped latest_marker_pose;
bool marker_pose_received = false;
// Callback function to update the marker pose
void markerPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
latest_marker_pose = *msg;
marker_pose_received = true;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, LOGNAME);
ros::NodeHandle nh("~");
ros::AsyncSpinner spinner(8);
spinner.start();
// Load the planning scene monitor
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor;
planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
if (!planning_scene_monitor->getPlanningScene())
{
ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor.");
return EXIT_FAILURE;
}
planning_scene_monitor->startSceneMonitor();
planning_scene_monitor->startWorldGeometryMonitor(
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC,
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
false /* skip octomap monitor */);
planning_scene_monitor->startStateMonitor();
// Create the pose tracker
moveit_servo::PoseTracking tracker(nh, planning_scene_monitor);
// Make a publisher for sending pose commands
ros::Publisher target_pose_pub =
nh.advertise<geometry_msgs::PoseStamped>("target_pose", 1 /* queue */, true /* latch */);
// Subscribe to servo status (and log it when it changes)
StatusMonitor status_monitor(nh, "status");
// Subscribe to the ArUco marker pose topic
ros::Subscriber marker_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>(
"/aruco_single/pose", 10, markerPoseCallback);
Eigen::Vector3d lin_tol{ 0.01, 0.01, 0.01 };
double rot_tol = 0.1;
// Wait for the first marker pose to be received
ros::Rate rate(10); // 10 Hz
while (ros::ok() && !marker_pose_received)
{
ros::spinOnce();
rate.sleep();
}
// Set the ArUco marker pose as the target pose
tracker.resetTargetPose();
latest_marker_pose.header.stamp = ros::Time::now();
target_pose_pub.publish(latest_marker_pose);
// Run the pose tracking in a new thread
std::thread move_to_pose_thread(
[&tracker, &lin_tol, &rot_tol] { tracker.moveToPose(lin_tol, rot_tol, 0.1 /* target pose timeout */); });
// Wait for the robot to reach the target pose
move_to_pose_thread.join();
// Make sure the tracker is stopped and clean up
tracker.stopMotion();
return EXIT_SUCCESS;
}
I'm aware that this issue might be somewhat out of the usual scope for this forum, but any leads or guidance would be helpful as I'm trying to resolve this problem. If I'm missing any details or if there's anything more you'd need to know to provide assistance, I'd be happy to provide more information.
Thanks a lot again.
I'd say that either the sign on your gains is incorrect, for example /servo_server/x_proportional_gain should be -1.5, or one of the frames is incorrect.
So I double checked on the frames and @AndyZe was right. The manipulator moves in the direction of the marker now. I also played with the gains and the sign seems alright for where the marker is in my environment. However, it doesn't go all the way and the arm only moves by a little bit. Do you have any suggestions to improve what's happening ? I also get the really close to singularity error when the arm is not close to a singularity.
[ INFO] [1723409195.202405413]: Servo status: Close to a singularity, decelerating
[ INFO] [1723409195.210512900]: Servo status: Very close to a singularity, emergency stop
[ INFO] [1723409195.362420551]: Servo status: Close to a singularity, decelerating
[ INFO] [1723409195.370440395]: Servo status: Very close to a singularity, emergency stop
[ INFO] [1723409195.594421121]: Servo status: Close to a singularity, decelerating
[ INFO] [1723409195.602422599]: Servo status: Very close to a singularity, emergency stop
[ INFO] [1723409196.106529147]: Servo status: Close to a singularity, decelerating
[ INFO] [1723409196.114424860]: Servo status: Very close to a singularity, emergency stop
[ INFO] [1723409196.138419679]: Servo status: Close to a singularity, decelerating
[ INFO] [1723409196.146358151]: Servo status: Very close to a singularity, emergency stop
[ INFO] [1723409196.242428384]: Servo status: Close to a singularity, decelerating
Would really appreciate any suggestions.
Thanks a lot
Try increasing these 2 parameters:
lower_singularity_threshold: 17 # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 30