How to use

setGroundProfile3D

method

in

us.ihmc.simulationconstructionset.GroundContactModel

  @Override
  public void setGroundProfile3D(GroundProfile3D groundProfile3D)
  {
   baseModel.setGroundProfile3D(groundProfile3D);
  }
}

  @Override
  public void setGroundProfile3D(GroundProfile3D groundProfile3D)
  {
   baseModel.setGroundProfile3D(groundProfile3D);
  }
}

private GroundContactModel createGroundContactModel(Robot robot, GroundProfile3D groundProfile)
{
 double kXY = 5000.0;
 double bXY = 100.0;
 double kZ = 1000.0;
 double bZ = 100.0;
 double alphaStick = 0.7;
 double alphaSlip = 0.5;
 GroundContactModel groundContactModel = new LinearStickSlipGroundContactModel(robot, kXY, bXY, kZ, bZ, alphaSlip, alphaStick,
    robot.getRobotsYoVariableRegistry());
 groundContactModel.setGroundProfile3D(groundProfile);
 return groundContactModel;
}

private GroundContactModel createGroundContactModel(Robot robot, GroundProfile3D groundProfile)
{
 double kXY = 5000.0;
 double bXY = 100.0;
 double kZ = 1000.0;
 double bZ = 100.0;
 double alphaStick = 0.7;
 double alphaSlip = 0.5;
 GroundContactModel groundContactModel = new LinearStickSlipGroundContactModel(robot, kXY, bXY, kZ, bZ, alphaSlip, alphaStick,
    robot.getRobotsYoVariableRegistry());
 groundContactModel.setGroundProfile3D(groundProfile);
 return groundContactModel;
}

  private GroundContactModel createGroundContactModel(Robot robot, GroundProfile3D groundProfile)
  {
   double kXY = 40000.0;
   double bXY = 10.0;
   double kZ = 80.0;
   double bZ = 500.0;
   double alphaStick = 0.7;
   double alphaSlip = 0.5;

   GroundContactModel groundContactModel = new LinearStickSlipGroundContactModel(robot, kXY, bXY, kZ, bZ, alphaSlip, alphaStick,
                         robot.getRobotsYoVariableRegistry());
   groundContactModel.setGroundProfile3D(groundProfile);

   return groundContactModel;
  }
}

public DRCDrillEnvironment()
{
 double forceVectorScale = 1.0 / 500.0;
 combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName());
 combinedTerrainObject.addTerrainObject(setUpGround("Ground", tableCenter, 0.1));
 
 RigidBodyTransform initialDrillTransform = new RigidBodyTransform(new AxisAngle4d(), tableCenter);
 drillRobot = new ContactableCylinderRobot("drill", initialDrillTransform , drillRadius, drillHeight, drillMass, "models/drill.obj");
 final int groundContactGroupIdentifier = 0;
 drillRobot.createAvailableContactPoints(groundContactGroupIdentifier, 30, forceVectorScale, true);
 for (int i = 0; i < 4; i++)
 {
   double angle = i * 2.0 * Math.PI / 4.0;
   double x = 1.5 * drillRadius * Math.cos(angle);
   double y = 1.5 * drillRadius * Math.sin(angle);
   GroundContactPoint groundContactPoint = new GroundContactPoint("gc_drill_" + i, new Vector3d(x, y, 0.0), drillRobot);
   drillRobot.getRootJoints().get(0).addGroundContactPoint(groundContactPoint);
 }
 GroundContactModel groundContactModel = new LinearGroundContactModel(drillRobot, groundContactGroupIdentifier,1422.0, 150.6, 50.0, 600.0, drillRobot.getRobotsYoVariableRegistry());
 groundContactModel.setGroundProfile3D(combinedTerrainObject);
 drillRobot.setGroundContactModel(groundContactModel);
 
 robots.add(drillRobot);
}

public HandDrillManipulationEnvironment()
{
 double forceVectorScale = 1.0 / 500.0;
 combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName());
 combinedTerrainObject.addTerrainObject(setUpGround("Ground", tableCenter, 0.1));
 
 RigidBodyTransform initialDrillTransform = new RigidBodyTransform(new AxisAngle(), tableCenter);
 drillRobot = new ContactableCylinderRobot("drill", initialDrillTransform , drillRadius, drillHeight, drillMass, "models/drill.obj");
 final int groundContactGroupIdentifier = 0;
 drillRobot.createAvailableContactPoints(groundContactGroupIdentifier, 30, forceVectorScale, true);
 for (int i = 0; i < 4; i++)
 {
   double angle = i * 2.0 * Math.PI / 4.0;
   double x = 1.5 * drillRadius * Math.cos(angle);
   double y = 1.5 * drillRadius * Math.sin(angle);
   GroundContactPoint groundContactPoint = new GroundContactPoint("gc_drill_" + i, new Vector3D(x, y, 0.0), drillRobot);
   drillRobot.getRootJoints().get(0).addGroundContactPoint(groundContactPoint);
 }
 GroundContactModel groundContactModel = new LinearGroundContactModel(drillRobot, groundContactGroupIdentifier,1422.0, 150.6, 50.0, 600.0, drillRobot.getRobotsYoVariableRegistry());
 groundContactModel.setGroundProfile3D(combinedTerrainObject);
 drillRobot.setGroundContactModel(groundContactModel);
 
 robots.add(drillRobot);
}

linearGroundModel.setGroundProfile3D(combinedEnvironment);
newBox.setGroundContactModel(linearGroundModel);
newBox.setGravity(-9.81);
linearGroundModel.setGroundProfile3D(combinedEnvironment);
newBox.setGroundContactModel(linearGroundModel);
linearGroundModel.setGroundProfile3D(combinedEnvironment);
newBox.setGroundContactModel(linearGroundModel);
newBox.setGravity(-9.81);

private void createDrill()
{
 Vector3D tableCenter = new Vector3D(-0.851, -6.833, 1.118);
 double drillHeight = 0.3;
 double drillRadius = 0.03;
 double drillMass = 1.5;
 double forceVectorScale = 1.0 / 500.0;
 RigidBodyTransform initialDrillTransform = new RigidBodyTransform(new AxisAngle(), tableCenter);
 ContactableCylinderRobot drillRobot = new ContactableCylinderRobot("drill", initialDrillTransform, drillRadius, drillHeight, drillMass,
                                   "models/drill.obj");
 final int groundContactGroupIdentifier = 0;
 drillRobot.createAvailableContactPoints(groundContactGroupIdentifier, 30, forceVectorScale, true);
 for (int i = 0; i < 4; i++)
 {
   double angle = i * 2.0 * Math.PI / 4.0;
   double x = 1.5 * drillRadius * Math.cos(angle);
   double y = 1.5 * drillRadius * Math.sin(angle);
   GroundContactPoint groundContactPoint = new GroundContactPoint("gc_drill_" + i, new Vector3D(x, y, 0.0), drillRobot);
   drillRobot.getRootJoints().get(0).addGroundContactPoint(groundContactPoint);
 }
 GroundContactModel groundContactModel = new LinearGroundContactModel(drillRobot, groundContactGroupIdentifier, 1422.0, 150.6, 50.0, 600.0,
    drillRobot.getRobotsYoVariableRegistry());
 groundContactModel.setGroundProfile3D(combinedTerrainObject);
 drillRobot.setGroundContactModel(groundContactModel);
 contactableRobots.add(drillRobot);
 combinedTerrainObject.addBox(-0.777, -6.916, -0.947, -6.746, 1.061, 1.08);
}

private void createDrill()
{
 Vector3d tableCenter = new Vector3d(-0.851, -6.833, 1.118);
 double drillHeight = 0.3;
 double drillRadius = 0.03;
 double drillMass = 1.5;
 double forceVectorScale = 1.0 / 500.0;
 RigidBodyTransform initialDrillTransform = new RigidBodyTransform(new AxisAngle4d(), tableCenter);
 ContactableCylinderRobot drillRobot = new ContactableCylinderRobot("drill", initialDrillTransform, drillRadius, drillHeight, drillMass,
    "models/drill.obj");
 final int groundContactGroupIdentifier = 0;
 drillRobot.createAvailableContactPoints(groundContactGroupIdentifier, 30, forceVectorScale, true);
 for (int i = 0; i < 4; i++)
 {
   double angle = i * 2.0 * Math.PI / 4.0;
   double x = 1.5 * drillRadius * Math.cos(angle);
   double y = 1.5 * drillRadius * Math.sin(angle);
   GroundContactPoint groundContactPoint = new GroundContactPoint("gc_drill_" + i, new Vector3d(x, y, 0.0), drillRobot);
   drillRobot.getRootJoints().get(0).addGroundContactPoint(groundContactPoint);
 }
 GroundContactModel groundContactModel = new LinearGroundContactModel(drillRobot, groundContactGroupIdentifier, 1422.0, 150.6, 50.0, 600.0,
    drillRobot.getRobotsYoVariableRegistry());
 groundContactModel.setGroundProfile3D(combinedTerrainObject);
 drillRobot.setGroundContactModel(groundContactModel);
 contactableRobots.add(drillRobot);
 combinedTerrainObject.addBox(-0.777, -6.916, -0.947, -6.746, 1.061, 1.08);
}

public FootstepVisualizer(GroundProfile3D groundProfile, Graphics3DObject linkGraphics, int maxNumberOfFootstepsPerSide, int maxContactPointsPerFoot, String description)
{
 nullRobot = new Robot("FootstepVisualizerRobot");
 
 if (groundProfile != null)
 {
   GroundContactModel gcModel = new LinearGroundContactModel(nullRobot, nullRobot.getRobotsYoVariableRegistry());
   gcModel.setGroundProfile3D(groundProfile);
   nullRobot.setGroundContactModel(gcModel);
 }
 
 scs = new SimulationConstructionSet(nullRobot);
 
 if (linkGraphics != null)
 {
   scs.setGroundVisible(false);
   scs.addStaticLinkGraphics(linkGraphics);
 }
 
 printifdebug("Attaching exit listener");
 scs.setDT(1, 1);
 yoGraphicsListRegistry = new YoGraphicsListRegistry();
 bagOfSingleFootstepVisualizers = new BagOfSingleFootstepVisualizers(maxNumberOfFootstepsPerSide, maxContactPointsPerFoot, registry, yoGraphicsListRegistry);
 addText(scs, yoGraphicsListRegistry, description);
}

public FootstepVisualizer(GroundProfile3D groundProfile, Graphics3DObject linkGraphics, int maxNumberOfFootstepsPerSide, int maxContactPointsPerFoot, String description)
{
 nullRobot = new Robot("FootstepVisualizerRobot");
 
 if (groundProfile != null)
 {
   GroundContactModel gcModel = new LinearGroundContactModel(nullRobot, nullRobot.getRobotsYoVariableRegistry());
   gcModel.setGroundProfile3D(groundProfile);
   nullRobot.setGroundContactModel(gcModel);
 }
 
 scs = new SimulationConstructionSet(nullRobot);
 
 if (linkGraphics != null)
 {
   scs.setGroundVisible(false);
   scs.addStaticLinkGraphics(linkGraphics);
 }
 
 printifdebug("Attaching exit listener");
 scs.setDT(1, 1);
 yoGraphicsListRegistry = new YoGraphicsListRegistry();
 bagOfSingleFootstepVisualizers = new BagOfSingleFootstepVisualizers(maxNumberOfFootstepsPerSide, maxContactPointsPerFoot, registry, yoGraphicsListRegistry);
 addText(scs, yoGraphicsListRegistry, description);
}