Skip to content

Feature/fst footstep height #729

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

Closed
wants to merge 8 commits into from
Closed

Feature/fst footstep height #729

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
e1a4402
Adding collision avoidance footsteping by adjusting the swing height
Beomyeong Mar 13, 2025
f6af31f
Adding current foot position to compare the footstep height changes
Beomyeong Mar 13, 2025
0a5d3a0
Reset the useSwingCollisionAvoidance to false after sending the footm…
Beomyeong Mar 14, 2025
b6374c5
Adding get orientation method
Beomyeong Mar 17, 2025
f4881b8
Setting the orientation to the footstep
Beomyeong Mar 17, 2025
f213bf8
Changing orientation offset for the tracker
Beomyeong Mar 17, 2025
169ce10
Adding footstep adjustment to move to be stable contact
Beomyeong Mar 20, 2025
70bee17
Changes in 04032025
Beomyeong Apr 3, 2025
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
6 changes: 3 additions & 3 deletions .../main/java/us/ihmc/avatar/networkProcessor/footstepStreamingModule/FSTStreamingState.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -153,7 +153,7 @@ public void doAction(double timeInState)
{
robotStepDuration = latestInput.getRobotStepDuration();
robotElapsedTimeCurrentStep = latestInput.getRobotElapsedTimeCurrentStep();
LogTools.info(robotElapsedTimeCurrentStep);
// LogTools.info(robotElapsedTimeCurrentStep);
if (robotElapsedTimeCurrentStep == 0.0)
latestAdjustmentSent = false;

Expand Down Expand Up @@ -275,8 +275,8 @@ public void doAction(double timeInState)
}
else // Send adjustment
{
LogTools.info("side: {}, landing: {}", robotSwingSide, robotSwingFootIsLanding);
LogTools.info("latestAdjustment: {}", latestAdjustmentSent);
// LogTools.info("side: {}, landing: {}", robotSwingSide, robotSwingFootIsLanding);
// LogTools.info("latestAdjustment: {}", latestAdjustmentSent);

if((robotElapsedTimeCurrentStep < robotStepDuration - footstepMarginTime.getValue())
&& !latestAdjustmentSent
Expand Down
2 changes: 1 addition & 1 deletion ihmc-graphics/src/libgdx/java/us/ihmc/rdx/vr/RDXVRTracker.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ public RDXVRTracker(ReferenceFrame vrPlayAreaYUpZBackFrame, int deviceIndex, RDX
}
default ->
{
trackerYBackZLeftXRightToXForwardZUp = new RigidBodyTransform(new YawPitchRoll(Math.toRadians(-90.0), Math.toRadians(0.0), Math.toRadians(-90.0)),
trackerYBackZLeftXRightToXForwardZUp = new RigidBodyTransform(new YawPitchRoll(Math.toRadians(0.0), Math.toRadians(90.0), Math.toRadians(-90.0)),
new Point3D());
}
}
Expand Down
2 changes: 1 addition & 1 deletion ihmc-graphics/src/libgdx/resources/vr/actions.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,7 @@
],
"default_bindings" : [
{
"binding_url" : "vive_focus3_bindings.json",
"binding_url" : "index_bindings.json",
"controller_type" : "knuckles"
}
],
Expand Down
6 changes: 3 additions & 3 deletions ihmc-graphics/src/libgdx/resources/vr/tracker_roles.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
{
"Chest" : "3B-3AK00133",
"Left Ankle" : "",
"Chest" : "",
"Left Ankle" : "LHR-41A915A6",
"Left Wrist" : "",
"Waist" : "",
"Right Ankle" : "3B-3AK00484",
"Right Ankle" : "LHR-743512BE",
"Right Wrist" : ""
}
245 changes: 240 additions & 5 deletions ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRFootstepPlacement.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,16 +4,21 @@
import com.badlogic.gdx.graphics.g3d.Renderable;
import com.badlogic.gdx.utils.Array;
import com.badlogic.gdx.utils.Pool;
import com.vividsolutions.jts.geom.util.PointExtracter;
import controller_msgs.msg.dds.FootstepDataListMessage;
import controller_msgs.msg.dds.FootstepDataMessage;
import org.lwjgl.openvr.InputDigitalActionData;
import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
import us.ihmc.avatar.ros2.ROS2ControllerHelper;
import us.ihmc.communication.packets.ExecutionMode;
import us.ihmc.euclid.matrix.RotationMatrix;
import us.ihmc.euclid.referenceFrame.FramePose3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
import us.ihmc.euclid.yawPitchRoll.YawPitchRoll;
import us.ihmc.log.LogTools;
import us.ihmc.rdx.tools.LibGDXTools;
import us.ihmc.rdx.tools.RDXModelLoader;
Expand All @@ -28,8 +33,11 @@
import us.ihmc.sensorProcessing.heightMap.HeightMapData;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.UUID;

import static java.lang.Math.abs;

public class RDXVRFootstepPlacement
{
private final static boolean USE_HEIGHTMAP = true;
Expand All @@ -51,6 +59,7 @@ public class RDXVRFootstepPlacement
private int footstepIndex = 0;
private LocomotionParameters locomotionParameters;
private double stepStartTime = -1.0;
private boolean useSwingCollisionAvoidance = false;

public RDXVRFootstepPlacement(RDXVRContext vrContext,
ROS2SyncedRobotModel syncedRobot,
Expand Down Expand Up @@ -169,7 +178,7 @@ public void createNewFootstep(RobotSide side)
footstepBeingExternallyPlaced = new RDXVRFootstep(side, footstepModels.get(side), footstepIndex++);
}

public boolean setFootstepPose(FramePose3DReadOnly pose)
public boolean setFootstepPose(FramePose3DReadOnly pose, Vector3DReadOnly currentPose)
{
if (footstepBeingExternallyPlaced != null)
{
Expand All @@ -181,11 +190,33 @@ public boolean setFootstepPose(FramePose3DReadOnly pose)
FramePose3D adaptedPose = new FramePose3D(pose);
if (!USE_STEPPABLE_REGION_ADAPTATION)
{
adaptedPose.getPosition().set(pose.getTranslationX(),
pose.getTranslationY(),
latestHeightMapData.getHeightAt(pose.getTranslationX(), pose.getTranslationY()));
adaptedPose.getPosition()
.set(pose.getTranslationX(),
pose.getTranslationY(),
latestHeightMapData.getHeightAt(pose.getTranslationX(), pose.getTranslationY()));

RotationMatrix rotationOfSurfaces = new RotationMatrix(latestHeightMapData.getOrientationAt(pose.getTranslationX(), pose.getTranslationY()));

LogTools.info(
"Euler Angle Orientation of the surface on the tracker: " + rotationOfSurfaces.getYaw() + ", " + rotationOfSurfaces.getPitch() + ", "
+ rotationOfSurfaces.getRoll());
if (rotationOfSurfaces.containsNaN())
rotationOfSurfaces.setIdentity();
adaptedPose.getOrientation().set(rotationOfSurfaces);

adjustmentUnstableSteppingPoint(pose.getTranslationX(),
pose.getTranslationY(),
latestHeightMapData.getHeightAt(pose.getTranslationX(), pose.getTranslationY()),
adaptedPose);
}
footstepBeingExternallyPlaced.setPose(adaptedPose);

LogTools.info("Current foot pose: " + currentPose + " new footstep pose : " + footstepBeingExternallyPlaced.getPose().getPosition());
if (abs(currentPose.getZ() - footstepBeingExternallyPlaced.getPose().getZ() ) > 0.1 )
{
LogTools.info("useSwingCollisionAvoidance is turned on");
useSwingCollisionAvoidance = true;
}
}
else
{
Expand Down Expand Up @@ -230,7 +261,20 @@ public void sendStep(boolean activeAdjustment)
footstepDataMessage.setRobotSide(footstepBeingExternallyPlaced.getSide().toByte());
footstepDataMessage.getLocation().set(footstepBeingExternallyPlaced.getPose().getPosition());
footstepDataMessage.getOrientation().set(footstepBeingExternallyPlaced.getPose().getOrientation());
footstepDataMessage.setTrajectoryType(TrajectoryType.DEFAULT.toByte());
if (!useSwingCollisionAvoidance)
{
footstepDataMessage.setTrajectoryType(TrajectoryType.DEFAULT.toByte());
}
else
{
// This 0.02 is temporal value for the safety.
footstepDataMessage.setSwingHeight(footstepBeingExternallyPlaced.getPose().getZ() + 0.05);
// Not sure this is the correct way to set the waypoint proportions
// footstepDataMessage.custom_waypoint_proportions_.set(0, 0.1);
// footstepDataMessage.custom_waypoint_proportions_.set(1, 0.8);
footstepDataMessage.setTrajectoryType(FootstepDataMessage.TRAJECTORY_TYPE_OBSTACLE_CLEARANCE);
useSwingCollisionAvoidance = false;
}

RDXBaseUI.pushNotification("Commanding %d footsteps...".formatted(messageList.getFootstepDataList().size()));
controllerHelper.publishToController(messageList);
Expand Down Expand Up @@ -300,4 +344,195 @@ public void resetTimer()
{
stepStartTime = -1.0;
}

private void adjustmentUnstableSteppingPoint(double xIndex, double yIndex, double zIndex, FramePose3D centerPose)
{
double lengthToToe = 0.2;
double lengthToHeel = 0.03;
double footWidth = 0.1;

Point3D centerOfFoot = new Point3D(xIndex, yIndex, zIndex);
Point3D leftToeCornerFoot = new Point3D(xIndex + lengthToToe,
yIndex + footWidth / 2.0,
latestHeightMapData.getHeightAt(xIndex + lengthToToe, yIndex + footWidth / 2.0));
Point3D rightToeCornerFoot = new Point3D(xIndex + lengthToToe,
yIndex - footWidth / 2.0,
latestHeightMapData.getHeightAt(xIndex + lengthToToe, yIndex - footWidth / 2.0));
Point3D leftHeelCornerFoot = new Point3D(xIndex - lengthToHeel,
yIndex + footWidth / 2.0,
latestHeightMapData.getHeightAt(xIndex - lengthToHeel, yIndex + footWidth / 2.0));
Point3D rightHeelCornerFoot = new Point3D(xIndex - lengthToHeel,
yIndex - footWidth / 2.0,
latestHeightMapData.getHeightAt(xIndex - lengthToHeel, yIndex - footWidth / 2.0));

boolean leftToeIn = true;
boolean rightToeIn = true;
boolean leftHeelIn = true;
boolean rightHeelIn = true;
if (centerOfFoot.getZ() - leftToeCornerFoot.getZ() > 0.05)
leftToeIn = false;
if (centerOfFoot.getZ() - rightToeCornerFoot.getZ() > 0.05)
rightToeIn = false;
if (centerOfFoot.getZ() - leftHeelCornerFoot.getZ() > 0.05)
leftHeelIn = false;
if (centerOfFoot.getZ() - rightHeelCornerFoot.getZ() > 0.05)
rightHeelIn = false;

// checking how many corners are out
int bitMask = (leftToeIn ? 1 : 0) + (rightToeIn ? 1 : 0) + (leftHeelIn ? 1 : 0) + (rightHeelIn ? 1 : 0);
if (bitMask == 1) // only one corner is out
{
if (!leftToeIn)
{
double displacementX = 0.0;
while (true)
{
if ((centerOfFoot.getZ() - latestHeightMapData.getHeightAt(leftToeCornerFoot.getX() + displacementX, leftToeCornerFoot.getY())) <= 0.02)
break;
else
displacementX -= 0.02;
}

double displacementY = 0.0;
while (true)
{
if ((centerOfFoot.getZ() - latestHeightMapData.getHeightAt(leftToeCornerFoot.getX(), leftToeCornerFoot.getY() + displacementY)) <= 0.02)
break;
else
displacementY -= 0.02;
}

centerPose.getPosition().addX(displacementX);

centerPose.getPosition().addY(displacementY);
}
else if (!rightToeIn)
{
double displacementX = 0.0;
while (true)
{
if ((centerOfFoot.getZ() - latestHeightMapData.getHeightAt(leftToeCornerFoot.getX() + displacementX, leftToeCornerFoot.getY())) <= 0.02)
break;
else
displacementX -= 0.02;
}

double displacementY = 0.0;
while (true)
{
if ((centerOfFoot.getZ() - latestHeightMapData.getHeightAt(leftToeCornerFoot.getX(), leftToeCornerFoot.getY() + displacementY)) <= 0.02)
break;
else
displacementY += 0.02;
}

centerPose.getPosition().addX(displacementX);
centerPose.getPosition().addY(displacementY);
}
else if (!leftHeelIn)
{
double displacementX = 0.0;
while (true)
{
if ((centerOfFoot.getZ() - latestHeightMapData.getHeightAt(leftToeCornerFoot.getX() + displacementX, leftToeCornerFoot.getY())) <= 0.02)
break;
else
displacementX += 0.02;
}

double displacementY = 0.0;
while (true)
{
if ((centerOfFoot.getZ() - latestHeightMapData.getHeightAt(leftToeCornerFoot.getX(), leftToeCornerFoot.getY() + displacementY)) <= 0.02)
break;
else
displacementY -= 0.02;
}

centerPose.getPosition().addX(displacementX);
centerPose.getPosition().addY(displacementY);
}
else if (!rightHeelIn)
{
double displacementX = 0.0;
while (true)
{
if ((centerOfFoot.getZ() - latestHeightMapData.getHeightAt(leftToeCornerFoot.getX() + displacementX, leftToeCornerFoot.getY())) <= 0.02)
break;
else
displacementX += 0.02;
}

double displacementY = 0.0;
while (true)
{
if ((centerOfFoot.getZ() - latestHeightMapData.getHeightAt(leftToeCornerFoot.getX(), leftToeCornerFoot.getY() + displacementY)) <= 0.02)
break;
else
displacementY += 0.02;
}

centerPose.getPosition().addX(displacementX);
centerPose.getPosition().addY(displacementY);
}
}
if (bitMask == 2)
{
// check the center of the two vertices
if (!leftHeelIn && !rightHeelIn)
{
double displacement = 0.0;
while (true)
{
if ((centerOfFoot.getZ() - latestHeightMapData.getHeightAt((leftHeelCornerFoot.getX() + rightHeelCornerFoot.getX()) / 2.0 + displacement,
(leftHeelCornerFoot.getY() + rightHeelCornerFoot.getY()) / 2.0)) <= 0.02)
break;
else
displacement += 0.02;
}
centerPose.getPosition().addX(displacement);
}
else if (!leftHeelIn && !leftToeIn)
{
double displacement = 0.0;
while (true)
{
if ((centerOfFoot.getZ() - latestHeightMapData.getHeightAt((leftHeelCornerFoot.getX() + rightHeelCornerFoot.getX()) / 2.0,
(leftHeelCornerFoot.getY() + rightHeelCornerFoot.getY()) / 2.0) + displacement)
<= 0.02)
break;
else
displacement -= 0.02;
}
centerPose.getPosition().addY(displacement);
}
else if (!leftToeIn && !rightToeIn)
{
double displacement = 0.0;
while (true)
{
if ((centerOfFoot.getZ() - latestHeightMapData.getHeightAt((leftHeelCornerFoot.getX() + rightHeelCornerFoot.getX()) / 2.0 + displacement,
(leftHeelCornerFoot.getY() + rightHeelCornerFoot.getY()) / 2.0)) <= 0.02)
break;
else
displacement -= 0.02;
}
centerPose.getPosition().addX(displacement);
}
else if (!rightToeIn && !rightHeelIn)
{
double displacement = 0.0;
while (true)
{
if ((centerOfFoot.getZ() - latestHeightMapData.getHeightAt((leftHeelCornerFoot.getX() + rightHeelCornerFoot.getX()) / 2.0,
(leftHeelCornerFoot.getY() + rightHeelCornerFoot.getY()) / 2.0) + displacement)
<= 0.02)
break;
else
displacement += 0.02;
}
centerPose.getPosition().addY(displacement);
}
}
}
}
10 changes: 8 additions & 2 deletions ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRFootstepStreaming.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,19 +114,25 @@ public void processToolboxOutput()
if (!latestStatus.getAdjustmentFootstep()) // First value estimate for a footstep
{
// Place and send footstep
RigidBodyTransform currentRobotFootTransformInWorld = new RigidBodyTransform(syncedRobot.getReferenceFrames()
.getSoleFrame(side)
.getTransformToWorldFrame());

footstepPlacer.createNewFootstep(side);
footstepPlacer.setFootstepPose(new FramePose3D(ReferenceFrame.getWorldFrame(),
latestStatus.getDesiredFootPosition(),
latestStatus.getDesiredFootOrientation()));
latestStatus.getDesiredFootOrientation()), currentRobotFootTransformInWorld.getTranslation());
// We can't trigger stepping here. We have to notify the KST and stop streaming
readyToStep.clear();
readyToStep.set();
}
else if (latestStatus.getAdjustmentFootstep() && !latestStatus.getLastAdjustment()) // Later values of updated estimate
{
RigidBodyTransform currentRobotFootTransformInWorld = new RigidBodyTransform(syncedRobot.getReferenceFrames().getSoleFrame(side).getTransformToWorldFrame());

if (footstepPlacer.setFootstepPose(new FramePose3D(ReferenceFrame.getWorldFrame(),
latestStatus.getDesiredFootPosition(),
latestStatus.getDesiredFootOrientation())))
latestStatus.getDesiredFootOrientation()), currentRobotFootTransformInWorld.getTranslation()))
{
step(true);
}
Expand Down
Loading