Skip to content

Commit 2f732ff

Browse files
LuigiPenco93stephenmcc
LuigiPenco93
and
stephenmcc
authored
Feature/teleop cleanup (#905)
* first commit * added ros2logrecord in teleop ui * cleanup kst params * undo bindings * Moving foot control notification to KSTTools, adding helper method to check if end-effector is foot * remove commented lines --------- Co-authored-by: Stephen McCrory <[email protected]>
1 parent e92df30 commit 2f732ff

File tree

13 files changed

+147
-46
lines changed

13 files changed

+147
-46
lines changed

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/RobotTarget.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,5 +2,5 @@
22

33
public enum RobotTarget
44
{
5-
SCS, GAZEBO, REAL_ROBOT, HEAD_ON_A_STICK
5+
SCS, GAZEBO, REAL_ROBOT, HEAD_ON_A_STICK, DISPLAY
66
}

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

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
import us.ihmc.avatar.networkProcessor.kinematicsStreamingToolboxModule.output.KSTOutputDataReadOnly;
1212
import us.ihmc.commons.MathTools;
1313
import us.ihmc.commons.lists.RecyclingArrayList;
14+
import us.ihmc.commons.thread.Notification;
1415
import us.ihmc.communication.controllerAPI.CommandInputManager;
1516
import us.ihmc.communication.packets.MessageTools;
1617
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
@@ -26,6 +27,7 @@
2627
import us.ihmc.humanoidRobotics.communication.packets.KinematicsToolboxMessageFactory;
2728
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
2829
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
30+
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyReadOnly;
2931
import us.ihmc.mecano.spatial.interfaces.SpatialVectorReadOnly;
3032
import us.ihmc.mecano.tools.MultiBodySystemTools;
3133
import us.ihmc.robotModels.FullHumanoidRobotModel;
@@ -319,6 +321,7 @@ public void onEntry()
319321
ikSolverJointGains.setMaximumFeedbackAndMaximumFeedbackRate(angularRateLimit.getValue(), Double.POSITIVE_INFINITY);
320322

321323
tools.resetUserInvalidInputFlag();
324+
tools.resetFootContactNotification();
322325
KinematicsStreamingToolboxParameters kstParameters = tools.getParameters();
323326
kstParameters.getDefaultSolverConfiguration().setJointVelocityWeight(defaultJointVelocityWeight.getValue());
324327
kstParameters.getDefaultSolverConfiguration().setJointAccelerationWeight(defaultJointAccelerationWeight.getValue());
@@ -503,6 +506,13 @@ else if (latestInput != null)
503506
WeightMatrix6D weightMatrix = rigidBodyInput.getWeightMatrix();
504507
setDefaultWeightsIfNeeded(rigidBodyInput, selectionMatrix, weightMatrix);
505508

509+
// Skip the blending for the foot, if the user has just started to control the foot
510+
RobotSide footSide = getFootSide(rigidBodyInput.getEndEffector(), tools.getDesiredFullRobotModel());
511+
if (footSide != null && tools.getStartFootControlNotification(footSide).poll())
512+
{
513+
rigidBodyControlStartTimeMap.get(rigidBodyInput.getEndEffector()).set(timeInState - streamingBlendingDuration.getValue());
514+
}
515+
506516
// Update time for which each rigid body started being controlled.
507517
YoDouble startTime = rigidBodyControlStartTimeMap.get(rigidBodyInput.getEndEffector());
508518
if (startTime.isNaN())
@@ -820,6 +830,16 @@ private static void blendWeightMatrix(WeightMatrix3D weightMatrix, double curren
820830
}
821831
}
822832

833+
private static RobotSide getFootSide(RigidBodyReadOnly rigidBody, FullHumanoidRobotModel fullHumanoidRobotModel)
834+
{
835+
for (RobotSide robotSide : RobotSide.values)
836+
{
837+
if (rigidBody.equals(fullHumanoidRobotModel.getFoot(robotSide)))
838+
return robotSide;
839+
}
840+
return null;
841+
}
842+
823843
@Override
824844
public void onExit(double timeInState)
825845
{

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

Lines changed: 18 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -17,12 +17,11 @@
1717
import us.ihmc.avatar.networkProcessor.kinematicsStreamingToolboxModule.output.KSTOutputDataBasics;
1818
import us.ihmc.avatar.networkProcessor.kinematicsStreamingToolboxModule.output.KSTOutputDataReadOnly;
1919
import us.ihmc.commonWalkingControlModules.configurations.SteppingParameters;
20-
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories.ContactableBodiesFactory;
2120
import us.ihmc.commons.Conversions;
21+
import us.ihmc.commons.thread.Notification;
2222
import us.ihmc.communication.controllerAPI.CommandInputManager;
2323
import us.ihmc.communication.controllerAPI.StatusMessageOutputManager;
2424
import us.ihmc.euclid.geometry.interfaces.Pose3DBasics;
25-
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
2625
import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics;
2726
import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameQuaternionBasics;
2827
import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
@@ -32,7 +31,6 @@
3231
import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
3332
import us.ihmc.euclid.tools.QuaternionTools;
3433
import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly;
35-
import us.ihmc.euclid.tuple3D.Point3D;
3634
import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry;
3735
import us.ihmc.humanoidRobotics.communication.kinematicsStreamingToolboxAPI.KinematicsStreamingToolboxConfigurationCommand;
3836
import us.ihmc.humanoidRobotics.communication.kinematicsStreamingToolboxAPI.KinematicsStreamingToolboxContactConfigurationCommand;
@@ -42,8 +40,6 @@
4240
import us.ihmc.humanoidRobotics.communication.kinematicsToolboxAPI.KinematicsToolboxRigidBodyCommand;
4341
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
4442
import us.ihmc.humanoidRobotics.communication.packets.KinematicsToolboxOutputConverter;
45-
import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames;
46-
import us.ihmc.idl.IDLSequence;
4743
import us.ihmc.log.LogTools;
4844
import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics;
4945
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
@@ -55,13 +51,11 @@
5551
import us.ihmc.robotModels.FullHumanoidRobotModel;
5652
import us.ihmc.robotModels.FullHumanoidRobotModelFactory;
5753
import us.ihmc.robotModels.FullRobotModelUtils;
58-
import us.ihmc.robotics.contactable.ContactablePlaneBody;
5954
import us.ihmc.robotics.robotSide.RobotSide;
6055
import us.ihmc.robotics.robotSide.SideDependentList;
6156
import us.ihmc.robotics.sensors.ForceSensorDefinition;
6257
import us.ihmc.robotics.sensors.IMUDefinition;
6358
import us.ihmc.sensorProcessing.communication.packets.dataobjects.RobotConfigurationDataFactory;
64-
import us.ihmc.wholeBodyController.RobotContactPointParameters;
6559
import us.ihmc.yoVariables.providers.DoubleProvider;
6660
import us.ihmc.yoVariables.registry.YoRegistry;
6761
import us.ihmc.yoVariables.variable.YoBoolean;
@@ -128,6 +122,7 @@ public class KSTTools
128122
private final KSTInputFilter inputFilter;
129123

130124
private final SideDependentList<Boolean> isFootInSupport = new SideDependentList<>();
125+
private final SideDependentList<Notification> startFootControl = new SideDependentList<>(new Notification(), new Notification());
131126

132127
private WholeBodyTrajectoryMessagePublisher trajectoryMessagePublisher = m ->
133128
{
@@ -289,6 +284,7 @@ public void update()
289284
}
290285
ikController.setInitialRobotConfigurationNamedMap(initialConfigurationMap);
291286
ikController.initialize();
287+
resetFootContactNotification();
292288
}
293289

294290
if (commandInputManager.isNewCommandAvailable(KinematicsStreamingToolboxContactConfigurationCommand.class))
@@ -304,6 +300,8 @@ public void update()
304300
}
305301
isFootInSupport.put(side, isFootInContact);
306302
ikController.setIsFootInSupport(side, isFootInSupport.get(side));
303+
if (!isFootInSupport.get(side))
304+
startFootControl.get(side).set();
307305
}
308306
SteppingParameters steppingParameters = robotModel.getWalkingControllerParameters().getSteppingParameters();
309307
ikController.updateSupportPolygon(isFootInSupport, steppingParameters.getFootLength(), steppingParameters.getFootWidth(), true);
@@ -383,6 +381,14 @@ public void resetUserInvalidInputFlag()
383381
invalidUserInput.set(false);
384382
}
385383

384+
public void resetFootContactNotification()
385+
{
386+
for (RobotSide side : RobotSide.values)
387+
{
388+
startFootControl.get(side).clear();
389+
}
390+
}
391+
386392
public boolean hasUserSubmittedInvalidInput()
387393
{
388394
return invalidUserInput.getValue();
@@ -687,6 +693,11 @@ public boolean isFootInSupport(RobotSide side)
687693
return isFootInSupport.get(side);
688694
}
689695

696+
public Notification getStartFootControlNotification(RobotSide side)
697+
{
698+
return startFootControl.get(side);
699+
}
700+
690701
/**
691702
* [Unsafe] Copies the joint positions from the given array of joints to the given list of messages.
692703
* <p>

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

Lines changed: 1 addition & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -296,19 +296,14 @@ public static KinematicsStreamingToolboxParameters defaultParameters()
296296
}
297297

298298
public void setDefault()
299-
{
300-
setDefault(false);
301-
}
302-
303-
public void setDefault(boolean usingRealtimePlugin)
304299
{
305300
clockType = ClockType.CPU_CLOCK;
306301
toolboxUpdatePeriod = 0.003;
307302
timeThresholdForSleeping = 3.0;
308303

309304
useStreamingPublisher = true;
310305
publishingPeriod = 0.006;
311-
streamIntegrationDuration = usingRealtimePlugin ? 2.0 * publishingPeriod : 0.1;
306+
streamIntegrationDuration = 2.0 * publishingPeriod;
312307

313308
holdChestAngularWeight.set(1.0, 1.0, 0.5);
314309
holdPelvisLinearWeight.set(10.0, 10.0, 20.0);

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/HumanoidKinematicsToolboxController.java

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
import us.ihmc.commonWalkingControlModules.staticEquilibrium.StabilityMarginRegionCalculator;
1717
import us.ihmc.commonWalkingControlModules.staticEquilibrium.WholeBodyContactState;
1818
import us.ihmc.commons.lists.RecyclingArrayList;
19+
import us.ihmc.commons.thread.Notification;
1920
import us.ihmc.communication.controllerAPI.CommandInputManager;
2021
import us.ihmc.communication.controllerAPI.StatusMessageOutputManager;
2122
import us.ihmc.concurrent.ConcurrentCopier;

‎ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KinematicsStreamingToolboxEndToEndTest.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -181,7 +181,7 @@ public void createToolboxController(DRCRobotModel robotModel)
181181
new ControllerNetworkSubscriber(toolboxInputTopic, commandInputManager, toolboxOutputTopic, statusOutputManager, toolboxROS2Node);
182182

183183
KinematicsStreamingToolboxParameters parameters = new KinematicsStreamingToolboxParameters();
184-
parameters.setDefault(false);
184+
parameters.setDefault();
185185
parameters.setToolboxUpdatePeriod(toolboxControllerPeriod);
186186

187187
toolboxController = new KinematicsStreamingToolboxController(commandInputManager,

‎ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -171,6 +171,6 @@ public static ROS2QosProfile getQoS(Class<?> messageClass)
171171
else if (outputMessageClasses.contains(messageClass))
172172
return Objects.requireNonNullElse(outputMessageClassSpecificQoS.get(messageClass), ROS2QosProfile.RELIABLE());
173173
else
174-
return ROS2QosProfile.DEFAULT();
174+
return ROS2QosProfile.BEST_EFFORT();
175175
}
176176
}

‎ihmc-communication/src/main/java/us/ihmc/communication/ros2log/ROS2LogRecord.java

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -50,8 +50,6 @@ else if (requestedState == ROS2LoggerRequestedState.FINISH)
5050
ROS2Topic<?> ros2Topic = topicsToLog.get(i);
5151
topicManagers.add(new RecordTopicManager<>(ros2Topic, ros2Node, timestampProvider));
5252
}
53-
54-
ThreadTools.sleepForever();
5553
}
5654

5755
public void start()
@@ -101,6 +99,15 @@ else if (System.currentTimeMillis() - lastReceivedTimestamp.get() > TIMEOUT_MILL
10199
}
102100
}
103101

102+
public void destroy()
103+
{
104+
if (executorService != null)
105+
{
106+
executorService.shutdownNow();
107+
}
108+
ros2Node.destroy();
109+
}
110+
104111
public static ROS2Topic<ROS2LogMessage> getROS2LogTopic()
105112
{
106113
return ROS2Tools.IHMC_ROOT.withModule(MODULE_NAME).withTypeName(ROS2LogMessage.class);

‎ihmc-graphics/src/libgdx/java/us/ihmc/rdx/vr/RDXVRManager.java

Lines changed: 22 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -277,22 +277,7 @@ public void renderEnableCheckbox()
277277
{
278278
if (vrEnabled.get())
279279
{
280-
RDXBaseUI.pushNotification("Enabling VR...");
281-
282-
ThreadTools.startAThread(() ->
283-
{
284-
ThreadTools.sleep(5000);
285-
286-
if (isVRReady())
287-
{
288-
RDXBaseUI.pushNotification("VR enabled");
289-
}
290-
else
291-
{
292-
RDXBaseUI.pushNotification("Unable to enable VR");
293-
vrEnabled.set(false);
294-
}
295-
}, getClass().getName() + "VREnableMonitor");
280+
enableVR();
296281
}
297282
else
298283
{
@@ -387,6 +372,27 @@ public void getVirtualRenderables(Array<Renderable> renderables, Pool<Renderable
387372
}
388373
}
389374

375+
public void enableVR()
376+
{
377+
vrEnabled.set(true);
378+
RDXBaseUI.pushNotification("Enabling VR...");
379+
380+
ThreadTools.startAThread(() ->
381+
{
382+
ThreadTools.sleep(5000);
383+
384+
if (isVRReady())
385+
{
386+
RDXBaseUI.pushNotification("VR enabled");
387+
}
388+
else
389+
{
390+
RDXBaseUI.pushNotification("Unable to enable VR");
391+
vrEnabled.set(false);
392+
}
393+
}, getClass().getName() + "VREnableMonitor");
394+
}
395+
390396
public RDXVRContext getContext()
391397
{
392398
return context;

‎ihmc-graphics/src/libgdx/java/us/ihmc/rdx/vr/RDXVRTeleporter.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@
2121

2222
public class RDXVRTeleporter
2323
{
24-
private static final Vector3D CHEST_TO_VR_HOME_OFFSET = new Vector3D(0.25, 0.0, 0.45);
24+
private static final Vector3D CHEST_TO_VR_HOME_OFFSET = new Vector3D(0.25, 0.0, 0.55);
2525
private static ReferenceFrame robotVRHomeReferenceFrame;
2626
private boolean preparingToTeleport = false;
2727
private ModelInstance ring;

0 commit comments

Comments
 (0)