@Override protected void setupGroundContactPointFootSwitches(QuadrantDependentList<FootSwitchInterface> footSwitches, double totalRobotWeight) { if (!simulatedRobot.hasValue()) { PrintTools.warn(this, "simulatedRobot is null, creating touchdown based foot switches."); setupTouchdownBasedFootSwitches(footSwitches, totalRobotWeight); return; } for (RobotQuadrant robotQuadrant : RobotQuadrant.values) { ContactablePlaneBody contactablePlaneBody = footContactableBodies.get().get(robotQuadrant); InverseDynamicsJoint parentJoint = contactablePlaneBody.getRigidBody().getParentJoint(); String jointName = parentJoint.getName(); String forceSensorName = contactablePlaneBody.getName() + "ForceSensor"; OneDegreeOfFreedomJoint forceTorqueSensorJoint = simulatedRobot.get().getOneDegreeOfFreedomJoint(jointName); List<GroundContactPoint> contactPoints = forceTorqueSensorJoint.getGroundContactPointGroup().getGroundContactPoints(); RigidBodyTransform transformToParentJoint = contactablePlaneBody.getSoleFrame().getTransformToDesiredFrame(parentJoint.getFrameAfterJoint()); WrenchCalculatorInterface wrenchCaluclator = new GroundContactPointBasedWrenchCalculator(forceSensorName, contactPoints, forceTorqueSensorJoint, transformToParentJoint); FootSwitchInterface footSwitch = new QuadrupedDebugFootSwitch(wrenchCaluclator, contactablePlaneBody, totalRobotWeight, registry); footSwitches.set(robotQuadrant, footSwitch); } }
calculator = new GroundContactPointBasedWrenchCalculator(joint.getName(), contactPoints, joint, new RigidBodyTransform(), new YoVariableRegistry("dummy1")); calculator = new GroundContactPointBasedWrenchCalculator(joint.getName(), contactPoints, joint2, new RigidBodyTransform(), new YoVariableRegistry("dummy2")); point0.setForce(new Vector3D(-1.0, 1.0, 0.0)); point1.setForce(new Vector3D(-1.0, 1.0, 0.0)); transformToJoint.setTranslation(new Vector3D(-1.0, -1.0, 0.0)); calculator = new GroundContactPointBasedWrenchCalculator(joint.getName(), contactPoints, joint3, transformToJoint, new YoVariableRegistry("dummy3"));