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
support@hebirobotics.com and cc me (
matt@hebirobotics.com) 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:
Code: Select all
# 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:
Code: Select all
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)