Adds LCM franka panda command publishers and receivers

[wernerpe]

Adds LCM interface to franka panda. A draft of this code was shared by @sammy-tri. I modified the name spaces, added bindings, and added testing for the bindings. I was somewhat unsure how to handle the panda control modes lcmt_panda_status::CONTROL_MODE_* so i added a namespace PandaControlMode in the panda_constants.h.

---

This change is 

[wernerpe]

Adding @sammy-tri for feature review

Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @wernerpe)

[wernerpe]

+assignee:@sammy-tri

Reviewable status: LGTM missing from assignee sammy-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @wernerpe)

[wernerpe]

I am confused why these builds are failing. I reran the script for generating the docstrings. I noticed that it changed more than just the docstrings corresponding to the new files so perhaps they were outdated? What would you recommend doing?

[jwnimmer-tri]

I reran the script for generating the docstrings. I noticed that it changed more than just the docstrings corresponding to the new files so perhaps they were outdated?

The docstring regeneration is not supported on Jammy, only Noble or macOS. That's why it changed unrelated files. Those extra changes should be reverted.

What would you recommend doing?

The platform reviewer can push the correct docstrings at the conclusion of the review.

[wernerpe]

gotcha I force pushed those docstring changes away. I wont touch the docstrings until feature review passes then 👍

Reviewable status: LGTM missing from assignee sammy-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @wernerpe)

[sammy-tri]

@sammy-tri reviewed 4 of 5 files at r2.
Reviewable status: LGTM missing from assignee sammy-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @wernerpe)

[sammy-tri]

@sammy-tri reviewed all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee sammy-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @wernerpe)

---

manipulation/franka_panda/panda_constants.h line 14 at r2 (raw file):

 * to enable multiple control modes simultaneously.
 * Values match lcmt_panda_status::CONTROL_MODE_* constants. */
enum class PandaControlMode : int {

I'm kinda jumping through hoops trying to figure out if this use of enum is legal either by the C++ standards or our coding style, and I kinda can't tell. It does allow producing invalid combinations (e.g. kPosition | kVelocity or kVelocity | kTorque (of course the LCM type has the same problem and even documents that some of the weird types might be allowed).

I'm not sure what I want to do about this. I think maybe we should define named constants for the or'd values because we know that there's a chance they can be stored and then the compiler can perform reasonable checks. What do you think?

[wernerpe]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sammy-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @wernerpe)

---

manipulation/franka_panda/panda_constants.h line 14 at r2 (raw file):

Previously, sammy-tri (Sam Creasey) wrote…

I'm kinda jumping through hoops trying to figure out if this use of enum is legal either by the C++ standards or our coding style, and I kinda can't tell. It does allow producing invalid combinations (e.g. kPosition | kVelocity or kVelocity | kTorque (of course the LCM type has the same problem and even documents that some of the weird types might be allowed).

I'm not sure what I want to do about this. I think maybe we should define named constants for the or'd values because we know that there's a chance they can be stored and then the compiler can perform reasonable checks. What do you think?

I am a bit confused. Since, I am aiming to use the drake franka driver I think all of the modes are mutually exclusive, so all the or logic is not necessary?
https://github.com/RobotLocomotion/drake-franka-driver/blob/main/franka-driver/franka_driver.cc#L32

[sammy-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sammy-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @wernerpe)

---

manipulation/franka_panda/panda_constants.h line 14 at r2 (raw file):

Previously, wernerpe (Peter Werner) wrote…

I am a bit confused. Since, I am aiming to use the drake franka driver I think all of the modes are mutually exclusive, so all the or logic is not necessary?
https://github.com/RobotLocomotion/drake-franka-driver/blob/main/franka-driver/franka_driver.cc#L32

ok, now I've confused myself. I'll dig for a bit in the old (pre-public) version and try to figure out what this logic was actually doing.

[sammy-tri]

-a:@sammy-tri +a:@ggould-tri if you could take over for this I'd appreciate it

Reviewable status: 1 unresolved discussion, LGTM missing from assignee ggould-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @wernerpe)

[ggould-tri]

👍 ; no problem. I'm a bit loaded at the moment but I should be able to fit this in this afternoon.

@wernerpe This seems to be failing one or more ruff lint tests; I recommend following the instructions to fix the lint to expose any other issues that might be hidden in the CI noise. I've flagged one likely error site below.

Running bazel run -- //tools/lint:ruff format bindings/pydrake/manipulation/test/franka_panda_test.py will probably set you right.

Reviewable status: 2 unresolved discussions, LGTM missing from assignee ggould-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @wernerpe)

---

bindings/pydrake/manipulation/test/franka_panda_test.py line 107 at r2 (raw file):

        # Constructor variant.
        mut.PandaStatusSender(num_joints=mut.kPandaArmNumJoints)

minor: The lack of terminal newline here may be causing much of your CI noise.

[wernerpe]

I just rebased on drake master and fixed the lint error. Thanks for the hint!

[ggould-tri]

@ggould-tri reviewed 3 of 26 files at r1, 5 of 5 files at r2, all commit messages.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee ggould-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @wernerpe)

---

manipulation/franka_panda/panda_constants.h line 14 at r2 (raw file):

Previously, sammy-tri (Sam Creasey) wrote…

ok, now I've confused myself. I'll dig for a bit in the old (pre-public) version and try to figure out what this logic was actually doing.

FYI My view, if the code is intended as written

• The implementation side absolutely treats this as a bitfield,
• Styleguide suggests "uint32_t or uint64_t are allowed for bitfields"
• I'm pretty sure enum class is wrong for bitfields, for exactly the reason the logic below indicates: enum class erases the storage class's underlying ops.

Moreover, in a formal sense kPosition | kVelocity is not a member of the enumeration. Most languages would disallow this approach.

None of this code is defective or ub as written, but I would suggest clarifying the storage type to unsigned or std::bitset<4> Nothing in our style guide disallows enums for this case, but it definitely rubs me wrong.

HOWEVER I can also find no evidence in e.g. https://frankarobotics.github.io/libfranka/0.15.0/classfranka_1_1Robot.html that control modes can ever be combined. While I assume that since you're writing franka code you know franka better than I do, unless you have evidence to the contrary, I believe these should be treated as mutually exclusive and the bitwise operators removed. The real robot is authoritative as to its own capabilities, after all.

---

manipulation/franka_panda/panda_command_receiver.cc line 106 at r3 (raw file):

  if (commanded_position_output_ == nullptr) {
    throw std::runtime_error(
        "Invalid call to PandaCommandReciver::get_command_position_output_port"

typo

Suggestion:

Receiver

---

manipulation/franka_panda/panda_command_receiver.cc line 116 at r3 (raw file):

  if (commanded_velocity_output_ == nullptr) {
    throw std::runtime_error(
        "Invalid call to PandaCommandReciver::get_command_velocity_output_port"

typo

Suggestion:

Receiver

---

manipulation/franka_panda/panda_command_receiver.cc line 126 at r3 (raw file):

  if (commanded_torque_output_ == nullptr) {
    throw std::runtime_error(
        "Invalid call to PandaCommandReciver::get_command_torque_output_port"

typo

Suggestion:

Receiver

[ggould-tri]

comments below. When you are satisfied with the state of this PR and discussions are resolved, let me know and I'll figure out how to push updated doxygen.

@ggould-tri reviewed 16 of 26 files at r1, 5 of 5 files at r3.
Reviewable status: 4 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @wernerpe)

[wernerpe]

thank you for taking the time! I think the main outstanding item is figuring out what to do about the bitfields. I am not sure what the best approach is...

Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

---

manipulation/franka_panda/panda_constants.h line 14 at r2 (raw file):

Previously, ggould-tri wrote…

FYI My view, if the code is intended as written

• The implementation side absolutely treats this as a bitfield,
• Styleguide suggests "uint32_t or uint64_t are allowed for bitfields"
• I'm pretty sure enum class is wrong for bitfields, for exactly the reason the logic below indicates: enum class erases the storage class's underlying ops.

Moreover, in a formal sense kPosition | kVelocity is not a member of the enumeration. Most languages would disallow this approach.

None of this code is defective or ub as written, but I would suggest clarifying the storage type to unsigned or std::bitset<4> Nothing in our style guide disallows enums for this case, but it definitely rubs me wrong.

HOWEVER I can also find no evidence in e.g. https://frankarobotics.github.io/libfranka/0.15.0/classfranka_1_1Robot.html that control modes can ever be combined. While I assume that since you're writing franka code you know franka better than I do, unless you have evidence to the contrary, I believe these should be treated as mutually exclusive and the bitwise operators removed. The real robot is authoritative as to its own capabilities, after all.

I see. I was using these combined control bits on the receiver end, in order to decide which fields of the LCM messages to pay attention to. So this more corresponds to the capabilities of the drake franka driver and not the franka arm itself. E.g. if I set both the position and velocity bit i combined both references to do PD control, whereas if I only sent positions I used the TRI approach of doing finite differencing to cook up velocity references. Hmm. What do you think is the best approach? At the end of the day I will likely need to modify the other PR on the drake-franka-driver (RobotLocomotion/drake-franka-driver#4) in tandem to make both compatible.

[ggould-tri]

@ggould-tri reviewed 1 of 1 files at r4, all commit messages.
Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @wernerpe)

---

manipulation/franka_panda/panda_constants.h line 14 at r2 (raw file):

Previously, wernerpe (Peter Werner) wrote…

I see. I was using these combined control bits on the receiver end, in order to decide which fields of the LCM messages to pay attention to. So this more corresponds to the capabilities of the drake franka driver and not the franka arm itself. E.g. if I set both the position and velocity bit i combined both references to do PD control, whereas if I only sent positions I used the TRI approach of doing finite differencing to cook up velocity references. Hmm. What do you think is the best approach? At the end of the day I will likely need to modify the other PR on the drake-franka-driver (RobotLocomotion/drake-franka-driver#4) in tandem to make both compatible.

Ah, that makes a lot more sense -- I hadn't realized the combined PD layer was in there (which is embarrassing as I wrote the same for UR arm some time ago).

I've put together a suggestion here.

---

manipulation/franka_panda/panda_constants.h line 49 at r4 (raw file):

constexpr int to_int(PandaControlMode mode) {
  return static_cast<int>(mode);
}

BTW: My suggestion for this code:

• The "enum class" was not serving as a bounded type domain but simply serving as a namespace to hold integer constants. Constants are not an enum
• Drake's styleguide specifically proposes the use of unsigned int types for bitwise operations
• We could use bitfield<> for this, but that class is unfamiliar to most readers and would be a bit too "clever" to optimize readability.

So the existing code can be simplified to replace enum class with namespace and to type alias an unsigned int type.

The static_cast is not dangerous since c++20 (it used to have ub) so I've not added a check there; this lets you keep the constexpr.

Suggestion:

using ControlMode = uint64_t;

namespace PandaControlMode {
  ControlMode kNone = 0,
  ControlMode kPosition = 1,
  ControlMode kVelocity = 2,
  ControlMode kTorque = 4,
};

// Helper to convert to int for use in comparisons and LCM messages
constexpr int to_int(ControlMode mode) {
  return static_cast<int>(mode);
}

[wernerpe]

Ok thanks for the feedback! I slightly changed the names around a bit because 'ControlMode' as an input type sounded too generic for the panda-specific LCM systems.

Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

[wernerpe]

Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

---

manipulation/franka_panda/panda_constants.h line 14 at r2 (raw file):

Previously, ggould-tri wrote…

Ah, that makes a lot more sense -- I hadn't realized the combined PD layer was in there (which is embarrassing as I wrote the same for UR arm some time ago).

I've put together a suggestion here.

Done.

[ggould-tri]

re-up; I'll see about spinning doxygen shortly.

@ggould-tri reviewed 10 of 10 files at r5, all commit messages.
Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @wernerpe)

[ggould-tri]

FYI once again complaining it needs bazel run -- //tools/lint:ruff format bindings/pydrake/manipulation/test/franka_panda_test.py

[ggould-tri]

Previously, ggould-tri wrote…

FYI once again complaining it needs bazel run -- //tools/lint:ruff format bindings/pydrake/manipulation/test/franka_panda_test.py

I fixed this and one other linter error when I pushed the new bindings docstrings...

[wernerpe]

Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

---

a discussion (no related file):

Previously, ggould-tri wrote…

I fixed this and one other linter error when I pushed the new bindings docstrings...

ok thank you!

[ggould-tri]

+(status: squashing now)

Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers

[ggould-tri]

@ggould-tri dismissed @sammy-tri from a discussion.
Reviewable status: needs at least two assigned reviewers

[ggould-tri]

Okay, I'm a bit confused by the assignment situation here, but I'm adding +a:@xuchen-han for platform review.

Reviewable status: LGTM missing from assignee xuchen-han(platform)

[xuchen-han]

-(status: squashing now)

@xuchen-han reviewed 11 of 26 files at r1, 1 of 5 files at r2, 4 of 5 files at r3, 8 of 10 files at r5, 3 of 3 files at r6, all commit messages.
Reviewable status: 2 unresolved discussions

---

manipulation/franka_panda/panda_status_sender.cc line 65 at r6 (raw file):

    int max_num_connected, const Eigen::VectorXd& zeros,
    const InputPort<double>& port1, const InputPort<double>& port2,
    const int port2_tail = -1) {

nit, looks like this parameter is never used?

Code quote:

    const int port2_tail = -1) {

---

manipulation/franka_panda/panda_status_sender.cc line 120 at r6 (raw file):

  const auto& torque_external = EvalFirstConnected(
      context, 0, 2, zero_vector_, get_torque_external_input_port(),
      get_torque_external_input_port());

nit, this is a bit awkward. Consider using the HasValue() ? ... : ... pattern above.

Code quote:

  const auto& torque_external = EvalFirstConnected(
      context, 0, 2, zero_vector_, get_torque_external_input_port(),
      get_torque_external_input_port());

---

manipulation/franka_panda/panda_status_receiver.h line 60 at r6 (raw file):

 private:
  template <std::vector<double> drake::lcmt_panda_status::*>

Didn't know about this syntax before... TIL

[wernerpe]

Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

---

manipulation/franka_panda/panda_status_sender.cc line 65 at r6 (raw file):

Previously, xuchen-han (Xuchen Han) wrote…

nit, looks like this parameter is never used?

i think it is used in line 82

[wernerpe]

Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @wernerpe)

[wernerpe]

+(status: squashing now)

Reviewable status: complete! all discussions resolved, LGTM from assignees ggould-tri(platform),xuchen-han(platform)