cv_ros_user's profile - activity

I am updating the AllowedCollisionMatrix in order to disable collision between an object attached to the robot end-effector and a collision object in the world for a specific robot movement. It works but it also removes the attached object from the scene. So it seems that it is not possible to disable temporary collision for an attached object and enable it afterward.

What would have been useful:

• to be able to update the ACM to disable collision between an attached object and another object in the scene
• use MoveIt to move the end-effector
• update the ACM to enable collision
• the attached object is still present in the scene with its position updated since the robot has moved

Unfortunately, it looks like it just removes the attached object from the scene.

A minimal code looks like this (tested on ROS Kinetic). Run with roslaunch panda_moveit_config demo.launch and rosrun <> <>.

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "move_group_interface_tutorial");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  static const std::string PLANNING_GROUP = "panda_arm";
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  robot_model_loader::RobotModelLoaderPtr robot_model_loader(new robot_model_loader::RobotModelLoader("robot_description"));
  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(new planning_scene_monitor::PlanningSceneMonitor(robot_model_loader));

  planning_scene_monitor->startSceneMonitor();
  planning_scene_monitor->startWorldGeometryMonitor();
  planning_scene_monitor->startStateMonitor();

  //Add collision object
  {
    moveit_msgs::CollisionObject collision_object;
    collision_object.header.frame_id = move_group.getPlanningFrame();
    collision_object.id = "box1";

    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[0] = 0.3;
    primitive.dimensions[1] = 1;
    primitive.dimensions[2] = 0.4;

    geometry_msgs::Pose box_pose;
    box_pose.orientation.w = 1.0;
    box_pose.position.x = 0.3;
    box_pose.position.y = 0;
    box_pose.position.z = 0;

    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;

    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(collision_object);
    planning_scene_interface.applyCollisionObjects(collision_objects);
  }

  //Add collision object and attach it
  {
    moveit_msgs::CollisionObject collision_object;
    collision_object.header.frame_id = move_group.getPlanningFrame();
    collision_object.id = "box_attach";

    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[0] = 0.1;
    primitive.dimensions[1] = 0.1;
    primitive.dimensions[2] = 0.1;

    geometry_msgs::Pose box_pose = move_group.getCurrentPose().pose;
    box_pose.position.z -= 0.175;

    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;

    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(collision_object);

    planning_scene_interface.applyCollisionObjects(collision_objects);

    moveit_msgs::AttachedCollisionObject aco;
    aco.object.id = collision_object.id;
    aco.link_name = move_group.getEndEffectorLink();
    aco.touch_links.push_back(move_group.getEndEffectorLink());
    aco.touch_links.push_back("panda_hand");
    aco.touch_links.push_back("panda_leftfinger");
    aco.touch_links.push_back("panda_rightfinger");
    aco.object.operation = moveit_msgs::CollisionObject::ADD;
    planning_scene_interface.applyAttachedCollisionObject(aco);

    std::map<std::string, moveit_msgs::AttachedCollisionObject> attached_co = planning_scene_interface.getAttachedObjects();
    std::cout << "\nAttachedCollisionObject after applyAttachedCollisionObject:" << std::endl;
    for (const auto& kv : attached_co) {
      std::cout << kv.first << " ; " << kv.second.object.id << std::endl;
    }
  }

  //Wait a little
  const int wait_time = 2;
  ros::Duration(wait_time).sleep();

  //Ignore collision between attached object and collision object in the world
  {
    planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor);
    collision_detection::AllowedCollisionMatrix& acm = ls->getAllowedCollisionMatrixNonConst();
    acm.setEntry("box1", "box_attach", true);
    std::cout << "\nAllowedCollisionMatrix:\n";
    acm.print(std ...

I want to fuse global pose from tag pose estimation with visual odometry and IMU.

I am using:

• a RealSense D435i with realsense-ros that provides all the transformations between each sensor
• and rtab_map for the visual odometry

Fusing relative measurements (visual odometry and IMU) looks correct with rviz:

ekf_local.yaml:

frequency: 30
sensor_timeout: 0.1
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: camera_link
world_frame: odom

odom0: /odom
odom0_config: [true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true,  true,  true]
odom0_differential: false
odom0_relative: true

imu0: /imu/data
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]

imu0_differential: true
imu0_relative: false
imu0_remove_gravitational_acceleration: true

I am not able to fuse absolute pose with visual odometry and IMU. I want to use something like this:

ekf_global.yaml:

frequency: 30
sensor_timeout: 0.1
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: camera_link
world_frame: map

odom0: /odom
odom0_config: [true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true, true, true]

odom0_differential: false
odom0_relative: true

odom1: /tag_pose

odom1_config: [true,  true,  true,
               true,  true,  true,
               false, false, false,
               false, false, false,
               false, false, false]

odom1_differential: false
odom1_relative: false

imu0: /imu/data
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]

imu0_differential: true
imu0_relative: false
imu0_remove_gravitational_acceleration: true

odom0 is the output of rtab_map:

frame_id: "odom"
child_frame_id: "camera_link"

imu0 is:

frame_id: "camera_imu_optical_frame"

(Realsense ROS publishes all the transformations between the camera_link and for instance camera_imu_optical_frame.)

What should be the transformation for odom1?

Is it the transformation between the map (frame_id) and the camera_link (so the raw transformation between the tag and the camera)? Is it the transformation between the map (frame_id) and the odom? Since I have another ekf_node that fuses the continuous data (vo and IMU), I can transform the raw tag pose.

The output of the local ekf is clear. It is the transformation between the odom and the camera_link. The odom frame is the initial position of the sensor when launching the nodes.

What is the output of the global ekf (/ekf_global/odometry/filtered)? From the doc it should be the transformation between the map and odom. But from my understanding:

• map frame is fixed and corresponds in my case to the tag frame
• odom frame is the initial pose of the sensor and the transformation map / odom should be fixed
• camera_link is the free floating camera frame

So does the ekf (/ekf_global/odometry/filtered) estimates the map --> odom transformation and I have to use the /ekf_local/odometry/filtered to get the transformation between the tag/map and the camera_link?

Edit:

I did some tests, recapped here.

The homogeneous transformation a_T_b means: X_a = a_T_b . X_b.

The vision library detects the tag and estimates the transformation camlink_T_tag. The goal was to still have the transformation camlink_T_tag when the tag is not detected (occlusion, ...).

Option 1:

• map and tag frames are the same
• ekf_local/odometry/filtered estimates odom_T_camlink
• ekf_global/odometry/filtered estimates map_T_camlink
• when the tag is detected ...