@Override public String toString() { return "ForceSensorDefinition: " + sensorName + " attached to " + rigidBody.getName(); }
@Override public String toString() { return "IMUDefinition: " + name + " attached to " + rigidBody.getName(); } }
@Override public int hashCode() { return 17 + (31 * contactSensorName.hashCode()) + contactingRigidBody.getName().hashCode(); }
public void setCenterOfPressureByName(FramePoint2D centerOfPressure, RigidBodyBasics foot) { RigidBodyBasics footFromNameMap = nameToRigidBodyMap.get(foot.getName()); setCenterOfPressure(centerOfPressure, footFromNameMap); }
public static Quaternion findControllerDesiredOrientation(SimulationConstructionSet scs, RigidBodyBasics chest) { String chestPrefix = chest.getName(); return findQuat4d("FeedbackControllerToolbox", chestPrefix + "DesiredOrientation", scs); }
public static Vector3D findControllerDesiredAngularVelocity(SimulationConstructionSet scs, RigidBodyBasics chest) { String chestPrefix = chest.getName(); return findVector3d("FeedbackControllerToolbox", chestPrefix + "DesiredAngularVelocity", scs); }
public static Vector3D findControlErrorRotationVector(SimulationConstructionSet scs, RigidBodyBasics chest) { String chestPrefix = chest.getName(); String nameSpace = FeedbackControllerToolbox.class.getSimpleName(); String varName = chestPrefix + "ErrorRotationVector"; return findVector3d(nameSpace, varName, scs); }
private static YoBoolean findOrientationControlEnabled(SimulationConstructionSet scs, RigidBodyBasics body) { String bodyName = body.getName(); String namespace = bodyName + "OrientationFBController"; String variable = bodyName + "IsOrientationFBControllerEnabled"; return getBooleanYoVariable(scs, variable, namespace); }
public void setCenterOfPressureByName(Point2D centerOfPressure, RigidBodyBasics foot) { RigidBodyBasics footFromNameMap = nameToRigidBodyMap.get(foot.getName()); setCenterOfPressure(centerOfPressure, footFromNameMap); }
public void getCenterOfPressureByName(FramePoint2D centerOfPressureToPack, RigidBodyBasics foot) { RigidBodyBasics footFromNameMap = nameToRigidBodyMap.get(foot.getName()); getCenterOfPressure(centerOfPressureToPack, footFromNameMap); }
private RigidBodyBasics getRigidBodyHasSameName(FullHumanoidRobotModel fullRobotModel, RigidBodyBasics givenRigidBody) { RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(fullRobotModel.getElevator()); RigidBodyBasics[] allRigidBodies = rootBody.subtreeArray(); for (RigidBodyBasics rigidBody : allRigidBodies) if (givenRigidBody.getName().equals(rigidBody.getName())) return rigidBody; return null; }
public void readControllerDataIntoEstimator() { for (int i = 0; i < estimatorFeet.size(); i++) { RigidBodyBasics foot = estimatorFeet.get(i); estimatorCenterOfPressureDataHolder.setCenterOfPressure(centerOfPressure.get(foot.getName()), foot); } if (robotMotionStatus.get() != null) estimatorRobotMotionStatusHolder.setCurrentRobotMotionStatus(robotMotionStatus.getAndSet(null)); intermediateDesiredJointDataHolder.readIntoEstimator(); }
public static int findControllerNumberOfWaypointsInQueueForHeight(SimulationConstructionSet scs, RigidBodyBasics rigidBody) { String bodyName = rigidBody.getName(); String offsetHeightTrajectoryName = bodyName + MultipleWaypointsPositionTrajectoryGenerator.class.getSimpleName(); String numberOfWaypointsVarName = bodyName + "NumberOfWaypoints"; int numberOfWaypoints = ((YoInteger) scs.getVariable(offsetHeightTrajectoryName, numberOfWaypointsVarName)).getIntegerValue(); return numberOfWaypoints; }
public void writeControllerDataFromController() { for (int i = 0; i < controllerFeet.size(); i++) { RigidBodyBasics foot = controllerFeet.get(i); controllerCenterOfPressureDataHolder.getCenterOfPressureByName(centerOfPressure.get(foot.getName()), foot); } robotMotionStatus.set(controllerRobotMotionStatusHolder.getCurrentRobotMotionStatus()); intermediateDesiredJointDataHolder.copyFromController(); }
public Pose3D getSpatialData(RigidBodyBasics rigidBody) { for (int i = 0; i < spatialData.getRigidBodyNames().size(); i++) { if (spatialData.getRigidBodyNames().get(i).equals(rigidBody.getName())) return getSpatialData(i); } return null; }
public static int findControllerNumberOfWaypointsInGenerator(SimulationConstructionSet scs, RigidBodyBasics body, String prefix) { String bodyName = body.getName(); int numberOfWaypoints = ((YoInteger) scs.getVariable(bodyName + "TaskspaceControlModule", bodyName + prefix + "TaskspaceNumberOfPointsInGenerator")).getIntegerValue(); return numberOfWaypoints; }
public FootVelocitySensor(double dt, RigidBodyBasics foot, ReferenceFrame measurementFrame, String parameterGroup, YoVariableRegistry registry) { super(FilterTools.stringToPrefix(foot.getName()) + "Velocity", dt, foot, measurementFrame, false, null, registry); this.sqrtHz = 1.0 / Math.sqrt(dt); weightFractionForFullTrust = FilterTools.findOrCreate(parameterGroup + "WeightFractionForFullTrust", registry, 0.5); weightFractionForNoTrust = FilterTools.findOrCreate(parameterGroup + "WeightFractionForNoTrust", registry, 0.05); maxVariance = FilterTools.findOrCreate(parameterGroup + "MaxVariance", registry, 1.0E10); minVariance = FilterTools.findOrCreate(parameterGroup + "MinVariance", registry, 0.01); String footName = FilterTools.stringToPrefix(foot.getName()); loadPercentage = new YoDouble(footName + "LoadPercentage", registry); variance = new YoDouble(footName + "Variance", registry); }
private YoDouble getPelvisOrientationErrorVariableName(SimulationConstructionSet scs, FullHumanoidRobotModel fullRobotModel) { String pelvisName = fullRobotModel.getPelvis().getName(); String namePrefix = pelvisName + FeedbackControllerDataReadOnly.Type.ERROR.getName() + FeedbackControllerDataReadOnly.Space.ROTATION_VECTOR.getName(); String varName = YoFrameVariableNameTools.createZName(namePrefix, ""); return (YoDouble) scs.getVariable(FeedbackControllerToolbox.class.getSimpleName(), varName); } }
private static YoFrameQuaternion findOrientationDesired(SimulationConstructionSet scs, RigidBodyBasics body) { String bodyName = body.getName(); String namespace = "FeedbackControllerToolbox"; YoDouble qx = getDoubleYoVariable(scs, bodyName + "DesiredOrientationQx", namespace); YoDouble qy = getDoubleYoVariable(scs, bodyName + "DesiredOrientationQy", namespace); YoDouble qz = getDoubleYoVariable(scs, bodyName + "DesiredOrientationQz", namespace); YoDouble qs = getDoubleYoVariable(scs, bodyName + "DesiredOrientationQs", namespace); return new YoFrameQuaternion(qx, qy, qz, qs, ReferenceFrame.getWorldFrame()); }
private void assertWeightsMatch(double xWeight, double yWeight, double zWeight, RigidBodyBasics rigidBody, SimulationConstructionSet scs) { String prefix = rigidBody.getName() + "TaskspaceOrientation"; YoDouble angularWeightX = (YoDouble) scs.getVariable(prefix + "CurrentWeightX"); YoDouble angularWeightY = (YoDouble) scs.getVariable(prefix + "CurrentWeightY"); YoDouble angularWeightZ = (YoDouble) scs.getVariable(prefix + "CurrentWeightZ"); assertEquals(xWeight, angularWeightX.getDoubleValue(), 1e-8); assertEquals(yWeight, angularWeightY.getDoubleValue(), 1e-8); assertEquals(zWeight, angularWeightZ.getDoubleValue(), 1e-8); }