Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

WIP Fix VR Joystick Stepping #617

Open
wants to merge 1 commit into
base: develop
Choose a base branch
from
Open

WIP Fix VR Joystick Stepping #617

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
247 changes: 29 additions & 218 deletions ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/RDXJoystickBasedStepping.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,11 @@
import us.ihmc.commonWalkingControlModules.configurations.SteppingParameters;
import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters;
import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.ContinuousStepGenerator;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories.StepGeneratorAPIDefinition;
import us.ihmc.commons.MathTools;
import us.ihmc.commons.thread.ThreadTools;
import us.ihmc.communication.HumanoidControllerAPI;
import us.ihmc.communication.ros2.ROS2PublisherMap;
import us.ihmc.ros2.ROS2Input;
import us.ihmc.euclid.geometry.Pose3D;
import us.ihmc.euclid.referenceFrame.FramePose3D;
Expand All @@ -35,6 +38,8 @@
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SegmentDependentList;
import us.ihmc.robotics.robotSide.SideDependentList;
import us.ihmc.ros2.ROS2Publisher;
import us.ihmc.ros2.ROS2Topic;
import us.ihmc.sensorProcessing.model.RobotMotionStatus;

import java.util.ArrayList;
Expand All @@ -48,18 +53,13 @@
public class RDXJoystickBasedStepping
{
private final SteppingParameters steppingParameters;
private final SegmentDependentList<RobotSide, ArrayList<Point2D>> controllerFootGroundContactPoints;
private Controller currentController;
private boolean currentControllerConnected;
private RDXVRController leftVRController;
private RDXVRController rightVRController;
private boolean walkingModeActive = false;
private double forwardJoystickValue = 0.0;
private double lateralJoystickValue = 0.0;
private double turningJoystickValue = 0.0;

private final ScheduledExecutorService executorService = Executors.newSingleThreadScheduledExecutor(ThreadTools.createNamedThreadFactory("FootstepPublisher"));
private final ContinuousStepGenerator continuousStepGenerator = new ContinuousStepGenerator();
private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
private final ImDouble turningVelocity = new ImDouble();
private final ImDouble forwardVelocity = new ImDouble();
Expand All @@ -77,6 +77,7 @@ public class RDXJoystickBasedStepping
private ROS2Input<CapturabilityBasedStatus> capturabilityBasedStatusInput;
private ROS2ControllerHelper controllerHelper;
private ROS2SyncedRobotModel syncedRobot;
private ROS2Publisher<ContinuousStepGeneratorInputMessage> stepGeneratorPublisher;

private RDXFootstepPlanGraphic footstepPlanGraphic;
private final SideDependentList<FramePose3D> lastSupportFootPoses = new SideDependentList<>(null, null);
Expand All @@ -94,7 +95,6 @@ public class RDXJoystickBasedStepping
public RDXJoystickBasedStepping(DRCRobotModel robotModel)
{
WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters();
controllerFootGroundContactPoints = robotModel.getContactPointParameters().getControllerFootGroundContactPoints();
steppingParameters = walkingControllerParameters.getSteppingParameters();
swingHeight.set(walkingControllerParameters.getSwingTrajectoryParameters().getMinSwingHeight());
swingDuration.set(walkingControllerParameters.getDefaultSwingTime());
Expand All @@ -108,38 +108,16 @@ public RDXJoystickBasedStepping(DRCRobotModel robotModel)
turnStepWidth.set(steppingParameters.getTurningStepWidth());
turnMaxAngleInward.set(steppingParameters.getMaxAngleTurnInwards());
turnMaxAngleOutward.set(steppingParameters.getMaxAngleTurnOutwards());
continuousStepGenerator.setNumberOfFootstepsToPlan(10);
continuousStepGenerator.setDesiredTurningVelocityProvider(turningVelocity::get);
continuousStepGenerator.setDesiredVelocityProvider(() -> new Vector2D(forwardVelocity.get(), lateralVelocity.get()));
continuousStepGenerator.configureWith(walkingControllerParameters);
continuousStepGenerator.addFootstepAdjustment(this::adjustFootstep);
continuousStepGenerator.setFootstepMessenger(this::prepareFootsteps);
continuousStepGenerator.setFootPoseProvider(robotSide -> lastSupportFootPoses.get(robotSide));
continuousStepGenerator.addFootstepValidityIndicator(this::isStepSnappable);
continuousStepGenerator.addFootstepValidityIndicator(this::isSafeDistanceFromObstacle);
continuousStepGenerator.addFootstepValidityIndicator(this::isSafeStepHeight);
}

public void create(RDXBaseUI baseUI, ROS2ControllerHelper controllerHelper, ROS2SyncedRobotModel syncedRobot)
{
this.controllerHelper = controllerHelper;
this.syncedRobot = syncedRobot;
capturabilityBasedStatusInput = controllerHelper.subscribeToController(CapturabilityBasedStatus.class);
controllerHelper.subscribeToControllerViaCallback(FootstepStatusMessage.class, footstepStatus ->
{
queuedTasksToProcess.add(() ->
{
continuousStepGenerator.consumeFootstepStatus(footstepStatus);

if (footstepStatus.getFootstepStatus() == FootstepStatus.COMPLETED.toByte())
{
lastSupportFootPoses.put(RobotSide.fromByte(footstepStatus.getRobotSide()),
new FramePose3D(ReferenceFrame.getWorldFrame(),
footstepStatus.getActualFootPositionInWorld(),
footstepStatus.getActualFootOrientationInWorld()));
}
});
});
ROS2Topic<?> inputTopic = StepGeneratorAPIDefinition.getInputTopic(syncedRobot.getRobotModel().getSimpleRobotName()).withTypeName(ContinuousStepGeneratorInputMessage.class);
stepGeneratorPublisher = (ROS2Publisher<ContinuousStepGeneratorInputMessage>) controllerHelper.getROS2Node().createPublisher(inputTopic);

syncedRobot.addRobotConfigurationDataReceivedCallback(robotConfigurationData ->
{
RobotMotionStatus newStatus = RobotMotionStatus.fromByte(robotConfigurationData.getRobotMotionStatus());
Expand All @@ -153,12 +131,6 @@ public void create(RDXBaseUI baseUI, ROS2ControllerHelper controllerHelper, ROS2
hasSuccessfullyStoppedWalking.set(true);
});

footstepPlanGraphic = new RDXFootstepPlanGraphic(controllerFootGroundContactPoints);
footstepPlanGraphic.setColor(RobotSide.LEFT, new Color(0.8627451f, 0.078431375f, 0.23529412f, 1.0f)); // crimson
footstepPlanGraphic.setColor(RobotSide.RIGHT, new Color(0.6039216f, 0.8039216f, 0.19607843f, 1.0f)); //yellowgreen

executorService.scheduleAtFixedRate(this::sendFootsteps, 0, 500, TimeUnit.MILLISECONDS);

leftVRController = baseUI.getVRManager().getContext().getController(RobotSide.LEFT);
rightVRController = baseUI.getVRManager().getContext().getController(RobotSide.RIGHT);
baseUI.getVRManager().getContext().addVRInputProcessor(context ->
Expand All @@ -171,133 +143,30 @@ public void create(RDXBaseUI baseUI, ROS2ControllerHelper controllerHelper, ROS2

public void update(boolean enabled)
{
while (!queuedTasksToProcess.isEmpty())
queuedTasksToProcess.poll().run();

currentController = Controllers.getCurrent();
currentControllerConnected = currentController != null;

if (enabled && (currentControllerConnected || (leftVRController.isConnected() && rightVRController.isConnected())))
if (!enabled || !leftVRController.isConnected() || !rightVRController.isConnected())
{
if (currentControllerConnected)
{
walkingModeActive = currentController.getButton(currentController.getMapping().buttonR1);
forwardJoystickValue = -currentController.getAxis(currentController.getMapping().axisLeftY);
lateralJoystickValue = -currentController.getAxis(currentController.getMapping().axisLeftX);
turningJoystickValue = -currentController.getAxis(currentController.getMapping().axisRightX);
}

if (rightVRController.isConnected())
{
walkingModeActive = rightVRController.getClickTriggerActionData().bState() && userNotClickingAnImGuiPanel;
turningJoystickValue = -rightVRController.getJoystickActionData().x();
}

if (leftVRController.isConnected())
{
forwardJoystickValue = leftVRController.getJoystickActionData().y();
lateralJoystickValue = -leftVRController.getJoystickActionData().x();
}

if (capturabilityBasedStatusInput.hasReceivedFirstMessage() && syncedRobot.getDataReceptionTimerSnapshot().isRunning(1.0))
{
CapturabilityBasedStatus capturabilityBasedStatus = capturabilityBasedStatusInput.getLatest();
boolean isLeftFootInSupport = !capturabilityBasedStatus.getLeftFootSupportPolygon3d().isEmpty();
boolean isRightFootInSupport = !capturabilityBasedStatus.getRightFootSupportPolygon3d().isEmpty();
isFootInSupport.set(RobotSide.LEFT, isLeftFootInSupport);
isFootInSupport.set(RobotSide.RIGHT, isRightFootInSupport);
boolean isInDoubleSupport = isLeftFootInSupport && isRightFootInSupport;

if (!supportFootPosesInitialized && isInDoubleSupport)
{
for (RobotSide robotSide : RobotSide.values)
{
lastSupportFootPoses.put(robotSide, new FramePose3D(syncedRobot.getReferenceFrames().getSoleFrame(robotSide)));
}

supportFootPosesInitialized = true;
}

if (supportFootPosesInitialized)
{
for (RobotSide robotSide : RobotSide.values)
{
if (isFootInSupport.get(robotSide))
{ // Touchdown may not have been made with the foot properly settled, so we update the support foot pose if its current pose is lower.
MovingReferenceFrame soleFrame = syncedRobot.getReferenceFrames().getSoleFrame(robotSide);
double currentHeight = soleFrame.getTransformToWorldFrame().getTranslationZ();
if (currentHeight < lastSupportFootPoses.get(robotSide).getZ())
lastSupportFootPoses.put(robotSide, new FramePose3D(soleFrame));
}
}

double stepTime = swingDuration.get() + transferDuration.get();
double deadband = 0.1;
forwardJoystickValue = DeadbandTools.applyDeadband(deadband, forwardJoystickValue);
forwardVelocity.set((maxStepLength.get() / stepTime) * MathTools.clamp(forwardJoystickValue, 1.0));
lateralJoystickValue = DeadbandTools.applyDeadband(deadband, lateralJoystickValue);
lateralVelocity.set((maxStepWidth.get() / stepTime) * MathTools.clamp(lateralJoystickValue, 1.0));
turningJoystickValue = DeadbandTools.applyDeadband(deadband, turningJoystickValue);
// if (forwardVelocity.get() < -0.001) // kinda like it better without this
// turningJoystickValue = -turningJoystickValue;
turningVelocity.set(((turnMaxAngleOutward.get() - turnMaxAngleInward.get()) / stepTime) * MathTools.clamp(turningJoystickValue, 1.0));

if (walkingModeActive)
{
isWalking.set(true);
continuousStepGenerator.startWalking();
hasSuccessfullyStoppedWalking.set(false);
}
else
{
disableWalking();
footstepPlanGraphic.clear();
}

continuousStepGenerator.setFootstepTiming(swingDuration.get(), transferDuration.get());
continuousStepGenerator.setStepTurningLimits(turnMaxAngleInward.get(), turnMaxAngleOutward.get());
continuousStepGenerator.setStepWidths(defaultStepWidth.get(), minStepWidth.get(), maxStepWidth.get());
continuousStepGenerator.setMaxStepLength(maxStepLength.get());
continuousStepGenerator.update(Double.NaN);
}
}

footstepPlanGraphic.update();
// TODO send a stop walking message?
return;
}
}

private void disableWalking()
{
isWalking.set(false);
footstepsToSendReference.set(null);
continuousStepGenerator.stopWalking();
sendPauseWalkingToController();
}
walkingModeActive = rightVRController.getClickTriggerActionData().bState();

private void sendFootsteps()
{
FootstepDataListMessage footstepsToSend = footstepsToSendReference.getAndSet(null);
if (footstepsToSend != null && isWalking.get())
{
controllerHelper.publishToController(footstepsToSend);
}
double deadband = 0.1;
forwardJoystickValue = DeadbandTools.applyDeadband(deadband, leftVRController.getJoystickActionData().y());
lateralJoystickValue = DeadbandTools.applyDeadband(deadband, -leftVRController.getJoystickActionData().x());
turningJoystickValue = DeadbandTools.applyDeadband(deadband, -rightVRController.getJoystickActionData().x());

if (!isWalking.get())
{
// Only send pause request if we think the command has not been executed yet. This is to be more robust in case packets are dropped.
if (!hasSuccessfullyStoppedWalking.get())
sendPauseWalkingToController();
}
}
double stepTime = swingDuration.get() + transferDuration.get();
forwardVelocity.set((maxStepLength.get() / stepTime) * MathTools.clamp(forwardJoystickValue, 1.0));
lateralVelocity.set((maxStepWidth.get() / stepTime) * MathTools.clamp(lateralJoystickValue, 1.0));
turningVelocity.set(((turnMaxAngleOutward.get() - turnMaxAngleInward.get()) / stepTime) * MathTools.clamp(turningJoystickValue, 1.0));

private void sendPauseWalkingToController()
{
if (currentControllerConnected)
{
PauseWalkingMessage pauseWalkingMessage = new PauseWalkingMessage();
pauseWalkingMessage.setPause(true);
controllerHelper.publishToController(pauseWalkingMessage);
}
ContinuousStepGeneratorInputMessage continuousStepGeneratorInput = new ContinuousStepGeneratorInputMessage();
continuousStepGeneratorInput.setWalk(walkingModeActive);
continuousStepGeneratorInput.setForwardVelocity(forwardVelocity.get());
continuousStepGeneratorInput.setLateralVelocity(lateralVelocity.get());
continuousStepGeneratorInput.setTurnVelocity(turningVelocity.get());
stepGeneratorPublisher.publish(continuousStepGeneratorInput);
}

public void renderImGuiWidgets()
Expand All @@ -315,73 +184,15 @@ public void renderImGuiWidgets()
ImGui.inputDouble(labels.get("Turn step width"), turnStepWidth);
ImGui.inputDouble(labels.get("Turn max angle inward"), turnMaxAngleInward);
ImGui.inputDouble(labels.get("Turn max angle outward"), turnMaxAngleOutward);
if (currentControllerConnected)
{
for (int i = currentController.getMinButtonIndex(); i < currentController.getMaxButtonIndex(); i++)
{
ImGui.text("Button " + i + ": " + currentController.getButton(i));
}
for (int i = 0; i < currentController.getAxisCount(); i++)
{
ImGui.text("Axis " + i + ": " + currentController.getAxis(i));
}
}
}

public void getRenderables(Array<Renderable> renderables, Pool<Renderable> pool)
{
footstepPlanGraphic.getRenderables(renderables, pool);
}

private boolean adjustFootstep(FramePose3DReadOnly stanceFootPose, FramePose2DReadOnly footstepPose, RobotSide footSide, FootstepDataMessage adjustedFootstep)
{
FramePose3D adjustedBasedOnStanceFoot = new FramePose3D();
adjustedBasedOnStanceFoot.getPosition().set(footstepPose.getPosition());
adjustedBasedOnStanceFoot.setZ(stanceFootPose.getZ());
adjustedBasedOnStanceFoot.getOrientation().set(footstepPose.getOrientation());

adjustedFootstep.getLocation().set(adjustedBasedOnStanceFoot.getPosition());
adjustedFootstep.getOrientation().set(adjustedBasedOnStanceFoot.getOrientation());
return true;
// return adjustedBasedOnStanceFoot;
}

private void prepareFootsteps(FootstepDataListMessage footstepDataListMessage)
{
ArrayList<MinimalFootstep> minimalFootsteps = new ArrayList<>();

for (int i = 0; i < footstepDataListMessage.getFootstepDataList().size(); i++)
{
FootstepDataMessage footstepDataMessage = footstepDataListMessage.getFootstepDataList().get(i);
footstepDataMessage.setSwingHeight(swingHeight.get());
Pose3D pose = new Pose3D(footstepDataMessage.getLocation(), footstepDataMessage.getOrientation());
minimalFootsteps.add(new MinimalFootstep(RobotSide.fromByte(footstepDataMessage.getRobotSide()), pose));
}

footstepPlanGraphic.generateMeshesAsync(minimalFootsteps);
footstepsToSendReference.set(new FootstepDataListMessage(footstepDataListMessage));
}

private boolean isStepSnappable(FramePose3DReadOnly touchdownPose, FramePose3DReadOnly stancePose, RobotSide swingSide)
{
return true;
}

private boolean isSafeDistanceFromObstacle(FramePose3DReadOnly touchdownPose, FramePose3DReadOnly stancePose, RobotSide swingSide)
{
return true;
}

private boolean isSafeStepHeight(FramePose3DReadOnly touchdownPose, FramePose3DReadOnly stancePose, RobotSide swingSide)
{
double heightChange = touchdownPose.getZ() - stancePose.getZ();
return heightChange < steppingParameters.getMaxStepUp() && heightChange > -steppingParameters.getMaxStepDown();
// footstepPlanGraphic.getRenderables(renderables, pool);
}

public void destroy()
{
sendPauseWalkingToController();
footstepPlanGraphic.destroy();
executorService.shutdownNow();
// footstepPlanGraphic.destroy();
}
}
Loading