RShoulderRoll and LeftShoulderRoll joint commands jump after dynup

[texhnolyze]

Summary

Looking at the ID_rory_2024-07-18T13:29:30_0.mcap recording from RoboCup 2024, there are jumps from joint command poitions of 0 to pi (3.14159).

As it's only a single message and not consistent between both joints it seems to be a bug, where a wrong motor command is sent.

As we do not actually move the shoulders half a motor rotation, this single message is either prevented by ros_control or it is only sent so shortly (1 single /DynamixelController/command` message, that the motors do not have time to execute it.

Expected behavior

There should not be a sudden jump from 0 position to pi position.

Steps to view or further investigate

1. Download Rosbag
2. Open it in Lichtblick/Foxglove
3. Add custom [user script]
4. Add plotting panel for /studio_script/shoulder_roll_commands.rShoulderRoll
5. Navigate to timestamp from screenshot or look for another time of getting up

Steps to Reproduce

As the effects of this issue are not apparent, I'm unsure when it actually happens or how to reproduce it.

[user script]:

// The ./types module provides helper types for your Input events and messages.
import { Input, Message } from "./types";

// Your script can output well-known message types, any of your custom message types, or
// complete custom message types.
//
// Use `Message` to access types from the schemas defined in your data source:
// type Twist = Message<"geometry_msgs/Twist">;
//
// Import from the @foxglove/schemas package to use foxglove schema types:
// import { Pose, LocationFix } from "@foxglove/schemas";
//
// Conventionally, it's common to make a _type alias_ for your script's output type
// and use that type name as the return type for your script function.
// Here we've called the type `Output` but you can pick any type name.
type Output = {
  rShoulderRoll?: number;
  lShoulderRoll?: number;
};

// These are the topics your script "subscribes" to. Studio will invoke your script function
// when any message is received on one of these topics.
export const inputs = ["/DynamixelController/command"];

// Any output your script produces is "published" to this topic. Published messages are only visible within Studio, not to your original data source.
export const output = "/studio_script/shoulder_roll_commands";

// This function is called with messages from your input topics.
// The first argument is an event with the topic, receive time, and message.
// Use the `Input<...>` helper to get the correct event type for your input topic messages.
export default function script(
  event: Input<"/DynamixelController/command">,
): Output {
  const publishMessage: Output = {
    rShoulderRoll: NaN,
    lShoulderRoll: NaN,
  };

  if (event.message && event.message.joint_names) {
    const rShoulderRollIndex = event.message.joint_names.findIndex(
      (n) => n === "RShoulderRoll",
    );
    const lShoulderRollIndex = event.message.joint_names.findIndex(
      (n) => n === "LShoulderRoll",
    );

    if (rShoulderRollIndex !== -1) {
      publishMessage.rShoulderRoll =
        event.message.positions[rShoulderRollIndex];
    }

    if (lShoulderRollIndex !== -1) {
      publishMessage.lShoulderRoll =
        event.message.positions[lShoulderRollIndex];
    }
  }

  return publishMessage;
}