Feature Inquiry: Motion Mode Control Command

[TheIndoorDad]

Hi Tobit,

I'm looking into ways to implement Motion Mode Control (i.e., send desired position and velocity with kp and kd and feedforward torque - 0x400 + ID in the control protocol). I've installed your repo as a python package and used it for position and velocity closed loop control, but unless I'm missing something the motion mode control command is not yet implemented. I've tried to puzzle out how I might modify your source to add the feature, but I'm a C++ novice so I haven't been able to figure it out (yet). Do you have any interest in developing this feature, or do you have any advice as to how I might get started doing so?

I'm aware this would at least require:

• Allowing a set of CAN addresses with a 0x400 offset (i.e., 0x400 + ID);
• Encoding inputs into the required data frame and transmitting to the CAN socket;
• Receiving and decoding reply data;
• Creating a python binding.

Thank you,
Stefan

[2b-t]

Hi Stefan,
I am interested in adding the Motion Control Mode (0x400 + id) and similarly multi-motor commands (0x280). The main reason I have not added them yet is that they do not fit the current design of the code: In my initial design I made a CAN driver publish everything on 0x140 + id and listen to responses on 0x240 + id and this was sufficient for my use case. This resulted in a fairly simple code that I could implement in a few hours. Supporting the other aforementioned use cases requires to split up the generation of messages and sending them, so a partial redesign of the software architecture. I have been very busy over the last few months and never came around to refactor the code accordingly.
I will try to have a look over the weekend how I could support these use cases and let you know!

[TheIndoorDad]

Thank you!

Stefan

[2b-t]

Hi @TheIndoorDad
I had a quick look. I should be able to implement the motion control mode without changing the public API. Internally I would only have to add another offset as a template parameter to the CanNode (with the additional offsets 0x400 and 0x500) and expose it in its API so that the ActuatorInterface can decide which CAN id should be used. In a similar fashion I could also add multi-actuator commands (0x280, not sure how many people actually would need those).

Things get much more involved and require API changes in case I allowed multiple commands to different actuators to be chained and synchronized. In this case I would have to redesign a fair amount of the code and split the generation of the commands and sending them similar to the snippet below:

myactuator_rmd::CanDriver driver {"can0"};
myactuator_rmd::ActuatorInterface actuator_1 {driver, 1};
myactuator_rmd::ActuatorInterface actuator_2 {driver, 2};

myactuator_rmd::VersionDateRequest const version_date_request {actuator_1.versionDate()};
std::future<std::uint32_t> version_date_future {driver.queue(version_date_request)};
myactuator_rmd::PositionAbsoluteRequest const position_absolute_request {actuator_2.positionAbsoluteSetpoint(180.0, 500.0)};
std::future<myactuator_rmd::Feedback> feedback_future {driver.queue(position_absolute_request)};
driver.sendRecv();

std::cout << version_date_future.get() << std::endl;
std::cout << feedback_future.get() << std::endl;

I am not sure if the latter is worth the effort as it makes the driver unintuitive to use. Potentially I could keep the current simple interface and add this as an advance way of interfacing the driver that likely only few people will use.

I will try to implement the motion control mode over the course of the next weeks but as I am quite busy at the moment, so probably only during my holidays at the end of this month.

If you want to have this feature quickly for testing, what you could do is create another CanDriver,

namespace myactuator_rmd {
  // Namespace it so that it does not collide with the existing one
  namespace experimental {

    class ActuatorInterface;
    
    // Use the different send and receive offsets here:
    class CanDriver: public CanNode<0x400,0x500> {
      public:
        CanDriver(std::string const& ifname)
        : CanNode{ifname} {
          return;
        }

        friend ActuatorInterface;
    };
  }
}

create another ActuatorInterface with a single method motionModeControl,

// Please double check the data types, I did not check in detail if the following types make sense
void ActuatorInterface::motionModeControl(double const p_des, double const v_des, double const t_ff,
    std::int16_t const kp, std::int16_t kd) {
  MotionModeControlRequest const request {p_des, v_des, t_ff, kp, kd};
  [[maybe_unused]] MotionModeControlResponse const response {driver_.sendRecv(request, actuator_id_)};
  return;
}

implement the parsing of MotionModeControlRequest and MotionModeControlResponse in requests and responses according to the formulas in the documentation and finally add the Python bindings here.

[TheIndoorDad]

Excellent, thank you. I should be able to get started with motion mode control as you've described - I'll keep you posted.

I'll also take the opportunity now to put my vote in for the addition of multi-motor commands (0x280) also - mostly as a convenience feature if it is not too great a challenge to implement. In my prior testing I often used multi-motor commands to have all motors on a bus hold position or report status at once, though in this case it was most helpful because I was sending commands manually. In a script I expect it would only be a small optimization vs. sending individual commands for each motor, and would require parsing of multiple responses for a single request.

I agree also that the simple interface is likely sufficient for my needs for now.

[TheIndoorDad]

Hi Tobit,

The data frame for motion mode commands is structured differently from others, with most parameters taking up 12-bit ranges and DATA[3] and DATA[6] split between different parameters (see screenshot from protocol below). Do you have any idea how I might implement this? I was looking into modifying single_motor_message.hpp but I'm not sure what to do without a nonstandard 4-bit data type.

Also note that data is ordered high -> low rather than Low byte -> high byte as in other commands.

I was in the process of working out the parsing of MotionModeControlRequest in requests.cpp when I realized I couldn't just use SingleMotorRequest as is.

[2b-t]

Hi Stefan,
You should be able to re-use the existing messages. As you can see here, they are just wrapping an std::array<std::uint8_t,8> data_. For setting individual bits you can use std::bitset similar to the following example that I quickly threw together (try it here):

// Start with a 16-bit unsigned integer
constexpr std::uint16_t v_des {1000};
// Extract the lower 12 bits
std::bitset<12> const v_des_bit {v_des};

// Create a bitset for the entire message
std::bitset<64> bit_data {};
// Copy over the upper 8 and the lower 4 bits
for (std::size_t i = 0; i < 8; ++i) {
  bit_data.set(i + 16, v_des_bit.test(i + 4));
}
for (std::size_t i = 0; i < 4; ++i) {
  bit_data.set(i + 24, v_des_bit.test(i + 0));
}

// Copy the bitset to the array
std::array<std::uint8_t, 8> data {};
// Taken from https://stackoverflow.com/a/58979953
std::bitset<64> const mask {0xff};
for (std::size_t i = 0; i < bit_data.size()/8; ++i) {
  data.at(data.size() - 1 - i) = static_cast<std::uint8_t>(((bit_data >> (8*i)) & mask).to_ulong());
}

Hope this helps!

[TheIndoorDad]

Thank you very much for your help Tobit. I greatly appreciate how generous you've been with your time, especially as you've said you're very busy.

My kludged attempt at implementing motion control is building as a python package without error now, but when I try to import myactuator_rmd_py I get an ImportError that I don't understand:

ImportError: /home/user/ContinuO_Python/myactuator_rmd_py.cpython-38-x86_64-linux-gnu.so: undefined symbol: _ZN14myactuator_rmd11motion_mode17ActuatorInterfaceC1ERNS_6DriverEj

Given that the package at least appears to be compiling and the error is at package import I wonder if it is something to do with the python bindings, but I don't know how to fix it. I also am not confident in the rest of my implementation. Here's what I've done step-by-step:

To can_driver.hpp, line 20, added (as provided):

// Namespace motion_mode so that it does not collide with the existing one
namespace motion_mode {

  class ActuatorInterface;
  
  // Use the different send and receive offsets here:
  class CanDriver: public CanNode<0x400,0x500> {
    public:
      CanDriver(std::string const& ifname)
      : CanNode{ifname} {
        return;
      }

      friend ActuatorInterface;
  };
}

To actuator_interface.hpp, line 29, added:

namespace motion_mode {
  class ActuatorInterface {
    public:
      ActuatorInterface(Driver& driver, std::uint32_t const actuator_id);

      void motionModeControl(float const p_des, float const v_des, std::uint16_t const kp, std::uint16_t const kd, float const t_ff);

    protected:
      Driver& driver_;
      std::uint32_t actuator_id_;
  };
}

and to actuator_interface.cpp, line 21, added:

namespace motion_mode {
  // Checked and modified data types slightly.
  void ActuatorInterface::motionModeControl(float const p_des, float const v_des, std::uint16_t const kp, std::uint16_t const kd, float const t_ff) {
    MotionModeControlRequest const request {p_des, v_des, kp, kd, t_ff};
    //[[maybe_unused]] MotionModeControlResponse const response {driver_.sendRecv(request, actuator_id_)};
    return;
  }
}

I wasn't certain whether these (above) also should be in the motion_mode namespace. Let me know if that is wrong. Also let me know if I have done the declaration in the .hpp file wrong (as I've mentioned I'm a novice in C++ - I believe I had to add a declaration in the header for the definition in the .cpp, right?) I left the response commented out for now.

To requests.hpp, line 24, added:

namespace motion_mode {
  class MotionModeControlRequest: public
  Message{
    public:
    MotionModeControlRequest(float const p_des, float const v_des, std::uint16_t const kp, std::uint16_t const kd, float const t_ff);
    using Message::Message;
  };
}

and to requests.cpp, added #include <bitset> and #include <array>, and at line 15:

namespace motion_mode {
  MotionModeControlRequest::MotionModeControlRequest(float const p_des,    float const v_des, std::uint16_t const kp, std::uint16_t const kd, float const t_ff)
  : Message{} {
    // Convert from float radians to uint16_t for data field.
    // Position -12.5 rad to 12.5 rad, int 0 to 65535.
    auto const p_int {static_cast<std::uint16_t>((p_des + 12.5f) * 2621.4f)};
    // Velocity -45 rad/s to 45 rad/s, int 0 to 4095 (12-bit).
    auto const v_int {static_cast<std::uint16_t>((v_des + 45.0f) * 45.5f)};
    // Ff-torque -24 Nm to 24 Nm, int 0 to 4095 (12-bit).
    auto const t_int {static_cast<std::uint16_t>((t_ff + 24.0f) * 85.3125f)};

    // Create bitsets for each parameter. 
    std::bitset<16> const p_bit {p_int};
    std::bitset<12> const v_bit {v_int};
    std::bitset<12> const kp_bit {kp};
    std::bitset<12> const kd_bit {kd};
    std::bitset<12> const t_bit {t_int};

    // Create bitset for entire message.
    std::bitset<64> bit_msg {};

    // Copy parameter bitsets to message bitset.
    for (std::size_t i = 0; i < 16; ++i) {
      bit_msg.set(i + 48, p_bit.test(i));
    }
    for (std::size_t i = 0; i < 12; ++i) {
      bit_msg.set(i + 36, v_bit.test(i));
      bit_msg.set(i + 24, kp_bit.test(i));
      bit_msg.set(i + 12, kd_bit.test(i));
      bit_msg.set(i, t_bit.test(i));
    }

    // Taken from https://stackoverflow.com/a/58979953
    std::bitset<64> const mask {0xff};
    for (std::size_t i = 0; i < bit_msg.size()/8; ++i) {
      Message::data_.at(Message::data_.size() - 1 - i) = static_cast<std::uint8_t>(((bit_msg >> (8*i)) & mask).to_ulong());
    }

    return;
  }
}

Thank you for the advice on std::bitset. I wasn't certain how to interface with the messages, so I guessed that I could use Message and assign to data_, but please let me know if this is wrong also. Otherwise comments similar to previous sections.

Finally, python bindings added to /bindings/myactuator_rmd.cpp, line 72 (just after pybind11::class_<myactuator_rmd::Driver>(m, "Driver"); (I thought that the Driver binding might be required before the new MotionCanDriver one):

pybind11::class_<myactuator_rmd::motion_mode::CanDriver, myactuator_rmd::Driver>(m, "MotionCanDriver")
  .def(pybind11::init<std::string const&>());
pybind11::class_<myactuator_rmd::motion_mode::ActuatorInterface>(m, "MotionActuatorInterface") 
  .def(pybind11::init<myactuator_rmd::Driver&, std::uint32_t>())
  .def("motionModeControl", &myactuator_rmd::motion_mode::ActuatorInterface::motionModeControl);

Most of this I did just by trying to copy what you had done in other examples, so I'm sure some of it is wrong in addition to the unresolved error importing. You've already helped a great deal, but if you could take another glance at this I would appreciate it.

[2b-t]

Hi again,
The error you are getting regarding _ZN14myactuator_rmd11motion_mode17ActuatorInterfaceC1ERNS_6DriverEj (see keyword name mangling for why it looks so "strange") means that inside the compiled library it is looking for a definition of myactuator_rmd::motion_mode::ActuatorInterface::ActuatorInterface(Driver&, unsigned int) but can't find it. The reason for this is that you have only provided the declaration ActuatorInterface(Driver& driver, std::uint32_t const actuator_id); but not its implementation. You need to add a definition similar to

myactuator_rmd/src/actuator_interface.cpp

Lines 22 to 26 in 3fb228a

	ActuatorInterface::ActuatorInterface(Driver& driver, std::uint32_t const actuator_id)
	: driver_{driver}, actuator_id_{actuator_id} {
	driver.addId(actuator_id); // Make the actuator listen to the responses
	return;
	}

as well.
Other than that the structure looks alright on first glance but I did not have a look at the message parsing in detail. Re-using the Message should work fine, yes.
I hope this helps! Let me know how the tests with the actuator go then!

[TheIndoorDad]

Yes, that has worked. Thank you!

I also had to add

    driver_.sendRecv(request, actuator_id_);

in the src/actuator_interface.cpp ActuatorInterface::motionModeControl defintion, after the request, since I hadn't coded a response and commented out the response line you marked as [[maybe unused]]. With that change motion mode control appears to be working in my very early tests, and I'm about to see if I can execute a walking gait on a quadruped using it.

[2b-t]

Nice, really happy to hear that!

[TheIndoorDad]

Hi Tobit,

I'm about to go away for a couple weeks, so I figured I would give you a brief update on my motion mode control testing.

So far, results have been mixed. As I mentioned in my previous post, the code is working for sending individual motion mode commands. However, the motors seem to behave more unreliably under motion mode control: it varies by motor, but some start to make a buzzing sound when even very small external torques are applied then trigger a stall error and shutdown. In some cases this even caused the rest of the motors on the bus to shutdown at the same time - though I hypothesize this may be caused by some protection in my power distribution system tripping from an increased current draw by the stalling motor (still investigating - its strange because this does not occur when using closed-loop position control and the external torque required to trigger this is very low). In other cases the motors don't stall but do buzz loudly when commanded to hold a position (i.e., desired position command with desired velocity = 0) under small load. I've tried using a variety of gain values (including e.g., setting kd=0 for position control only) and have not found a solution yet, but still investigating. Anyway, the result of these issues is that I have not yet been able to test path following with repeated commands in motion mode control. I will continue to investigate on my return at the end of the month.

Also FYI: the motors do not allow motion mode commands and closed-loop (position or velocity) commands to be mixed. Using one will lock the motor into that control mode, requiring shutdown before the other may be used.

Cheers,
Stefan

[2b-t]

Hi Stefan,
Thanks for the update. I unfortunately do not have multiple motors to try it with but if there is something that I could test on a single motor, fork this repository, push your changes to a new branch and let me know what I should test.

A few remarks:

• Are you sure that there is no error in the conversion of the floating point numbers in the message that might cause this issue?
• How are you choosing the set points? Are you sending a feed forward torque? From what I understand the set point of this mode is actually current and is calculated according to I = [kp*(p_des - p_actual) + kd*(v_des - v_actual) + t_ff]*kt. So when setting the feed-forward torque to 0 this is essentially a PD controller with the gains kp*kt and kd*kt.

[TheIndoorDad]

Hi Tobit,

I forked the repository and added my changes as a new branch motion_control_mode (as I probably should have done in the first place). Feel free to try it out or use however you like.

You may be able to test on a single motor, but the behaviour noted in my last comment seems to occur on some of my motors and not on others, so it may be a toss-up whether any motor you test will do the same. This also causes me to suspect it is something to do with the motors, rather than our software.

For a test, I would recommend sending a single command to some position p_des (in rad from -12.5 to 12.5) with:

v_des = 0 # -45.0 to 45.0 rad/s motor-side of reducer
# Motion control gains, 12-bit int 0-4095:
kp = 50 # converts from 0-4095 to 0-500 (50 ~= 6.1).
kd = 819 # converts from 0-4095 to 0-5 (819 = 1).
t_ff = 0 # -24.0 to 24.0 Nm

And see if your motor exhibits the same buzzing or stall when a small (much smaller than expected) external torque is applied. My motors are either RMD-X8-Pro-H 1:6 (V3) or RMD-X8-Pro 1:9 (V2) - I've tried adjusting the gains and the buzzing/stall behaviour occurs regardless on certain motors, just with varying stiffness, but might be that different models will require different gains.

Syntax example:

motion_can = myactuator_rmd_py.MotionCanDriver("can0")
motion_motor = myactuator_rmd_py.MotionActuatorInterface(motion_can, can_ID)

motion_motor.motionModeControl(p_des, v_des, kp, kd, t_ff) # gains as 12-bit int 0-4095, see above comment

Per your remarks:

• I've been over the float conversion a few times and I don't think there is any issue, but I can't say I'm certain. In particular, I'm not very experienced with memory limits when converting various data types in computing, so I wouldn't be too surprised if there was something I'm missing there. In my python scripts I have been using round(p_des, 3) to round p, v input parameters to 3 decimal places before sending to the function (p_des, v_des, and t_ff are of type float in C++). Also be careful not to go outside the input limits (e.g., -12.5 to 12.5 rad for p_des).
• I've used t_ff = 0 for all my testing.
• For single commands, I've tested a variety of position/velocity setpoints based on the desired behaviour (according to the formula you posted). I've tested pure position control (kd = 0), velocity control (kp = 0), and position commands with v_des = 0 (kp != 0, kd != 0). In the future, I intend to send commands every time step (~2 ms) with position and velocity setpoints from a simulated walking gait (motors are joints in a leg).

[2b-t]

Hi Stefan,
perfect, thanks! I will test it with my actuator once I am back from my holidays and double-check the implementation but I could imagine this being issue with the firmware on the actuators.
It may be worth to get in touch with MyActuator specifying the firmware version of the motors that you are using (use their Windows tool for that or getVersionDate in this SDK for this purpose) and check back with them if this is a known issue. It could be that this is a known problem and there is a firmware fix for it or it might be a user error that they might be able to point out easily.
I will report back once I tried it on my actuator.
Tobit

[TheIndoorDad]

A quick update with good news: the stalling problem was resolved with a firmware update. The problem occurred on firmware version 2023041301, and not with firmware version 2022101701 (on the same motors).

[2b-t]

Awesome Stefan, thanks for letting me know!
Unfortunately I am quite busy with work at the moment and did not manage to try your fork out on my motor yet. In less than two weeks from now there is a submission deadline for a major robotics conference and therefore it is quite busy around the lab...

[TheIndoorDad]

No problem. I'm still working on troubleshooting the other issues we discussed.