Skip to content
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,9 @@ public class RDXAbilityHand implements RDXHandInterface
private final ImFloat[] desiredVelocities = new ImFloat[6];

private float[] currentPositions = new float[6];
private float[] handGoalPositions = new float[6];
private boolean setSlidersUsingGoalPosition = false;

private Grip executeGrip = null;
private boolean executeVelToPos = false;
private final Throttler publishThrottler = new Throttler().setFrequency(30.0);
Expand Down Expand Up @@ -65,6 +68,7 @@ public void update()
}

currentPositions = communication.readState(identifier).getActuatorPositions();
handGoalPositions = communication.readState(identifier).getGoalPositions();

if ((executeGrip != null || executeVelToPos) && publishThrottler.run())
{
Expand Down Expand Up @@ -109,9 +113,16 @@ public void renderImGuiWidgets()
ImGui.sameLine();
}

// Set position slider values using current position when commanding a grip (goal position doesn't get set the same way)
if (executeGrip != null)
setSlidersUsingGoalPosition = false;

// Update position slider values using either current or goal positions
float[] sliderPositionSetter = setSlidersUsingGoalPosition ? handGoalPositions : currentPositions;

boolean scheduleExecuteVelToPos = false;
for (int i = 0; i < 6; i++)
{
{
float sliderMin = 0.0f;
float sliderMax = i == 5 ? -120.0f : 120.0f; // thumb rotator moves negative
float currentNotch = (currentPositions[i] - sliderMin) / (sliderMax - sliderMin);
Expand All @@ -122,7 +133,7 @@ public void renderImGuiWidgets()
scheduleExecuteVelToPos |= ImGui.sliderFloat(labels.getHidden(FINGER_NAMES[i]), desiredPositions[i].getData(), sliderMin, sliderMax,
"%s: %.2f%s flexion".formatted(FINGER_NAMES[i], currentPositions[i], EuclidCoreMissingTools.DEGREE_SYMBOL));
if (!ImGui.isItemActive() && !executeVelToPos) // Prevent overriding externally submitted positions too
desiredPositions[i].set(currentPositions[i]);
desiredPositions[i].set(sliderPositionSetter[i]);
ImGui.popItemWidth();
ImGui.sameLine();
ImGui.pushItemWidth(ImGui.getColumnWidth());
Expand All @@ -131,7 +142,12 @@ public void renderImGuiWidgets()
}

if (scheduleExecuteVelToPos)
{
executeVelToPos = true;
// Set position slider values using goal position when using position sliders
setSlidersUsingGoalPosition = true;
}

ImGui.endDisabled();
}

Expand Down
Loading