CODEWITHSUNDEEP
drakeros2
louis
louis

Reputation: 103

Drake method not working within ROS2 control update loop

While building a ROS2 control controller class (that implements ControllerInterface), I have notices that calling the Drake method CalcJacobianTranslationalVelocity within the update function triggers the error below when a command is published to the topic.

Command that triggers error

ros2 topic pub /my_ik_controller/commands std_msgs/msg/Float64MultiArray "data:

  • 0.5
  • 0.5"

ERROR

[ERROR] [ros2_control_node-2]: process has died [pid 10489, exit code -11, cmd '/opt/ros/jazzy/lib/controller_manager/ros2_control_node --ros-args --params-file /home/name/Documents/ros2_projects/drawing_robot_ws/install/base_package/share/base_package/config/ik_controller.yaml --params-file /tmp/launch_params_wjnehmxb'].

However, the exact same isolated code works fine if...

Here is the relevant code.

  // -----------BELOW NORMALLY WITHIN ROS2 CONTROLLER HEADER------------
  private:
    MultibodyPlant<double> _plant;
    std::unique_ptr<Context<double>> _plantContextPointer;


  // -----------BELOW NORMALLY WITHIN ROS2 CONTROLLER CONSTRUCTOR------------
  DifferentialInverseKinematicsCalculator::DifferentialInverseKinematicsCalculator()
    :_plant (0.0) // time step 0.0 for continuous system
  {
  }


  // -----------BELOW NORMALLY WITHIN ROS2 CONTROLLER ON_INIT()------------
  const std::string& urdf = get_robot_description();
  Parser parser(&_plant);
  parser.AddModelsFromString(urdf, ".urdf");
  const auto& basebody = _plant.GetBodyByName("base_link");
  _plant.WeldFrames(_plant.world_frame(), basebody.body_frame(), RigidTransformd());
  _plant.Finalize();
  auto _plantContextPointer = _plant.CreateDefaultContext();


  // -----------BELOW NORMALLY WITHIN ROS2 CONTROLLER UPDATE()------------
  Eigen::Matrix3Xd jacobian(3, _plant.num_positions()); // if spatial velocity, includes rotation, so 6, not 3.
  Eigen::Vector3d p_BoBp_B;
  p_BoBp_B << 0.0,0.0,0.0;
  const Frame<double>& _endFrame = _plant.GetBodyByName("end_effector").body_frame();
  const Frame<double>& _worldFrame = _plant.world_frame();
  _plant.CalcJacobianTranslationalVelocity(
      *_plantContextPointer, drake::multibody::JacobianWrtVariable::kV, _endFrame, p_BoBp_B, _worldFrame, _worldFrame, &jacobian);

The error will not be thrown if the final line, _plant.CalcJacobian... is commented out.

Any ideas what might be causing this? Is it something to do with the realtime loop of the update method?

Upvotes: 0

Views: 34

Answers (0)

Related Questions