Skip to content

Commit

Permalink
Merge branch 'develop' of https://stash.ihmc.us/scm/rob/ihmc-open-rob…
Browse files Browse the repository at this point in the history 
…otics-software into develop
  • Loading branch information
Sean Mason authored and Sean Mason committed Jul 18, 2016
2 parents b91de1b + c7beccf commit 16e580f
Show file tree
Hide file tree
Showing 24 changed files with 335 additions and 81 deletions.
52 changes: 52 additions & 0 deletions ...hmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedFootContactableBodiesFactory.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
package us.ihmc.quadrupedRobotics.estimator.stateEstimator;

import us.ihmc.SdfLoader.SDFFullQuadrupedRobotModel;
import us.ihmc.commonWalkingControlModules.bipedSupportPolygons.ListOfPointsContactablePlaneBody;
import us.ihmc.humanoidRobotics.bipedSupportPolygons.ContactablePlaneBody;
import us.ihmc.quadrupedRobotics.estimator.referenceFrames.CommonQuadrupedReferenceFrames;
import us.ihmc.quadrupedRobotics.model.QuadrupedPhysicalProperties;
import us.ihmc.robotics.robotSide.QuadrantDependentList;
import us.ihmc.robotics.robotSide.RobotQuadrant;
import us.ihmc.robotics.screwTheory.RigidBody;
import us.ihmc.tools.factories.FactoryTools;
import us.ihmc.tools.factories.RequiredFactoryField;

public class QuadrupedFootContactableBodiesFactory
{
private final RequiredFactoryField<SDFFullQuadrupedRobotModel> fullRobotModel = new RequiredFactoryField<>("fullRobotModel");
private final RequiredFactoryField<CommonQuadrupedReferenceFrames> referenceFrames = new RequiredFactoryField<>("referenceFrames");
private final RequiredFactoryField<QuadrupedPhysicalProperties> physicalProperties = new RequiredFactoryField<>("physicalProperties");

public QuadrantDependentList<ContactablePlaneBody> createFootContactableBodies()
{
FactoryTools.checkAllRequiredFactoryFieldsAreSet(this);

QuadrantDependentList<ContactablePlaneBody> footContactableBodies = new QuadrantDependentList<ContactablePlaneBody>();

for (RobotQuadrant robotQuadrant : RobotQuadrant.values)
{
RigidBody foot = fullRobotModel.get().getFoot(robotQuadrant);
ListOfPointsContactablePlaneBody footContactableBody = new ListOfPointsContactablePlaneBody(foot, referenceFrames.get().getFootFrame(robotQuadrant),
physicalProperties.get()
.getFootGroundContactPoints(robotQuadrant));
footContactableBodies.set(robotQuadrant, footContactableBody);
}

return footContactableBodies;
}

public void setFullRobotModel(SDFFullQuadrupedRobotModel fullRobotModel)
{
this.fullRobotModel.set(fullRobotModel);
}

public void setReferenceFrames(CommonQuadrupedReferenceFrames referenceFrames)
{
this.referenceFrames.set(referenceFrames);
}

public void setPhysicalProperties(QuadrupedPhysicalProperties physicalProperties)
{
this.physicalProperties.set(physicalProperties);
}
}
58 changes: 58 additions & 0 deletions ...cs/src/us/ihmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedFootSwitchFactory.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
package us.ihmc.quadrupedRobotics.estimator.stateEstimator;

import us.ihmc.SdfLoader.SDFFullRobotModel;
import us.ihmc.commonWalkingControlModules.sensors.footSwitch.FootSwitchInterface;
import us.ihmc.commonWalkingControlModules.sensors.footSwitch.SettableFootSwitch;
import us.ihmc.humanoidRobotics.bipedSupportPolygons.ContactablePlaneBody;
import us.ihmc.robotics.dataStructures.registry.YoVariableRegistry;
import us.ihmc.robotics.robotSide.QuadrantDependentList;
import us.ihmc.robotics.robotSide.RobotQuadrant;
import us.ihmc.robotics.screwTheory.TotalMassCalculator;
import us.ihmc.tools.factories.FactoryTools;
import us.ihmc.tools.factories.RequiredFactoryField;

public class QuadrupedFootSwitchFactory
{
private final RequiredFactoryField<Double> gravity = new RequiredFactoryField<>("gravity");
private final RequiredFactoryField<YoVariableRegistry> yoVariableRegistry = new RequiredFactoryField<>("yoVariableRegistry");
private final RequiredFactoryField<QuadrantDependentList<ContactablePlaneBody>> footContactableBodies = new RequiredFactoryField<>("footContactableBodies");
private final RequiredFactoryField<SDFFullRobotModel> fullRobotModel = new RequiredFactoryField<>("fullRobotModel");

public QuadrantDependentList<FootSwitchInterface> createFootSwitches()
{
FactoryTools.checkAllRequiredFactoryFieldsAreSet(this);

QuadrantDependentList<FootSwitchInterface> footSwitches = new QuadrantDependentList<FootSwitchInterface>();
double gravityMagnitude = Math.abs(gravity.get());
double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(fullRobotModel.get().getElevator()) * gravityMagnitude;

for (RobotQuadrant robotQuadrant : RobotQuadrant.values)
{
SettableFootSwitch footSwitch = new SettableFootSwitch(footContactableBodies.get().get(robotQuadrant), robotQuadrant, totalRobotWeight, yoVariableRegistry.get());
// KinematicsBasedFootSwitch footSwitch = new KinematicsBasedFootSwitch(namePrefix, footContactableBodies.get(), switchZThreshold, totalRobotWeight, robotQuadrant, yoVariableRegistry.get());
footSwitches.set(robotQuadrant, footSwitch);
}

return footSwitches;
}

public void setGravity(double gravity)
{
this.gravity.set(gravity);
}

public void setYoVariableRegistry(YoVariableRegistry yoVariableRegistry)
{
this.yoVariableRegistry.set(yoVariableRegistry);
}

public void setFootContactableBodies(QuadrantDependentList<ContactablePlaneBody> footContactableBodies)
{
this.footContactableBodies.set(footContactableBodies);
}

public void setFullRobotModel(SDFFullRobotModel fullRobotModel)
{
this.fullRobotModel.set(fullRobotModel);
}
}
132 changes: 80 additions & 52 deletions ...rc/us/ihmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedStateEstimatorFactory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,20 +3,15 @@
import java.util.HashMap;
import java.util.Map;

import us.ihmc.SdfLoader.SDFFullQuadrupedRobotModel;
import us.ihmc.SdfLoader.SDFFullRobotModel;
import us.ihmc.quadrupedRobotics.model.QuadrupedPhysicalProperties;
import us.ihmc.commonWalkingControlModules.bipedSupportPolygons.ListOfPointsContactablePlaneBody;
import us.ihmc.commonWalkingControlModules.sensors.footSwitch.FootSwitchInterface;
import us.ihmc.commonWalkingControlModules.sensors.footSwitch.SettableFootSwitch;
import us.ihmc.humanoidRobotics.bipedSupportPolygons.ContactablePlaneBody;
import us.ihmc.quadrupedRobotics.estimator.referenceFrames.CommonQuadrupedReferenceFrames;
import us.ihmc.humanoidRobotics.model.CenterOfPressureDataHolder;
import us.ihmc.robotics.dataStructures.registry.YoVariableRegistry;
import us.ihmc.robotics.robotSide.QuadrantDependentList;
import us.ihmc.robotics.robotSide.RobotQuadrant;
import us.ihmc.robotics.screwTheory.RigidBody;
import us.ihmc.robotics.screwTheory.SixDoFJoint;
import us.ihmc.robotics.screwTheory.TotalMassCalculator;
import us.ihmc.robotics.sensors.ForceSensorDataHolder;
import us.ihmc.sensorProcessing.model.RobotMotionStatus;
import us.ihmc.sensorProcessing.model.RobotMotionStatusHolder;
Expand All @@ -25,75 +20,108 @@
import us.ihmc.sensorProcessing.stateEstimation.evaluation.FullInverseDynamicsStructure;
import us.ihmc.simulationconstructionset.yoUtilities.graphics.YoGraphicsListRegistry;
import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.DRCKinematicsBasedStateEstimator;
import us.ihmc.tools.factories.FactoryTools;
import us.ihmc.tools.factories.RequiredFactoryField;

public class QuadrupedStateEstimatorFactory
{

private static double switchZThreshold = 0.005;
private final RequiredFactoryField<QuadrupedSensorInformation> sensorInformation = new RequiredFactoryField<>("sensorInformation");
private final RequiredFactoryField<StateEstimatorParameters> stateEstimatorParameters = new RequiredFactoryField<>("stateEstimatorParameters");
private final RequiredFactoryField<SDFFullRobotModel> fullRobotModel = new RequiredFactoryField<>("fullRobotModel");
private final RequiredFactoryField<SensorOutputMapReadOnly> sensorOutputMapReadOnly = new RequiredFactoryField<>("sensorOutputMapReadOnly");
private final RequiredFactoryField<QuadrantDependentList<ContactablePlaneBody>> footContactableBodies = new RequiredFactoryField<>("footContactableBodies");
private final RequiredFactoryField<QuadrantDependentList<FootSwitchInterface>> footSwitches = new RequiredFactoryField<>("footSwitches");
private final RequiredFactoryField<Double> gravity = new RequiredFactoryField<>("gravity");
private final RequiredFactoryField<Double> estimatorDT = new RequiredFactoryField<>("estimatorDT");
private final RequiredFactoryField<YoVariableRegistry> yoVariableRegistry = new RequiredFactoryField<>("yoVariableRegistry");
private final RequiredFactoryField<YoGraphicsListRegistry> yoGraphicsListRegistry = new RequiredFactoryField<>("yoGraphicsListRegistry");

public static DRCKinematicsBasedStateEstimator createStateEstimator(QuadrupedSensorInformation sensorInformation, StateEstimatorParameters stateEstimatorParameters, SDFFullRobotModel fullRobotModel, SensorOutputMapReadOnly sensorOutputMapReadOnly,
QuadrantDependentList<ContactablePlaneBody> feet, QuadrantDependentList<FootSwitchInterface> footSwitches, double gravity, double estimatorDT, YoVariableRegistry registry, YoGraphicsListRegistry yoGraphicsListRegistry)
public DRCKinematicsBasedStateEstimator createStateEstimator()
{
RigidBody elevator = fullRobotModel.getElevator();
SixDoFJoint rootInverseDynamicsJoint = fullRobotModel.getRootJoint();
RigidBody estimationLink = fullRobotModel.getPelvis();
FactoryTools.checkAllRequiredFactoryFieldsAreSet(this);

RigidBody elevator = fullRobotModel.get().getElevator();
SixDoFJoint rootInverseDynamicsJoint = fullRobotModel.get().getRootJoint();
RigidBody estimationLink = fullRobotModel.get().getPelvis();
FullInverseDynamicsStructure inverseDynamicsStructure = new FullInverseDynamicsStructure(elevator, estimationLink, rootInverseDynamicsJoint);

RobotMotionStatusHolder robotMotionStatusFromController = new RobotMotionStatusHolder();
robotMotionStatusFromController.setCurrentRobotMotionStatus(RobotMotionStatus.IN_MOTION);
ForceSensorDataHolder forceSensorDataHolderToUpdate = null;
CenterOfPressureDataHolder centerOfPressureDataHolder = null;

Map<RigidBody, ContactablePlaneBody> feetMap = new HashMap<RigidBody, ContactablePlaneBody>();
Map<RigidBody, FootSwitchInterface> footSwitchMap = new HashMap<RigidBody, FootSwitchInterface>();
for(RobotQuadrant quadrant : RobotQuadrant.values)
for (RobotQuadrant quadrant : RobotQuadrant.values)
{
ContactablePlaneBody contactablePlaneBody = feet.get(quadrant);
ContactablePlaneBody contactablePlaneBody = footContactableBodies.get().get(quadrant);
RigidBody rigidBody = contactablePlaneBody.getRigidBody();
feetMap.put(rigidBody, contactablePlaneBody);
FootSwitchInterface footSwitch = footSwitches.get(quadrant);
FootSwitchInterface footSwitch = footSwitches.get().get(quadrant);
footSwitchMap.put(rigidBody, footSwitch);
}

String[] imuSensorsToUseInStateEstimator = sensorInformation.getImuNames();
double gravityMagnitude = Math.abs(gravity);

// Create the sensor readers and state estimator here:
DRCKinematicsBasedStateEstimator stateEstimator = new DRCKinematicsBasedStateEstimator(inverseDynamicsStructure, stateEstimatorParameters, sensorOutputMapReadOnly, forceSensorDataHolderToUpdate, imuSensorsToUseInStateEstimator,
gravityMagnitude, footSwitchMap, null, robotMotionStatusFromController, feetMap, yoGraphicsListRegistry);

registry.addChild(stateEstimator.getYoVariableRegistry());

String[] imuSensorsToUseInStateEstimator = sensorInformation.get().getImuNames();
double gravityMagnitude = Math.abs(gravity.get());

DRCKinematicsBasedStateEstimator stateEstimator = new DRCKinematicsBasedStateEstimator(inverseDynamicsStructure, stateEstimatorParameters.get(),
sensorOutputMapReadOnly.get(), forceSensorDataHolderToUpdate,
imuSensorsToUseInStateEstimator, gravityMagnitude, footSwitchMap,
centerOfPressureDataHolder , robotMotionStatusFromController, feetMap,
yoGraphicsListRegistry.get());

yoVariableRegistry.get().addChild(stateEstimator.getYoVariableRegistry());

return stateEstimator;
}

public static QuadrantDependentList<FootSwitchInterface> createFootSwitches(QuadrantDependentList<ContactablePlaneBody> quadrupedFeet, double gravity, SDFFullRobotModel fullRobotModel, YoVariableRegistry registry)
public void setSensorInformation(QuadrupedSensorInformation sensorInformation)
{
QuadrantDependentList<FootSwitchInterface> footSwitches = new QuadrantDependentList<FootSwitchInterface>();
double gravityMagnitude = Math.abs(gravity);
double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()) * gravityMagnitude;

for (RobotQuadrant robotQuadrant : RobotQuadrant.values)
{
ContactablePlaneBody contactablePlaneBody = quadrupedFeet.get(robotQuadrant);
String namePrefix = contactablePlaneBody.getName() + "StateEstimator";
SettableFootSwitch footSwitch = new SettableFootSwitch(quadrupedFeet.get(robotQuadrant), robotQuadrant, totalRobotWeight, registry);
// KinematicsBasedFootSwitch footSwitch = new KinematicsBasedFootSwitch(namePrefix, quadrupedFeet, switchZThreshold, totalRobotWeight, robotQuadrant, registry);
footSwitches.set(robotQuadrant, footSwitch);
}
return footSwitches;
this.sensorInformation.set(sensorInformation);
}

public static QuadrantDependentList<ContactablePlaneBody> createFootContactableBodies(SDFFullQuadrupedRobotModel fullRobotModel, CommonQuadrupedReferenceFrames referenceFrames,
QuadrupedPhysicalProperties quadrupedPhysicalProperties)

public void setStateEstimatorParameters(StateEstimatorParameters stateEstimatorParameters)
{
QuadrantDependentList<ContactablePlaneBody> footContactableBodies = new QuadrantDependentList<ContactablePlaneBody>();

for (RobotQuadrant robotQuadrant : RobotQuadrant.values)
{
RigidBody foot = fullRobotModel.getFoot(robotQuadrant);
ListOfPointsContactablePlaneBody footContactableBody = new ListOfPointsContactablePlaneBody(foot, referenceFrames.getFootFrame(robotQuadrant), quadrupedPhysicalProperties.getFootGroundContactPoints(robotQuadrant));
footContactableBodies.set(robotQuadrant, footContactableBody);
}
return footContactableBodies;
this.stateEstimatorParameters.set(stateEstimatorParameters);
}

public void setFullRobotModel(SDFFullRobotModel fullRobotModel)
{
this.fullRobotModel.set(fullRobotModel);
}

public void setSensorOutputMapReadOnly(SensorOutputMapReadOnly sensorOutputMapReadOnly)
{
this.sensorOutputMapReadOnly.set(sensorOutputMapReadOnly);
}

public void setFootContactableBodies(QuadrantDependentList<ContactablePlaneBody> footContactableBodies)
{
this.footContactableBodies.set(footContactableBodies);
}

public void setFootSwitches(QuadrantDependentList<FootSwitchInterface> footSwitches)
{
this.footSwitches.set(footSwitches);
}

public void setGravity(double gravity)
{
this.gravity.set(gravity);
}

public void setEstimatorDT(double estimatorDT)
{
this.estimatorDT.set(estimatorDT);
}

public void setYoVariableRegistry(YoVariableRegistry yoVariableRegistry)
{
this.yoVariableRegistry.set(yoVariableRegistry);
}

public void setYoGraphicsListRegistry(YoGraphicsListRegistry yoGraphicsListRegistry)
{
this.yoGraphicsListRegistry.set(yoGraphicsListRegistry);
}
}
30 changes: 25 additions & 5 deletions ...QuadrupedRobotics/src/us/ihmc/quadrupedRobotics/factories/QuadrupedSimulationFactory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@
import us.ihmc.quadrupedRobotics.controller.positionDevelopment.QuadrupedPositionDevelopmentControllerManager;
import us.ihmc.quadrupedRobotics.estimator.referenceFrames.QuadrupedReferenceFrames;
import us.ihmc.quadrupedRobotics.estimator.sensorProcessing.simulatedSensors.SDFQuadrupedPerfectSimulatedSensor;
import us.ihmc.quadrupedRobotics.estimator.stateEstimator.QuadrupedFootContactableBodiesFactory;
import us.ihmc.quadrupedRobotics.estimator.stateEstimator.QuadrupedFootSwitchFactory;
import us.ihmc.quadrupedRobotics.estimator.stateEstimator.QuadrupedSensorInformation;
import us.ihmc.quadrupedRobotics.estimator.stateEstimator.QuadrupedStateEstimatorFactory;
import us.ihmc.quadrupedRobotics.mechanics.inverseKinematics.QuadrupedInverseKinematicsCalculators;
Expand Down Expand Up @@ -153,21 +155,39 @@ private void createSensorReader()

private void createContactibleFeet()
{
contactableFeet = QuadrupedStateEstimatorFactory.createFootContactableBodies(fullRobotModel.get(), referenceFrames.get(), physicalProperties.get());
QuadrupedFootContactableBodiesFactory footContactableBodiesFactory = new QuadrupedFootContactableBodiesFactory();
footContactableBodiesFactory.setFullRobotModel(fullRobotModel.get());
footContactableBodiesFactory.setPhysicalProperties(physicalProperties.get());
footContactableBodiesFactory.setReferenceFrames(referenceFrames.get());
contactableFeet = footContactableBodiesFactory.createFootContactableBodies();
}

private void createFootSwitches()
{
footSwitches = QuadrupedStateEstimatorFactory.createFootSwitches(contactableFeet, gravity.get(), fullRobotModel.get(), sdfRobot.get().getRobotsYoVariableRegistry());
QuadrupedFootSwitchFactory footSwitchFactory = new QuadrupedFootSwitchFactory();
footSwitchFactory.setFootContactableBodies(contactableFeet);
footSwitchFactory.setFullRobotModel(fullRobotModel.get());
footSwitchFactory.setGravity(gravity.get());
footSwitchFactory.setYoVariableRegistry(sdfRobot.get().getRobotsYoVariableRegistry());
footSwitches = footSwitchFactory.createFootSwitches();
}

private void createStateEstimator()
{
if (useStateEstimator.get())
{
stateEstimator = QuadrupedStateEstimatorFactory
.createStateEstimator(sensorInformation.get(), stateEstimatorParameters.get(), fullRobotModel.get(), sensorReader.getSensorOutputMapReadOnly(), contactableFeet,
footSwitches, gravity.get(), controlDT.get(), sdfRobot.get().getRobotsYoVariableRegistry(), yoGraphicsListRegistry);
QuadrupedStateEstimatorFactory stateEstimatorFactory = new QuadrupedStateEstimatorFactory();
stateEstimatorFactory.setEstimatorDT(controlDT.get());
stateEstimatorFactory.setFootContactableBodies(contactableFeet);
stateEstimatorFactory.setFootSwitches(footSwitches);
stateEstimatorFactory.setFullRobotModel(fullRobotModel.get());
stateEstimatorFactory.setGravity(gravity.get());
stateEstimatorFactory.setSensorInformation(sensorInformation.get());
stateEstimatorFactory.setSensorOutputMapReadOnly(sensorReader.getSensorOutputMapReadOnly());
stateEstimatorFactory.setStateEstimatorParameters(stateEstimatorParameters.get());
stateEstimatorFactory.setYoGraphicsListRegistry(yoGraphicsListRegistry);
stateEstimatorFactory.setYoVariableRegistry(sdfRobot.get().getRobotsYoVariableRegistry());
stateEstimator = stateEstimatorFactory.createStateEstimator();
}
else
{
Expand Down
Loading

0 comments on commit 16e580f

Please sign in to comment.