Stop position commands

[HebiMigrator]

I'm sending a position command to my actuators, they fulfill the demand fine - this happens due to a fulfilled if-statement within a While Loop. When I try to send velocity commands after the if-statement has been fulfilled it bugs on my. It's like the position command is still active. Can I somehow stop the position command for affecting the actuators after fulfilled the first time?

Best,
Rasmus

Originally posted by HD-Lab_Rasmus on forums.hebi.us » Wed Mar 03, 2021 9:50 am

[HebiMigrator]

Rasmus,

Thanks for reaching out.

My guess here is that you are using a CommandStruct/GroupCommand which still has position commands in it. The solution here is either to clear the position command from the CommandStruct/GroupCommand, or to use a new CommandStruct/GroupCommand object. If you let me know which API you are using, I can give more explicit code samples, but generally the documentation for this object in MATLAB, Python, and C++ can be found here:
https://docs.hebi.us/docs/matlab/hebi-m ... truct.html
https://docs.hebi.us/docs/python/2.3.0/ ... oupCommand
https://docs.hebi.us/docs/cpp/cpp-3.3.0 ... mmand.html

However, there could be a lot of things going on here -- if this isn't the issue, would you be able to share a code snippet here, and describe a little more about what exactly is happening?

Best,
Matt

Originally posted by matt_tesch on forums.hebi.us » Mon Mar 08, 2021 11:00 am

[HebiMigrator]

Hi Matt,

I'm using the GroupCommand in Python.

My code looks like this:

A main while loop is running while i use a Mobile Device to control actuators.
group_m is my mobile device, group_a1 and group_a2 are the actuators.

while not abort_flag:
    current_time = time()
    fbk = group_m.get_next_feedback(reuse_fbk=fbk)
    buttons = np.zeros(8)
    sliders = np.zeros(8)
    for i in range(8):
        buttons[i] = fbk.io.b.get_int(i + 1)
    for i in range(8):
        if fbk.io.a.has_int(i + 1):
            sliders[i] = fbk.io.a.get_int(i + 1)
        elif fbk.io.a.has_float(i + 1):
            sliders[i] = fbk.io.a.get_float(i + 1)

I get feedback from the motors and then try to command a specific position if a specific button is pressed:

    # Feedback from motors
    fbk_a1 = group_a1.get_next_feedback(reuse_fbk=group_feedback1)
    fbk_a2 = group_a2.get_next_feedback(reuse_fbk=group_feedback2)

 # 1m forward
    if buttons[2] == 1:
        pos_addtion_1m = 1.7*2*np.pi #[rad]
        group_command1.position = fbk_a1.position[0] + pos_addtion_1m
        group_a1.send_command(group_command1)
        group_command2.position = fbk_a2.position[0] - pos_addtion_1m
        group_a2.send_command(group_command2)
    # 2m forward
    if buttons[3] == 1:
        pos_addtion_2m = 3.4*2*np.pi #[rad]
        group_command1.position = fbk_a1.position[0] + pos_addtion_2m
        group_a1.send_command(group_command1)
        group_command2.position = fbk_a2.position[0] - pos_addtion_2m
        group_a2.send_command(group_command2)

I hope this makes sense, coding is not my strong side. Also, can I combine the actuators in one group and command them individually?

Best,
Rasmus

Originally posted by HD-Lab_Rasmus on forums.hebi.us » Mon Mar 08, 2021 12:41 pm

[HebiMigrator]

Rasmus,

Sorry for the late reply.

Thanks for sharing the code snippets -- I can point out a few things here that should help and may address the problem. I don't see any velocity commands, though. If you are able to share a full file, I can provide more complete feedback; alternatively, I would be happy to have a short call to walk through some of your questions here as well. If that would help, email [email protected] and cc me ([email protected]) and we'll get you set up.

Regarding the code samples:

Block 1 -- mobile feedback:

The overall approach you have here should work, but I want to point out a couple notes that should fix some bugs that I think will crop up. First, note that the group_m.get_next_feedback call has a specified timeout (I think the default is 0.25s), and does not return immediately if you there is not data that has been returned from the mobile IO device. This means that if you turn off the phone or let it go to sleep (or if the wifi cuts out), your entire control loop will be slowed down here. This will cause issues where commands to the actuators will be very choppy.

Instead, I recommend using the following pattern here:

# Initialize these here so they don't get reset if you don't get feedback
buttons = np.zeros(8)
sliders = np.zeros(8)

while not abort_flag:
    current_time = time()
    
    # The "0" timeout returns immediately with "None" if there is not data ready to be read
    # You do not need to store the returned value of "reuse_fbk", because it is set within the function
    if self._group.get_next_feedback(timeout_ms=0, reuse_fbk=fbk) is None:
        # handle this case if necessary
        print("Did not get feedback from mobile IO")
    else:
        for i in range(8):
            buttons[i] = fbk.io.b.get_int(i + 1)
        for i in range(8):
            if fbk.io.a.has_int(i + 1):
                sliders[i] = fbk.io.a.get_int(i + 1)
            elif fbk.io.a.has_float(i + 1):
                sliders[i] = fbk.io.a.get_float(i + 1)

The second block of code is straightforward, but I would note that there is an extra copy here b/c the "reuse_fbk" argument is not the same as the returned data:

    fbk_a1 = group_a1.get_next_feedback(reuse_fbk=group_feedback1)

This means that you are writing the data into group_feedback1, and then also copying that into fbk_a1. I expect you only need one of these variables.

Also, you should check the return value before using fbk_a1, as this could be "None" if the connection to the module timed out, and unwrapping that value later will cause a crash.

Finally, in the third block -- this is a little concerning, because you seem to be commanding a large "jump" in position here -- over a full rotation of the actuator in a single timestep. Depending on the gains that are set on this module and the available power supply, this may even result in a large sudden power draw into the actuator which decreases the voltage available on the power bus, resetting the modules.

Instead, you should update the position incrementally (see the other post you had where I referenced some of the "trajectory" tools we have). As a couple of references:

Simple single module trajectory:
I would start here and get used to this as a standalone example before attempting a full integration into your code. This example demonstrates a smooth motion of a module over a period of time to reach a new destination:
https://github.com/HebiRobotics/hebi-py ... jectory.py

Full system trajectories with mobile IO:
This is a full system example that is pretty close to what you are trying to do, and I think is a good reference to look at to help. Note that the "grav comp" sections can be ignored for your case. This also involves use of the "mobile IO" (experimental) API that makes working with feedback from mobile IO devices a bit smoother. Similar to what I mentioned above, you would want to pass in "timeout_ms = 0" here on the mobile_io.update() call to prevent this from potentially hanging your feedback loop.
https://github.com/HebiRobotics/hebi-py ... control.py

(Note that the actuator command parts of this code have been replaced in our main code branch by use of the "arm" API for controlling full robot arms, so this is an older non-main branch that should be closer to your use case)

Originally posted by matt_tesch on forums.hebi.us » Mon Mar 15, 2021 12:16 pm

[HebiMigrator]

Whoops -- missed your last question there.

You can combine the actuators into one group, but still index into the commands to send individual values for commands. However, when you call "send command", messages are sent to all actuators in the group with their respective commands; you cannot just send to a single member of the group.

Best,
Matt

Originally posted by matt_tesch on forums.hebi.us » Mon Mar 15, 2021 12:30 pm