public void addAdditionalOffset(Vector3DReadOnly additionalOffset) { this.additionalOffset.add(additionalOffset); }
public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup createInitialSetupAtNthWall(int nthWall) { double alpha = ((double) nthWall)/7.0; Vector3D startingLocation = morph(firstPlatform, lastPlatform, alpha); startingLocation.add(new Vector3D(-1.0 - 0.1, 1.0 - 0.1, 0.0)); return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(startingLocation, yaw - Math.PI/2.0); }
public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup createInitialSetupAtNthPlatformToesTouching(int nthPlatform) { double alpha = ((double) nthPlatform)/7.0; Vector3D startingLocation = morph(firstPlatform, lastPlatform, alpha); startingLocation.add(new Vector3D(-0.11, -0.16, 0.0)); return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(startingLocation, yaw); }
public static Vector3D morph(Vector3D point1, Vector3D point2, double alpha) { Vector3D framePoint1 = new Vector3D(point1); Vector3D framePoint2 = new Vector3D(point2); framePoint1.scale(1.0 - alpha); framePoint2.scale(alpha); framePoint1.add(framePoint2); return framePoint1; }
public static Vector3D morph(Vector3D point1, Vector3D point2, double alpha) { Vector3D framePoint1 = new Vector3D(point1); Vector3D framePoint2 = new Vector3D(point2); framePoint1.scale(1.0 - alpha); framePoint2.scale(alpha); framePoint1.add(framePoint2); return framePoint1; }
private void generateExpectedLinearAcceleration() { FrameVector3D centerAppliedAccelerationPart = new FrameVector3D(randomLinearAcceleration); FrameVector3D centerCoriolisAccelerationPart = new FrameVector3D(bodyFrame); centerCoriolisAccelerationPart.cross(randomAngularVelocity, randomLinearVelocity); FrameVector3D gravitationalAccelerationPart = new FrameVector3D(fullRobotModel.getWorldFrame()); gravitationalAccelerationPart.setZ(GRAVITY); gravitationalAccelerationPart.changeFrame(bodyFrame); FrameVector3D centripedalAccelerationPart = new FrameVector3D(bodyFrame); centripedalAccelerationPart.cross(randomAngularVelocity, jointToIMUOffset); centripedalAccelerationPart.cross(randomAngularVelocity, centripedalAccelerationPart); FrameVector3D angularAccelerationPart = new FrameVector3D(bodyFrame); angularAccelerationPart.cross(randomAngularAcceleration, jointToIMUOffset); expectedLinearAccelerationOfIMUInIMUFrame.set(centerAppliedAccelerationPart); expectedLinearAccelerationOfIMUInIMUFrame.add(centerCoriolisAccelerationPart); expectedLinearAccelerationOfIMUInIMUFrame.add(gravitationalAccelerationPart); expectedLinearAccelerationOfIMUInIMUFrame.add(centripedalAccelerationPart); expectedLinearAccelerationOfIMUInIMUFrame.add(angularAccelerationPart); transformJointToIMU.transform(expectedLinearAccelerationOfIMUInIMUFrame); }
positionFilter.add(positionError); disturbanceTransform.set(rotationFilter, positionFilter);
private static void setTranslationSettingZAndPreservingXAndY(Point3D highestVertex, RigidBodyTransform transformToReturn) { Vector3D newTranslation = new Vector3D(highestVertex.getX(), highestVertex.getY(), 0.0); transformToReturn.transform(newTranslation); newTranslation.scale(-1.0); newTranslation.add(highestVertex); transformToReturn.setTranslation(newTranslation); }
@Override public void doControl() { for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); ExternalForcePoint efp = externalForcePoints.get(i); proportionalTerm.set(efp.getYoPosition()); proportionalTerm.sub(initialPositions.get(i)); proportionalTerm.scale(-holdPelvisKp.getDoubleValue()); // proportionalTerm.setZ(Math.max(proportionalTerm.getZ(), 0.0)); derivativeTerm.set(efp.getYoVelocity()); derivativeTerm.scale(-holdPelvisKv.getDoubleValue()); pdControlOutput.add(proportionalTerm, derivativeTerm); efp.setForce(pdControlOutput); efp.getYoForce().getYoZ().add(robotWeight / efp_offsetFromRootJoint.size()); efp_positionViz.get(i).update(); } }
edge02.sub(p0); singleCross.cross(edge01, edge02); planeNormal.add(singleCross);
/** * assumes that the GeometricJacobian.compute() method has already been called */ public void compute() { translation.set(point); int angularPartStartRow = 0; int linearPartStartRow = SpatialVector.SIZE / 2; for (int i = 0; i < geometricJacobian.getNumberOfColumns(); i++) { DenseMatrix64F geometricJacobianMatrix = geometricJacobian.getJacobianMatrix(); // angular part: tempVector.set(angularPartStartRow, i, geometricJacobianMatrix); tempJacobianColumn.cross(tempVector, translation); tempVector.set(linearPartStartRow, i, geometricJacobianMatrix); // linear part tempJacobianColumn.add(tempVector); tempJacobianColumn.get(angularPartStartRow, i, jacobianMatrix); } }
private void positionRobotInWorld(HumanoidFloatingRootJointRobot robot) { robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.get(rotation, positionInWorld); positionInWorld.setZ(groundZ + getPelvisToFoot(robot)); positionInWorld.add(offset); robot.setPositionInWorld(positionInWorld); FrameQuaternion frameOrientation = new FrameQuaternion(ReferenceFrame.getWorldFrame(), rotation); double[] yawPitchRoll = new double[3]; frameOrientation.getYawPitchRoll(yawPitchRoll); yawPitchRoll[0] = initialYaw; frameOrientation.setYawPitchRoll(yawPitchRoll); robot.setOrientation(frameOrientation); robot.update(); }
private void positionRobotInWorld(HumanoidFloatingRootJointRobot robot) { robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.get(rotation, positionInWorld); positionInWorld.setZ(groundZ + getPelvisToFoot(robot)); positionInWorld.add(offset); robot.setPositionInWorld(positionInWorld); FrameQuaternion frameOrientation = new FrameQuaternion(ReferenceFrame.getWorldFrame(), rotation); double[] yawPitchRoll = new double[3]; frameOrientation.getYawPitchRoll(yawPitchRoll); yawPitchRoll[0] = initialYaw; frameOrientation.setYawPitchRoll(yawPitchRoll); robot.setOrientation(frameOrientation); robot.update(); }
points[i].getTrackVelocity(tempTrackVelocity); tempVelocity.add(tempTrackVelocity); points[i].setVelocity(tempVelocity);
parentToMidpointVector3d.add(originOneInParent, originTwoInParent); parentToMidpointVector3d.scale(0.5);
unfilteredVector.add(EuclidCoreRandomTools.nextPoint3D(random, 0.0, 0.5));
public void velocityAt(double x, double y, double z, Vector3D normal) { xyPoint.setIncludingFrame(WORLD_FRAME, x, y); double prevZ = previousPlane.getZOnPlane(xyPoint); p1.setIncludingFrame(WORLD_FRAME, x, y, z); p1.changeFrame(planeFrame); xyPoint.setToNaN(planeFrame); xyPoint.setIncludingFrame(p1); double currentZ = plane.getZOnPlane(xyPoint); v1.changeFrame(planeFrame); plane.getNormal(v1); v1.changeFrame(WORLD_FRAME); previousPlane.getNormal(v2); v3.add(v1, v2); v3.scale(-0.5); // v3.scale(currentZ - prevZ); normal.set(v3); // normal.set(0.0,0.0,v3.z); }
public void update(Vector3DReadOnly inputAngularVelocity, Vector3DReadOnly inputLinearAcceleration, Vector3DReadOnly inputMagneticVector) { if (!hasBeenInitialized.getValue()) { initialize(inputLinearAcceleration, inputMagneticVector); return; } boolean success = computeOrientationError((QuaternionReadOnly) estimatedOrientation, inputLinearAcceleration, inputMagneticVector, quaternionUpdate); if (success) { quaternionUpdate.getRotationVector(totalError); yoErrorTerm.set(totalError); integralTerm.scaleAdd(integralGain.getValue() * updateDT, yoErrorTerm, yoIntegralTerm); yoIntegralTerm.set(integralTerm); angularVelocityTerm.scaleAdd(proportionalGain.getValue(), totalError, inputAngularVelocity); angularVelocityTerm.add(integralTerm); } else { yoErrorTerm.setToZero(); angularVelocityTerm.set(inputAngularVelocity); } rotationUpdate.setAndScale(updateDT, angularVelocityTerm); quaternionUpdate.setRotationVector(rotationUpdate); estimatedOrientation.multiply(quaternionUpdate); if (estimatedAngularVelocity != null) estimatedAngularVelocity.add(inputAngularVelocity, integralTerm); }