Bimanual

moveit_core/collision_detection_fcl/src/collision_common.cpp

@@ -611,6 +611,11 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void
     // get overwritten.
     thread_local DistanceResultsData dist_result;
     dist_result.distance = fcl_result.min_distance;
+    // print the distance
+    // if (cdata->req->verbose)
+    // {
+    // RCLCPP_INFO(getLogger(), "Distance between %s and %s is %f", cd1->getID().c_str(), cd2->getID().c_str(), dist_result.distance);
+    // }
 
     // Careful here: Get the collision geometry data again, since FCL might
     // swap o1 and o2 in the result.

moveit_ros/moveit_servo/config/servo_parameters.yaml

@@ -340,3 +340,14 @@ servo:
       bounds<>: [0.0, 1.0]
     }
   }
+
+  position_control_gain: {
+    type: double,
+    default_value: 1.0,
+    description: "Gain multiplier applied to joint position deltas before publishing. \
+                  Useful when commanding a forward_position_controller where larger steps \
+                  are needed to achieve the desired Cartesian velocity.",
+    validation: {
+      gt<>: 0.0
+    }
+  }

moveit_ros/moveit_servo/src/collision_monitor.cpp

@@ -62,6 +62,8 @@ CollisionMonitor::CollisionMonitor(const planning_scene_monitor::PlanningSceneMo
   scene_collision_request_.group_name = servo_params.move_group_name;
 
   self_collision_request_.distance = true;
+  self_collision_request_.verbose = true;
+  self_collision_request_.detailed_distance = true;
   self_collision_request_.group_name = servo_params.move_group_name;
 }
 

moveit_ros/moveit_servo/src/servo.cpp

@@ -463,7 +463,10 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo
     servo_status_ = StatusCode::INVALID;
     RCLCPP_WARN_STREAM(logger_, "Incoming servo command type does not match known command types.");
   }
+  // if we are in forward_position_controller, multiply the joint_position_deltas by 4
 
+  joint_position_deltas *= servo_params_.position_control_gain; 
+
   return joint_position_deltas;
 }
 

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -367,7 +367,7 @@ void ServoNode::servoLoop()
         // if all joint_cmd_rolling_window_ is empty or all commands in it are outdated, use current robot state
         joint_cmd_rolling_window_.clear();
         current_state = servo_->getCurrentRobotState(false /* block for current robot state */);
-        current_state.velocities *= 0.0;
+        // current_state.velocities *= 0.0;
       }
 
       // update robot state values
@@ -376,7 +376,7 @@ void ServoNode::servoLoop()
 
       next_joint_state = std::nullopt;
       const CommandType expected_type = servo_->getCommandType();
-
+      
       if (expected_type == CommandType::JOINT_JOG && new_joint_jog_msg_)
       {
         next_joint_state = processJointJogCommand(robot_state);