A trajectory with velocity only is not possible

[AndyZe]

I believe it used to be possible to publish a velocity-only command of type trajectory_msgs/JointTrajectory to a joint_trajectory_controller/joint_trajectory topic. That's not possible anymore:

Mismatch between joint_names size (7) and positions (0) at point #0.

That warning comes from here:

ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp

Line 1343 in 9b344c7

bool JointTrajectoryController::validate_trajectory_point_field(

I tried commenting that vector size check but then the control node just crashes.

(I'm testing on Humble branch.)

[christophfroehlich]

Isn't allow_integration_in_goal_trajectories what you are looking for?

[AndyZe]

It looks like the logic related to allow_integration_in_goal_trajectories isn't correct. Note this isn't a big issue for me, just wanted to point out the issue.

    if (params_.allow_integration_in_goal_trajectories)
    {
      const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() &&
                             points[i].accelerations.empty();
      const bool position_error =
        !points[i].positions.empty() &&
        !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false); // SHOULD BE TRUE FOR ALLOW_EMPTY?

[christophfroehlich]

Why do you think?
If there are position/velocity/acceleration fields but not of the correct size (checked in validate_trajectory_point_field), or all are empty, then validate_trajectory_msg returns false. With allow_empty=true, then the size would not be checked.
If they are not complete but got accepted, they states will be integrated here

ros2_controllers/joint_trajectory_controller/src/trajectory.cpp

Lines 172 to 176 in f64c964

	// it changes points only if position and velocity do not exist, but their derivatives
	deduce_from_derivatives(
	point, next_point, state_before_traj_msg_.positions.size(), (t1 - t0).seconds());
	interpolate_between_points(t0, point, t1, next_point, sample_time, output_state);

[AndyZe]

Thanks for the hint on allow_integration_in_goal_trajectories. I've added that but weirdness is still happening.

I'm not sure what the cause is. I'll try to give a thorough bug report since you're asking for more detail.

ros2 control list_controllers shows the controller is active: robin_velocity_controller joint_trajectory_controller/JointTrajectoryController active

allow_integration_in_goal_trajectories is set to true now, thanks for that hint:

ros2 param dump robin_velocity_controller -> allow_integration_in_goal_trajectories: true

The velocity interfaces seem to be claimed as they should be. 7 are claimed:

command interfaces
	elev1_j1/position [available] [unclaimed]
	elev1_j1/velocity [available] [claimed]
	elev1_j2/position [available] [unclaimed]
	elev1_j2/velocity [available] [claimed]
	elev1_j3/position [available] [unclaimed]
	elev1_j3/velocity [available] [unclaimed]
	wrist_pitch/position [available] [unclaimed]
	wrist_pitch/velocity [available] [claimed]
	wrist_roll/position [available] [unclaimed]
	wrist_roll/velocity [available] [claimed]
	wrist_yaw/position [available] [unclaimed]
	wrist_yaw/velocity [available] [claimed]
	yaw1/position [available] [unclaimed]
	yaw1/velocity [available] [claimed]
	yaw2/position [available] [unclaimed]
	yaw2/velocity [available] [claimed]

Here's the code which sends out my action request to the correct 7 joints, it seems:

  traj_client_ptr_ =
      rclcpp_action::create_client<FollowJointTrajectory>(node_, "/robin_velocity_controller/follow_joint_trajectory");
...
BT::NodeStatus SendStopCommand::onStart()
{
  if (!traj_client_ptr_->wait_for_action_server())
  {
    RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting");
    return BT::NodeStatus::FAILURE;
  }

  trajectory_msgs::msg::JointTrajectoryPoint traj_point_msg;
  traj_point_msg.velocities = std::vector<double>(0.0, JOINT_NAMES.size());
  // TODO: design spec of the joints says they should be able to decelerate in 2s, so we use that value here.
  // Likely they could safely stop faster than that.
  builtin_interfaces::msg::Duration time_from_start;
  time_from_start.sec = STOPPING_SECONDS;
  traj_point_msg.time_from_start = time_from_start;

  trajectory_msgs::msg::JointTrajectory traj_msg;
  traj_msg.joint_names = JOINT_NAMES;
  traj_msg.points.emplace_back(traj_point_msg);
  traj_msg.header.stamp = node_->get_clock()->now();

  FollowJointTrajectory::Goal goal_msg;
  goal_msg.trajectory = traj_msg;

  // Send the goal and wait for the result
  traj_goal_future_ = traj_client_ptr_->async_send_goal(goal_msg);
  if (rclcpp::spin_until_future_complete(node_, traj_goal_future_) != rclcpp::FutureReturnCode::SUCCESS)
  {
    RCLCPP_ERROR(node_->get_logger(), "Failed to send goal");
    return BT::NodeStatus::FAILURE;
  }

  auto goal_handle = traj_goal_future_.get();
  if (!goal_handle)
  {
    RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
    return BT::NodeStatus::FAILURE;
  }

  // Wait for the traj to complete
  RCLCPP_INFO(node_->get_logger(), "Waiting for the traj goal to complete");
  return BT::NodeStatus::RUNNING;

I see this print from the traj controller, nothing else: [ros2_control_node-5] [INFO] [1733098757.168904128] [robin_velocity_controller]: Received new action goal

My app prints RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");.

[christophfroehlich]

This is weird. The goal gets rejected only if validate_trajectory_msg returns false, and for every return false there should be something in the error log. (except for all_empty, I'll add a PR for that). Maybe you could build it from source and debug this method?