Skip to content

Commit 99ba635

Browse files
PotatoPeeler3000LuigiPenco93reesep5ihmc-rosie-windowscalvertdw
authored
Outside demo branch with Alexander on the week before Thanksgiving in Novdember 2025 (#1121)
* alex teleop * ghost robot * fixed a weird constructor in OpenAlexanderRobotModel * tracker roles * disable kst server * Got local logging working and tuned the realsense and height map for real world testing (#1063) * Settup logging to work on both the high and low level process, fixed a couple of bugs in the starting, and naming of the thing * This shouldn't be a hard coded value, neesds to be the cell size from the parameters * Shrink height map to reduce networking lag * Adjust realsense transform, adjust local logging for testing * Tested more of the logging onboard. Update is either the server, or the intraprocess logger, not both. Optimized the IntraprocessYoVariableLogger so now the longest part is compressing the data before it gets written to disk * Fix gradle deploy, remove link in warning prints * Fix VR finger streaming. --------- Co-authored-by: Alexander OCU <rosie@ihmc.org> Co-authored-by: VR-OCU <lpenco@ihmc.org> * Setup behavior tree scene. (#1065) * added a getter (#1069) * Fix sync feedback loop bug with footstep plan action interactables. (#1070) * minor null check in multi thread factory * undo robot model stuff * remove artifcats from merge * remove artifcats * tuned kst * fixes pr * Revert "remove artifcats from merge" This reverts commit ca073ed. * fix with develop * Update OpenAlexanderMomentumOptimizationSettings.java * Bugfix/fix pelvis rotation on delays (#1106) * started working on a fix for pelvis rotationd elays * rolled back on how the time in the phase is found, and started adding a decay factor so the feedforward values dont go to the moon * fixed a bound inclusiveness problem * added some changes to prefer previous reachable regions to yield more… (#1105) * added some changes to prefer previous reachable regions to yield more consistent, smoother results * reverted line change --------- Co-authored-by: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> * remove yaw component from angular velocity filtering of the foot (#1107) Co-authored-by: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> * add a filter for big downward forces on the second threshold (#1108) Co-authored-by: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> * [Feature] CSG Heartbeat (#1104) * Make ROS2HeartbeatMonitor garbage free * Add heartbeats to CSG objects * Added destroy method to CSGROS2CommunicationHelper and cleaned up usage in the panel --------- Co-authored-by: Alexander OCU <rosie@ihmc.org> Co-authored-by: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> * Add null check to ImageSensor * Bugfix/fix capture region and rate limit (#1110) * fixed the reachability constraint return * properly reset the feedback conditions for the ICP feedback * Feature/rdx buttons (#1111) * set up the buttons for whether the steps are adjustable * added a swing height slider to rdx * Remaking the demopose branch for some conflict reason (#1118) Co-authored-by: Alexander OCU <rosie@ihmc.org> * Intraprocess logger safety (#1114) * Fix Ability hand VR rotator streaming. * Catch exception in intraprocess writer compression thread and also check that the directory is usable * Minor * Minor * Revert thing * Switch CountDownLatch to object wait/notify --------- Co-authored-by: Duncan Calvert <dcalvert@ihmc.us> Co-authored-by: ihmc-rosie-windows <rosie@ihmc.org> * Fix the infinite loop with the height map in the controlller, cleaned up the UI for RDX with CSG, and removed a duplicate updater. (#1116) * Fix Ability hand VR rotator streaming. * Fix the bugs with the CSG and the height map, and more * made csg rdx panel buttons for snapping to heightmap and accounting for ground height drift/footstep z error --------- Co-authored-by: Duncan Calvert <dcalvert@ihmc.us> Co-authored-by: ihmc-rosie <rosie@ihmc.us> Co-authored-by: Alexander OCU <rosie@ihmc.org> * upped the icp distance threshold for falling * Feature/height map noise hacking (#1119) * Add option to parameters to tune the minimum acceptable depth values * Tuned value * Update transform and parameter for swing --------- Co-authored-by: ihmc-rosie-windows <rosie@ihmc.org> * Added an option to log a full resolution heartbeat for the depth image (#1120) Co-authored-by: nkitchel <nkitchel@ihmc.org> * Revert "Added an option to log a full resolution heartbeat for the depth image (#1120)" This reverts commit 2cdb895. * Add destroy method to the AvatarStepGeneratorThread --------- Co-authored-by: Luigi Penco <lpenco@ihmc.org> Co-authored-by: Alexander OCU <rpeterson@ihmc.org> Co-authored-by: ihmc-rosie-windows <rosie@ihmc.org> Co-authored-by: Duncan Calvert <dcalvert@ihmc.org> Co-authored-by: Robert Griffin <rgriffin@ihmc.us> Co-authored-by: LuigiPenco <161648875+LuigiPenco93@users.noreply.github.com> Co-authored-by: Tomasz Bialek <103042423+TomaszTB@users.noreply.github.com> Co-authored-by: ihmc-rosie <rosie@ihmc.us> Co-authored-by: ds58 <30220598+ds58@users.noreply.github.com> Co-authored-by: Duncan Calvert <dcalvert@ihmc.us>
1 parent cac28c3 commit 99ba635

File tree

62 files changed

+1145
-384
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

62 files changed

+1145
-384
lines changed

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
import java.util.ArrayList;
44
import java.util.Arrays;
55
import java.util.List;
6+
import java.util.function.Consumer;
67

78
import us.ihmc.avatar.drcRobot.DRCRobotModel;
89
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData;
@@ -11,6 +12,7 @@
1112
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextTools;
1213
import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.LowLevelOneDoFJointDesiredDataHolder;
1314
import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.FootstepValidityIndicator;
15+
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.ContinuousStepGeneratorParametersCommand;
1416
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.HumanoidSteppingPlugin;
1517
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.HumanoidSteppingPluginFactory;
1618
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.StepGeneratorCommandInputManager;
@@ -90,6 +92,8 @@ public AvatarStepGeneratorThread(HumanoidSteppingPluginFactory pluginFactory,
9092
for (FootstepValidityIndicator footstepValidityIndicator : environmentalConstraints.getFootstepValidityIndicators())
9193
pluginFactory.addFootstepValidityIndicator(footstepValidityIndicator);
9294

95+
csgCommandInputManager.addCSGParametersCommandConsumer(csgParameters -> environmentalConstraints.setSnapToHeightMap(csgParameters.getRequestSnapToHeightmap()));
96+
9397
// clear the environment at the beginning of every update
9498
pluginFactory.addUpdatable(environmentalConstraints);
9599
}
@@ -211,4 +215,9 @@ public StepGeneratorCommandInputManager getCsgCommandInputManager()
211215
{
212216
return csgCommandInputManager;
213217
}
218+
219+
public void destroy()
220+
{
221+
csgCommandInputManager.destroy();
222+
}
214223
}

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logging/IntraprocessYoVariableLogger.java‎

Lines changed: 41 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,6 @@
3434
import java.util.ArrayList;
3535
import java.util.Calendar;
3636
import java.util.List;
37-
import java.util.concurrent.CountDownLatch;
3837

3938
public class IntraprocessYoVariableLogger
4039
{
@@ -46,6 +45,7 @@ public class IntraprocessYoVariableLogger
4645
public static final String MODEL_RESOURCE_BUNDLE = "resources.zip";
4746
public static final String INDEX_FILENAME = "robotData.dat";
4847
public static final Path DEFAULT_INCOMING_LOGS_DIRECTORY = Paths.get(System.getProperty("user.home")).resolve(".ihmc").resolve("logs");
48+
public static final Path BACKUP_INCOMING_LOGS_DIRECTORY = Paths.get(System.getProperty("user.home")).resolve(".ihmc").resolve("logs2");
4949

5050
private final List<RegistrySendBufferBuilder> registrySendBufferBuilders;
5151
private final double dt;
@@ -67,7 +67,7 @@ public class IntraprocessYoVariableLogger
6767
private ByteBuffer compressedBuffer;
6868
private ByteBuffer indexBuffer;
6969
private ConcurrentRingBuffer<ByteBuffer> compressionBufferRing;
70-
private CountDownLatch compressionThreadLatch;
70+
private final Object compressionThreadLock = new Object();
7171
private RepeatingTaskThread compressionThread;
7272
private FileChannel dataChannel;
7373
private FileChannel indexChannel;
@@ -97,6 +97,23 @@ public boolean create()
9797
DateFormat dateFormat = new SimpleDateFormat("yyyyMMdd_HHmmssSSS");
9898
String fileTimestamp = dateFormat.format(Calendar.getInstance().getTime());
9999
logFolder = DEFAULT_INCOMING_LOGS_DIRECTORY.resolve(fileTimestamp + logName + INTRAPROCESS_LOG_POSTFIX);
100+
FileTools.ensureDirectoryExists(logFolder);
101+
102+
Path tempFile = logFolder.resolve(".temp");
103+
FileTools.deleteQuietly(tempFile);
104+
if (!tempFile.toFile().createNewFile())
105+
{
106+
// If we failed to create a temp file, try the backup logs dir
107+
logFolder = BACKUP_INCOMING_LOGS_DIRECTORY;
108+
FileTools.ensureDirectoryExists(logFolder);
109+
110+
if (!tempFile.toFile().createNewFile())
111+
{
112+
// We failed to use both the default and backup logs dir
113+
return false;
114+
}
115+
}
116+
FileTools.deleteQuietly(tempFile);
100117

101118
YoVariableHandShakeBuilder handshakeBuilder = new YoVariableHandShakeBuilder("main", dt);
102119
handshakeBuilder.setFrames(ReferenceFrame.getWorldFrame());
@@ -178,10 +195,12 @@ public boolean create()
178195
*/
179196
indexBuffer = ByteBuffer.allocate(2 * Long.BYTES);
180197
compressionBufferRing = new ConcurrentRingBuffer<>(() -> ByteBuffer.allocate(singleTickBufferSize), 2);
181-
compressionThreadLatch = new CountDownLatch(1);
182198
compressionThread = new RepeatingTaskThread("IntraprocessLoggerCompressionThread", () ->
183199
{
184-
compressionThreadLatch.await();
200+
synchronized (compressionThreadLock)
201+
{
202+
compressionThreadLock.wait();
203+
}
185204

186205
if (compressionBufferRing.poll())
187206
{
@@ -201,8 +220,20 @@ public boolean create()
201220
indexBuffer.putLong(dataChannel.position());
202221
indexBuffer.flip();
203222

204-
indexChannel.write(indexBuffer);
205-
dataChannel.write(compressedBuffer);
223+
try
224+
{
225+
long ignored0 = indexChannel.write(indexBuffer);
226+
long ignored1 = dataChannel.write(compressedBuffer);
227+
}
228+
catch (IOException e)
229+
{
230+
LogTools.error("Could not log to: " + logFolder.toAbsolutePath());
231+
LogTools.error("Stopping Intraprocess logger...");
232+
233+
destroy();
234+
235+
compressionThread.blockingKill();
236+
}
206237
}
207238

208239
compressionBufferRing.flush();
@@ -252,7 +283,6 @@ public synchronized void update(long timestamp)
252283
{
253284
if (destroyed)
254285
{
255-
LogTools.error("Logger has already been destroyed.");
256286
return;
257287
}
258288

@@ -294,7 +324,10 @@ public synchronized void update(long timestamp)
294324
compressionBufferRing.commit();
295325
}
296326

297-
compressionThreadLatch.countDown();
327+
synchronized (compressionThreadLock)
328+
{
329+
compressionThreadLock.notify();
330+
}
298331
}
299332

300333
private File createFileInLogFolder(String filename)

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KSTTools.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -267,7 +267,7 @@ public void update()
267267
{
268268
areHandTaskspaceOutputsEnabled.get(robotSide).set(configurationCommand.isHandTaskspaceEnabled(robotSide));
269269
areArmJointspaceOutputsEnabled.get(robotSide).set(configurationCommand.isArmJointspaceEnabled(robotSide));
270-
areLegJointspaceOutputsEnabled.get(robotSide).set(configurationCommand.isLegJointspaceEnabled(robotSide));
270+
areLegJointspaceOutputsEnabled.get(robotSide).set(configurationCommand.isLegJointspaceEnabled(robotSide) && !isFootInSupport.get(robotSide));
271271
}
272272

273273
if (commandInputManager.isNewCommandAvailable(KinematicsStreamingToolboxInitialConfigurationCommand.class))

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/wholeBodyHardwareControl/AvatarMultiThreadingManager.java‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,7 @@ public class AvatarMultiThreadingManager
8888

8989
private ControllerTask controllerTask;
9090
private StepGeneratorTask stepGeneratorTask;
91+
private final AvatarStepGeneratorThread stepGeneratorThread;
9192

9293
private final AvatarAffinityInterface affinity;
9394

@@ -134,6 +135,7 @@ public AvatarMultiThreadingManager(String prefix,
134135
this.hardwareCommunicationInterface = hardwareCommunicationInterface;
135136
this.lowLevelOutputProcessor = lowLevelOutputProcessor;
136137
this.estimatorThread = estimatorThread;
138+
this.stepGeneratorThread = stepGeneratorThread;
137139
this.affinity = affinity;
138140
this.masterThreadDt = masterThreadDt;
139141
this.monotonicTimeProvider = monotonicTimeProvider;
@@ -398,6 +400,8 @@ public void destroy()
398400
((RepeatingTaskThread) masterThread).stopRepeating();
399401
}
400402

403+
stepGeneratorThread.destroy();
404+
401405
ThreadTools.sleep(1000L);
402406

403407
System.out.println("MASTER THREAD SHUTDOWN COMPLETE -- EXITING");

‎ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/ContactStateManager.java‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -114,7 +114,6 @@ public void initialize()
114114

115115
public void initializeForStanding()
116116
{
117-
118117
inSingleSupport.set(false);
119118
inStanding.set(true);
120119

@@ -136,6 +135,7 @@ public void initializeForStanding()
136135
public void initializeForTransfer(double transferDuration, double swingDuration)
137136
{
138137
timeAtStartOfSupportSequence.set(time.getValue());
138+
139139
timeInSupportSequence.set(0.0);
140140
currentStateDuration.set(transferDuration);
141141
totalStateDuration.set(transferDuration + swingDuration);
@@ -159,14 +159,14 @@ public void initializeForSingleSupport(double transferDuration, double swingDura
159159

160160
double stepDuration = transferDuration + swingDuration;
161161
timeAtStartOfSupportSequence.set(time.getValue() - transferDuration);
162+
162163
timeInSupportSequence.set(transferDuration);
163164
currentStateDuration.set(stepDuration);
164165
totalStateDuration.set(stepDuration);
165166
remainingTimeInContactSequence.set(swingDuration);
166167
adjustedRemainingTimeUnderDisturbance.set(swingDuration);
167168
offsetTimeInState.set(transferDuration);
168169

169-
170170
adjustedTimeInSupportSequence.set(transferDuration);
171171
totalTimeAdjustment.set(0.0);
172172

@@ -176,6 +176,7 @@ public void initializeForSingleSupport(double transferDuration, double swingDura
176176
public void initializeForTransferToStanding(double finalTransferDuration )
177177
{
178178
timeAtStartOfSupportSequence.set(time.getValue());
179+
179180
timeInSupportSequence.set(0.0);
180181
currentStateDuration.set(finalTransferDuration);
181182
totalStateDuration.set(finalTransferDuration);

‎ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -417,8 +417,7 @@ private void submitSolverTaskConditions(FrameConvexPolygon2DReadOnly supportPoly
417417

418418
UnrolledInverseFromMinor_DDRM.inv(transformedGains, inverseTransformedGains);
419419

420-
solver.resetCoPFeedbackConditions();
421-
solver.resetFeedbackDirection();
420+
solver.resetFeedbackConditions();
422421
solver.setFeedbackConditions(scaledCoPFeedbackWeight, transformedGains, dynamicsObjectiveWeight.getValue());
423422
solver.setMaxCMPDistanceFromEdge(maxAllowedDistanceCMPSupport.getValue());
424423
solver.setCopSafeDistanceToEdge(safeCoPDistanceToEdge.getValue());
@@ -442,6 +441,11 @@ private void submitSolverTaskConditions(FrameConvexPolygon2DReadOnly supportPoly
442441

443442
solver.setMaximumFeedbackRate(feedbackGains.getFeedbackPartMaxRate(), controlDT);
444443
}
444+
else
445+
{
446+
solver.removeMaximumFeedbackMagnitude();
447+
solver.removeFeedbackRateLimit();
448+
}
445449
ignoreRateLimitThisTick = false;
446450

447451
double feedbackRateWeight = usingHighCoPDamping.getValue() ? highlyDampedFeedbackRateWeight.getValue() : this.feedbackRateWeight.getValue();

‎ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPControllerQPSolver.java‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -294,6 +294,18 @@ public void setMaximumFeedbackRate(double maximumFeedbackRate, double controlDT)
294294
this.controlDT = controlDT;
295295
}
296296

297+
public void removeFeedbackRateLimit()
298+
{
299+
this.maximumFeedbackRate = Double.POSITIVE_INFINITY;
300+
}
301+
302+
public void removeMaximumFeedbackMagnitude()
303+
{
304+
CommonOps_DDRM.setIdentity(maxFeedbackTransform);
305+
maxFeedbackXMagnitude = Double.POSITIVE_INFINITY;
306+
maxFeedbackYMagnitude = Double.POSITIVE_INFINITY;
307+
}
308+
297309
/**
298310
* Zeros all the pertinent scalars, vectors, and matrices for the solver. Should be called at the beginning of every computation tick.
299311
*/
@@ -357,6 +369,15 @@ private void reshape()
357369
solution.reshape(problemSize, 1);
358370
}
359371

372+
public void resetFeedbackConditions()
373+
{
374+
resetCoPFeedbackConditions();
375+
resetCMPFeedbackConditions();
376+
resetFeedbackDirection();
377+
removeFeedbackRateLimit();
378+
removeMaximumFeedbackMagnitude();
379+
}
380+
360381
/**
361382
* Resets the controller conditions on the feedback minimization task, the feedback gains, and the dynamic relaxation minimization task.
362383
* Also sets that the controller is not to attempt to regularize the feedback minimization task.

0 commit comments

Comments
 (0)