public void setRigidBody(RigidBody rigidBody) { this.rigidBody = rigidBody; rigidBodyName = rigidBody.getName(); }
public void setEndEffector(RigidBody endEffector) { this.endEffector = endEffector; endEffectorName = endEffector.getName(); }
@Override public String toString() { return "ForceSensorDefinition: " + sensorName + " attached to " + rigidBody.getName(); }
@Override public String getName() { return rigidBody.getName(); }
public void setRigidBody(RigidBody controlledBody) { this.controlledBody = controlledBody; rigidBodyName = controlledBody.getName(); }
public void setContactingRigidBody(RigidBody rigidBody) { this.rigidBody = rigidBody; rigidBodyName = rigidBody.getName(); }
public void setBase(RigidBody base) { this.base = base; baseName = base.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 RigidBodyToIndexMap(RigidBody rootBody) { rigidBodies = ScrewTools.computeSupportAndSubtreeSuccessors(rootBody); rigidBodyNames = new String[rigidBodies.length]; for (int index = 0; index < rigidBodies.length; index++) { RigidBody rigidBody = rigidBodies[index]; rigidBodyNames[index] = rigidBody.getName(); rigidBodyIndexByNameMap.put(rigidBody.getName(), index); rigidBodyByNameMap.put(rigidBody.getName(), rigidBody); } }
public void getCenterOfPressureByName(FramePoint2d centerOfPressureToPack, RigidBody foot) { RigidBody footFromNameMap = nameToRigidBodyMap.get(foot.getName()); getCenterOfPressure(centerOfPressureToPack, footFromNameMap); }
public void setCenterOfPressureByName(Point2d centerOfPressure, RigidBody foot) { RigidBody footFromNameMap = nameToRigidBodyMap.get(foot.getName()); setCenterOfPressure(centerOfPressure, footFromNameMap); }
public void getCenterOfPressureByName(Point2d centerOfPressureToPack, RigidBody foot) { RigidBody footFromNameMap = nameToRigidBodyMap.get(foot.getName()); getCenterOfPressure(centerOfPressureToPack, footFromNameMap); }
public void addBodyForControl(RigidBody bodyForControl) { if (!controlledBodies.contains(bodyForControl)) { controlledBodies.add(bodyForControl); jointChainsForControl.put(bodyForControl, new ArrayList<OneDoFJoint[]>()); } else { PrintTools.warn(this, "Class has already registered " + bodyForControl.getName() + "."); } }
public void addDesiredWrench(RigidBody controlledBody, Wrench desiredWrench) { if (hasBody(controlledBody)) { if (desiredWrenches.get(controlledBody) != null) PrintTools.warn(this, "Class already contains wrench for body " + controlledBody.getName() + ". It is being overwritten."); desiredWrenches.put(controlledBody, desiredWrench); } }
public void addDesiredSelectionMatrix(RigidBody controlledBody, DenseMatrix64F selectionMatrix) { if (hasBody(controlledBody)) { if (desiredSelectionMatrices.get(controlledBody) != null) PrintTools.warn(this, "Class already contains selection matrix for body " + controlledBody.getName() + ". It is being overwritten."); desiredSelectionMatrices.put(controlledBody, selectionMatrix); } }
public void readControllerDataIntoEstimator() { for (int i = 0; i < estimatorFeet.size(); i++) { RigidBody foot = estimatorFeet.get(i); estimatorCenterOfPressureDataHolder.setCenterOfPressure(centerOfPressure.get(foot.getName()), foot); } if (robotMotionStatus.get() != null) estimatorRobotMotionStatusHolder.setCurrentRobotMotionStatus(robotMotionStatus.getAndSet(null)); intermediateDesiredJointDataHolder.readIntoEstimator(); }
public void writeControllerDataFromController() { for (int i = 0; i < controllerFeet.size(); i++) { RigidBody foot = controllerFeet.get(i); controllerCenterOfPressureDataHolder.getCenterOfPressureByName(centerOfPressure.get(foot.getName()), foot); } robotMotionStatus.set(controllerRobotMotionStatusHolder.getCurrentRobotMotionStatus()); intermediateDesiredJointDataHolder.copyFromController(); }
public void createYoVariable(RigidBody controlledBody) { YoWrench yoWrench = new YoWrench(controlledBody.getName() + "_VMCDesiredWrench", controlledBody.getBodyFixedFrame(), ReferenceFrame.getWorldFrame(), registry); yoWrenches.put(controlledBody, yoWrench); }
public void set(RigidBody rigidBody, FramePoint measurementPointInBodyFrame, FrameVector velocityOfMeasurementPointInWorldFrame, boolean isPointVelocityValid) { this.rigidBodyName = rigidBody.getName(); this.bodyFixedReferenceFrameName = measurementPointInBodyFrame.getReferenceFrame().getName(); this.isPointVelocityValid = isPointVelocityValid; measurementPointInBodyFrame.get(this.measurementPointInBodyFrame); velocityOfMeasurementPointInWorldFrame.get(this.velocityOfMeasurementPointInWorldFrame); }