Skip to content

Commit cda8e81

Browse files
BhavyanshMBhavyanshM
committed
Merge branch 'develop' into feature/perception-api
2 parents 278ccf3 + 8f345d0 commit cda8e81

File tree

68 files changed

+625
-143
lines changed

Some content is hidden

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

68 files changed

+625
-143
lines changed

‎atlas/settings.gradle.kts

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
pluginManagement {
22
plugins {
3-
id("us.ihmc.ihmc-build") version "0.29.1"
3+
id("us.ihmc.ihmc-build") version "0.29.2"
44
}
55
}
66

@@ -10,7 +10,7 @@ buildscript {
1010
mavenLocal()
1111
}
1212
dependencies {
13-
classpath("us.ihmc:ihmc-build:0.29.1")
13+
classpath("us.ihmc:ihmc-build:0.29.2")
1414
}
1515
}
1616

‎example-simulations/settings.gradle.kts

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
pluginManagement {
22
plugins {
3-
id("us.ihmc.ihmc-build") version "0.29.1"
3+
id("us.ihmc.ihmc-build") version "0.29.2"
44
}
55
}
66

@@ -10,7 +10,7 @@ buildscript {
1010
mavenLocal()
1111
}
1212
dependencies {
13-
classpath("us.ihmc:ihmc-build:0.29.1")
13+
classpath("us.ihmc:ihmc-build:0.29.2")
1414
}
1515
}
1616

‎ihmc-avatar-interfaces/settings.gradle.kts

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
pluginManagement {
22
plugins {
3-
id("us.ihmc.ihmc-build") version "0.29.1"
3+
id("us.ihmc.ihmc-build") version "0.29.2"
44
}
55
}
66

@@ -10,7 +10,7 @@ buildscript {
1010
mavenLocal()
1111
}
1212
dependencies {
13-
classpath("us.ihmc:ihmc-build:0.29.1")
13+
classpath("us.ihmc:ihmc-build:0.29.2")
1414
}
1515
}
1616

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

Lines changed: 45 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -345,12 +345,19 @@ public class KinematicsToolboxController extends ToolboxController
345345
* number of iterations before converging.
346346
*/
347347
private final YoBoolean minimizeAngularMomentum = new YoBoolean("minimizeAngularMomentum", registry);
348+
/**
349+
* When {@code true}, the solver will add an objective to minimize the overall linear momentum
350+
* generated. This is not recommended when using this toolbox as an IK solver as it'll increase the
351+
* number of iterations before converging.
352+
*/
353+
private final YoBoolean minimizeLinearMomentum = new YoBoolean("minimizeLinearMomentum", registry);
348354
/**
349355
* The weight to be used for minimizing the angular momentum, around 0.1 seems good for a robot that
350356
* is about 130kg.
351357
*/
352358
private final YoDouble angularMomentumWeight = new YoDouble("angularMomentumWeight", registry);
353-
private final MomentumCommand angularMomentumCommand = new MomentumCommand();
359+
private final YoDouble linearMomentumWeight = new YoDouble("linearMomentumWeight", registry);
360+
private final MomentumCommand momentumCommand = new MomentumCommand();
354361

355362
/**
356363
* @param commandInputManager the message/command barrier used by this controller. Submit
@@ -428,7 +435,6 @@ public KinematicsToolboxController(CommandInputManager commandInputManager,
428435

429436
minimizeAngularMomentum.set(false);
430437
angularMomentumWeight.set(0.125);
431-
angularMomentumCommand.setSelectionMatrixForAngularControl();
432438

433439
enableSelfCollisionAvoidance.set(true);
434440
enableStaticCollisionAvoidance.set(true);
@@ -797,10 +803,16 @@ public void updateInternal()
797803
allFeedbackControlCommands.clear();
798804
allFeedbackControlCommands.addCommandList(feedbackControlCommandBuffer);
799805

800-
if (minimizeAngularMomentum.getValue())
806+
if (minimizeAngularMomentum.getValue() || minimizeLinearMomentum.getValue())
801807
{
802-
angularMomentumCommand.setWeight(angularMomentumWeight.getValue());
803-
inverseKinematicsCommandBuffer.addMomentumCommand().set(angularMomentumCommand);
808+
momentumCommand.setWeight(angularMomentumWeight.getValue(), linearMomentumWeight.getValue());
809+
if (!minimizeAngularMomentum.getValue())
810+
momentumCommand.setSelectionMatrixForLinearControl();
811+
else if (!minimizeLinearMomentum.getValue())
812+
momentumCommand.setSelectionMatrixForAngularControl();
813+
else
814+
momentumCommand.setSelectionMatrixToIdentity();
815+
inverseKinematicsCommandBuffer.addMomentumCommand().set(momentumCommand);
804816
}
805817

806818
/*
@@ -1501,7 +1513,7 @@ public boolean isUserControllingJoint(OneDoFJointBasics joint)
15011513
public boolean isUserControllingCenterOfMass()
15021514
{
15031515
return !userFBCommands.getCenterOfMassFeedbackControlCommandBuffer().isEmpty()
1504-
|| !previousUserFBCommands.getCenterOfMassFeedbackControlCommandBuffer().isEmpty();
1516+
|| !previousUserFBCommands.getCenterOfMassFeedbackControlCommandBuffer().isEmpty();
15051517
}
15061518

15071519
public boolean isUserProvidingSupportPolygon()
@@ -1569,11 +1581,38 @@ public void setPreserveUserCommandHistory(boolean value)
15691581
preserveUserCommandHistory.set(value);
15701582
}
15711583

1584+
public void minimizeMomentum(boolean enableAngular, boolean enableLinear)
1585+
{
1586+
minimizeAngularMomentum(enableAngular);
1587+
minimizeLinearMomentum(enableLinear);
1588+
}
1589+
15721590
public void minimizeAngularMomentum(boolean enable)
15731591
{
15741592
minimizeAngularMomentum.set(enable);
15751593
}
15761594

1595+
public void minimizeLinearMomentum(boolean enable)
1596+
{
1597+
minimizeLinearMomentum.set(enable);
1598+
}
1599+
1600+
public void setMomentumWeight(double angularWeight, double linearWeight)
1601+
{
1602+
setAngularMomentumWeight(angularWeight);
1603+
setLinearMomentumWeight(linearWeight);
1604+
}
1605+
1606+
public void setAngularMomentumWeight(double weight)
1607+
{
1608+
angularMomentumWeight.set(weight);
1609+
}
1610+
1611+
public void setLinearMomentumWeight(double weight)
1612+
{
1613+
linearMomentumWeight.set(weight);
1614+
}
1615+
15771616
public void setEnableSelfCollisionAvoidance(boolean enableSelfCollisionAvoidance)
15781617
{
15791618
this.enableSelfCollisionAvoidance.set(enableSelfCollisionAvoidance);

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

Lines changed: 50 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,7 @@
3838
import us.ihmc.robotics.controllers.pidGains.YoPIDSE3Gains;
3939
import us.ihmc.robotics.controllers.pidGains.implementations.YoPIDGains;
4040
import us.ihmc.robotics.math.filters.AlphaFilteredYoFramePoint;
41+
import us.ihmc.robotics.math.filters.AlphaFilteredYoFramePose3D;
4142
import us.ihmc.robotics.math.filters.AlphaFilteredYoFrameQuaternion;
4243
import us.ihmc.robotics.math.filters.AlphaFilteredYoVariable;
4344
import us.ihmc.robotics.robotSide.RobotSide;
@@ -89,10 +90,13 @@ public class KSTStreamingState implements State
8990
private final RigidBodyBasics chest;
9091
private final SideDependentList<RigidBodyBasics> hands = new SideDependentList<>();
9192
private final SideDependentList<OneDoFJointBasics[]> armJoints = new SideDependentList<>();
93+
private final YoDouble lockPoseFilterBreakFrequency = new YoDouble("lockPoseFilterBreakFrequncy", registry);
9294
private final YoBoolean lockPelvis = new YoBoolean("lockPelvis", registry);
9395
private final YoBoolean lockChest = new YoBoolean("lockChest", registry);
9496
private final YoFramePose3D lockPelvisPose = new YoFramePose3D("lockPelvisPose", worldFrame, registry);
97+
private final AlphaFilteredYoFramePose3D lockPelvisPoseFiltered;
9598
private final YoFramePose3D lockChestPose = new YoFramePose3D("lockChestPose", worldFrame, registry);
99+
private final AlphaFilteredYoFramePose3D lockChestPoseFiltered;
96100

97101
private final YoDouble defaultArmMessageWeight = new YoDouble("defaultArmMessageWeight", registry);
98102
private final YoDouble defaultNeckMessageWeight = new YoDouble("defaultNeckMessageWeight", registry);
@@ -174,6 +178,8 @@ public KSTStreamingState(KSTTools tools)
174178
ikController.getCenterOfMassSafeMargin().set(parameters.getCenterOfMassSafeMargin());
175179
ikController.setPublishingSolutionPeriod(parameters.getPublishingSolutionPeriod());
176180
ikController.getMomentumWeight().set(parameters.getCenterOfMassHoldWeight());
181+
ikController.minimizeMomentum(parameters.isMinimizeAngularMomentum(), parameters.isMinimizeLinearMomentum());
182+
ikController.setMomentumWeight(parameters.getAngularMomentumWeight(), parameters.getLinearMomentumWeight());
177183
desiredFullRobotModel = tools.getDesiredFullRobotModel();
178184
ikCommandInputManager = tools.getIKCommandInputManager();
179185

@@ -186,7 +192,8 @@ public KSTStreamingState(KSTTools tools)
186192
neckJoints = new OneDoFJointBasics[0];
187193
else
188194
neckJoints = MultiBodySystemTools.createOneDoFJointPath(chest, head);
189-
defaultNeckJointMessages = Stream.of(neckJoints).map(joint -> KinematicsToolboxMessageFactory.newOneDoFJointMessage(joint, 10.0, 0.0))
195+
defaultNeckJointMessages = Stream.of(neckJoints)
196+
.map(joint -> KinematicsToolboxMessageFactory.newOneDoFJointMessage(joint, 10.0, 0.0))
190197
.collect(Collectors.toList());
191198
defaultPelvisMessage.setEndEffectorHashCode(pelvis.hashCode());
192199
defaultPelvisMessage.getDesiredOrientationInWorld().setToZero();
@@ -199,7 +206,8 @@ public KSTStreamingState(KSTTools tools)
199206
hands.put(robotSide, hand);
200207
armJoints.put(robotSide, joints);
201208
defaultArmJointMessages.put(robotSide,
202-
Stream.of(joints).map(joint -> KinematicsToolboxMessageFactory.newOneDoFJointMessage(joint, 10.0, 0.0))
209+
Stream.of(joints)
210+
.map(joint -> KinematicsToolboxMessageFactory.newOneDoFJointMessage(joint, 10.0, 0.0))
203211
.collect(Collectors.toList()));
204212
}
205213

@@ -245,6 +253,14 @@ public KSTStreamingState(KSTTools tools)
245253
filteredRobotState = new YoKinematicsToolboxOutputStatus("Filtered", rootJoint, oneDoFJoints, registry);
246254
outputRobotState = new YoKinematicsToolboxOutputStatus("FD", rootJoint, oneDoFJoints, registry);
247255

256+
{ // Filter for locking
257+
DoubleProvider alpha = () -> AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(lockPoseFilterBreakFrequency.getValue(),
258+
toolboxControllerPeriod);
259+
lockPoseFilterBreakFrequency.set(0.25);
260+
lockPelvisPoseFiltered = new AlphaFilteredYoFramePose3D("lockPelvisPoseFiltered", "", lockPelvisPose, alpha, registry);
261+
lockChestPoseFiltered = new AlphaFilteredYoFramePose3D("lockChestPoseFiltered", "", lockChestPose, alpha, registry);
262+
}
263+
248264
YoDouble inputFrequencyAlpha = new YoDouble("inputFrequencyFilter", registry);
249265
inputFrequencyAlpha.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(2.0, toolboxControllerPeriod));
250266
inputFrequency = new AlphaFilteredYoVariable("inputFrequency", registry, inputFrequencyAlpha, rawInputFrequency);
@@ -272,10 +288,16 @@ public KSTStreamingState(KSTTools tools)
272288
rawInputOrientation = new YoFrameQuaternion(namePrefix + "RawOrientation", worldFrame, registry);
273289
rawExtrapolatedInputPosition = new YoFramePoint3D(namePrefix + "RawExtrapolatedPosition", worldFrame, registry);
274290
rawExtrapolatedInputOrientation = new YoFrameQuaternion(namePrefix + "RawExtrapolatedOrientation", worldFrame, registry);
275-
filteredExtrapolatedInputPosition = new AlphaFilteredYoFramePoint(namePrefix
276-
+ "FilteredExtrapolatedPosition", "", registry, inputsAlphaProvider, rawExtrapolatedInputPosition);
277-
filteredExtrapolatedInputOrientation = new AlphaFilteredYoFrameQuaternion(namePrefix
278-
+ "FilteredExtrapolatedOrientation", "", rawExtrapolatedInputOrientation, inputsAlphaProvider, registry);
291+
filteredExtrapolatedInputPosition = new AlphaFilteredYoFramePoint(namePrefix + "FilteredExtrapolatedPosition",
292+
"",
293+
registry,
294+
inputsAlphaProvider,
295+
rawExtrapolatedInputPosition);
296+
filteredExtrapolatedInputOrientation = new AlphaFilteredYoFrameQuaternion(namePrefix + "FilteredExtrapolatedOrientation",
297+
"",
298+
rawExtrapolatedInputOrientation,
299+
inputsAlphaProvider,
300+
registry);
279301
YoFixedFrameSpatialVector rawInputSpatialVelocity = new YoFixedFrameSpatialVector(namePrefix + "RawVelocity", worldFrame, registry);
280302
YoFixedFrameSpatialVector decayingInputSpatialVelocity = new YoFixedFrameSpatialVector(namePrefix + "DecayingVelocity", worldFrame, registry);
281303
rawInputPositionMap.put(rigidBody, rawInputPosition);
@@ -365,15 +387,17 @@ public void onEntry()
365387
if (lockPelvis.getValue())
366388
{
367389
lockPelvisPose.setFromReferenceFrame(controllerFullRobotModel.getPelvis().getBodyFixedFrame());
368-
defaultPelvisMessage.getDesiredPositionInWorld().set(lockPelvisPose.getPosition());
369-
defaultPelvisMessage.getDesiredOrientationInWorld().set(lockPelvisPose.getOrientation());
390+
lockPelvisPoseFiltered.update();
391+
defaultPelvisMessage.getDesiredPositionInWorld().set(lockPelvisPoseFiltered.getPosition());
392+
defaultPelvisMessage.getDesiredOrientationInWorld().set(lockPelvisPoseFiltered.getOrientation());
370393
defaultPelvisMessage.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(true, true, true, worldFrame));
371394
defaultPelvisMessage.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(true, true, true, worldFrame));
372395
MessageTools.packWeightMatrix3DMessage(defaultPelvisMessageLockWeight.getValue(), defaultPelvisMessage.getLinearWeightMatrix());
373396
MessageTools.packWeightMatrix3DMessage(defaultPelvisMessageLockWeight.getValue(), defaultPelvisMessage.getAngularWeightMatrix());
374397
}
375398
else
376399
{
400+
lockPelvisPoseFiltered.reset();
377401
lockPelvisPose.setFromReferenceFrame(pelvis.getBodyFixedFrame());
378402
defaultPelvisMessage.getDesiredPositionInWorld().set(lockPelvisPose.getPosition());
379403
defaultPelvisMessage.getDesiredOrientationInWorld().setToYawOrientation(lockPelvisPose.getYaw());
@@ -388,15 +412,17 @@ public void onEntry()
388412
if (lockChest.getValue())
389413
{
390414
lockChestPose.setFromReferenceFrame(controllerFullRobotModel.getChest().getBodyFixedFrame());
391-
defaultChestMessage.getDesiredPositionInWorld().set(lockChestPose.getPosition());
392-
defaultChestMessage.getDesiredOrientationInWorld().set(lockChestPose.getOrientation());
415+
lockChestPoseFiltered.update();
416+
defaultChestMessage.getDesiredPositionInWorld().set(lockChestPoseFiltered.getPosition());
417+
defaultChestMessage.getDesiredOrientationInWorld().set(lockChestPoseFiltered.getOrientation());
393418
defaultChestMessage.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(true, true, true, worldFrame));
394419
defaultChestMessage.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(true, true, true, worldFrame));
395420
MessageTools.packWeightMatrix3DMessage(defaultChestMessageLockWeight.getValue(), defaultChestMessage.getLinearWeightMatrix());
396421
MessageTools.packWeightMatrix3DMessage(defaultChestMessageLockWeight.getValue(), defaultChestMessage.getAngularWeightMatrix());
397422
}
398423
else
399424
{
425+
lockChestPoseFiltered.reset();
400426
lockChestPose.setFromReferenceFrame(chest.getBodyFixedFrame());
401427
defaultChestMessage.getDesiredPositionInWorld().setToZero();
402428
defaultChestMessage.getDesiredOrientationInWorld().setToYawOrientation(lockChestPose.getYaw());
@@ -502,15 +528,25 @@ public void doAction(double timeInState)
502528
if (lockPelvis.getValue() && !tools.getConfigurationCommand().isPelvisTaskspaceEnabled())
503529
{
504530
lockPelvisPose.setFromReferenceFrame(controllerFullRobotModel.getPelvis().getBodyFixedFrame());
505-
defaultPelvisMessage.getDesiredPositionInWorld().set(lockPelvisPose.getPosition());
506-
defaultPelvisMessage.getDesiredOrientationInWorld().set(lockPelvisPose.getOrientation());
531+
lockPelvisPoseFiltered.update();
532+
defaultPelvisMessage.getDesiredPositionInWorld().set(lockPelvisPoseFiltered.getPosition());
533+
defaultPelvisMessage.getDesiredOrientationInWorld().set(lockPelvisPoseFiltered.getOrientation());
534+
}
535+
else
536+
{
537+
lockPelvisPoseFiltered.reset();
507538
}
508539

509540
if (lockChest.getValue() && !tools.getConfigurationCommand().isChestTaskspaceEnabled())
510541
{
511542
lockChestPose.setFromReferenceFrame(controllerFullRobotModel.getChest().getBodyFixedFrame());
512-
defaultChestMessage.getDesiredPositionInWorld().set(lockChestPose.getPosition());
513-
defaultChestMessage.getDesiredOrientationInWorld().set(lockChestPose.getOrientation());
543+
lockChestPoseFiltered.update();
544+
defaultChestMessage.getDesiredPositionInWorld().set(lockChestPoseFiltered.getPosition());
545+
defaultChestMessage.getDesiredOrientationInWorld().set(lockChestPoseFiltered.getOrientation());
546+
}
547+
else
548+
{
549+
lockChestPoseFiltered.reset();
514550
}
515551

516552
estimateInputsVelocity();

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

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -146,7 +146,6 @@ public KSTTools(CommandInputManager commandInputManager,
146146
ikCommandInputManager.registerConversionHelper(new KinematicsToolboxCommandConverter(desiredFullRobotModel, ikController.getDesiredReferenceFrames()));
147147

148148
ikController.setPreserveUserCommandHistory(false);
149-
ikController.minimizeAngularMomentum(true);
150149

151150
streamingMessageFactory = new KSTStreamingMessageFactory(fullRobotModelFactory);
152151
trajectoryMessageFactory = new KinematicsToolboxOutputConverter(fullRobotModelFactory);

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

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,11 @@ public class KinematicsStreamingToolboxParameters
2727
private double defaultAngularRateLimit;
2828
private double outputJointVelocityScale;
2929

30+
private boolean minimizeAngularMomentum;
31+
private boolean minimizeLinearMomentum;
32+
private double angularMomentumWeight;
33+
private double linearMomentumWeight;
34+
3035
private double defaultStreamingBlendingDuration;
3136

3237
private double inputPoseLPFBreakFrequency;
@@ -65,6 +70,11 @@ public void setDefault()
6570
defaultAngularRateLimit = 10.0;
6671
outputJointVelocityScale = 0.75;
6772

73+
minimizeAngularMomentum = true;
74+
minimizeLinearMomentum = false;
75+
angularMomentumWeight = 0.125;
76+
linearMomentumWeight = 0.0;
77+
6878
defaultStreamingBlendingDuration = 2.0;
6979

7080
inputPoseLPFBreakFrequency = 4.0;
@@ -164,6 +174,26 @@ public double getOutputJointVelocityScale()
164174
return outputJointVelocityScale;
165175
}
166176

177+
public boolean isMinimizeAngularMomentum()
178+
{
179+
return minimizeAngularMomentum;
180+
}
181+
182+
public boolean isMinimizeLinearMomentum()
183+
{
184+
return minimizeLinearMomentum;
185+
}
186+
187+
public double getAngularMomentumWeight()
188+
{
189+
return angularMomentumWeight;
190+
}
191+
192+
public double getLinearMomentumWeight()
193+
{
194+
return linearMomentumWeight;
195+
}
196+
167197
public double getDefaultStreamingBlendingDuration()
168198
{
169199
return defaultStreamingBlendingDuration;
@@ -269,6 +299,26 @@ public void setOutputJointVelocityScale(double outputJointVelocityScale)
269299
this.outputJointVelocityScale = outputJointVelocityScale;
270300
}
271301

302+
public void setMinimizeAngularMomentum(boolean minimizeAngularMomentum)
303+
{
304+
this.minimizeAngularMomentum = minimizeAngularMomentum;
305+
}
306+
307+
public void setMinimizeLinearMomentum(boolean minimizeLinearMomentum)
308+
{
309+
this.minimizeLinearMomentum = minimizeLinearMomentum;
310+
}
311+
312+
public void setAngularMomentumWeight(double angularMomentumWeight)
313+
{
314+
this.angularMomentumWeight = angularMomentumWeight;
315+
}
316+
317+
public void setLinearMomentumWeight(double linearMomentumWeight)
318+
{
319+
this.linearMomentumWeight = linearMomentumWeight;
320+
}
321+
272322
public void setDefaultStreamingBlendingDuration(double defaultStreamingBlendingDuration)
273323
{
274324
this.defaultStreamingBlendingDuration = defaultStreamingBlendingDuration;

‎ihmc-common-walking-control-modules/build.gradle.kts

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ testDependencies {
4646

4747
visualizersDependencies {
4848
api(ihmc.sourceSetProject("main"))
49-
api("us.ihmc:simulation-construction-set:0.23.3")
49+
api("us.ihmc:simulation-construction-set:0.23.4")
5050
api("us.ihmc:scs2-simulation-construction-set:17-0.14.3")
5151

5252
var javaFXVersion = "17.0.2"

0 commit comments

Comments
 (0)