/** * Sets the point and vector of this plane to zero. After calling this method, this plane becomes * invalid. A new valid point and valid normal will have to be set so this plane is again usable. */ @Override public void setToZero() { point.setToZero(); normal.setToZero(); hasPointBeenSet = false; hasNormalBeenSet = false; }
/** * add a trajectory point to this message */ public void addTrajectoryPoint(double time, double zHeight, double velocity) { tempPoint.setToZero(); tempPoint.setZ(zHeight); tempVector.setToZero(); tempVector.setZ(velocity); euclideanTrajectory.addTrajectoryPoint(time, tempPoint, tempVector); }
observerPosition.setToZero(); observerPosition.applyTransform(transform); spatialVector.applyTransform(transform);
observerPosition.setToZero(); observerPosition.applyTransform(transform); spatialVector.applyTransform(transform);
observerPosition.setToZero(); observerPosition.applyInverseTransform(transform); spatialVector.applyInverseTransform(transform);
pointOfApplication.setToZero();
pointOfApplication.setToZero();
observerPosition.setToZero(); observerPosition.applyInverseTransform(transform); spatialVector.applyInverseTransform(transform);
/** * Transforms this spatial inertia using the given transform. * <p> * See the Word™ document located in the document folder of this project for more * information about the transformation rule for spatial inertia. Also see Duindam, <i>Port-Based * Modeling and Control for Efficient Bipedal Walking Robots</i>, page 40, equation (2.57) from * which the equations here were derived. * </p> */ @Override public void applyTransform(Transform transform) { if (transform instanceof RigidBodyTransform) { applyTransform((RigidBodyTransform) transform); } else { translation.setToZero(); translation.applyTransform(transform); // Let's first apply the rotation onto the CoM and the mass moment of inertia: momentOfInertia.applyTransform(transform); centerOfMassOffset.applyTransform(transform); // Now we can simply apply the translation on the CoM and mass moment of inertia: MecanoTools.translateMomentOfInertia(mass, centerOfMassOffset, false, translation, momentOfInertia); centerOfMassOffset.add(translation); } }
/** * Transforms this spatial inertia by the inverse of the given transform. * <p> * See the Word™ document located in the document folder of this project for more * information about the transformation rule for spatial inertia. Also see Duindam, <i>Port-Based * Modeling and Control for Efficient Bipedal Walking Robots</i>, page 40, equation (2.57) from * which the equations here were derived. * </p> */ @Override public void applyInverseTransform(Transform transform) { if (transform instanceof RigidBodyTransform) { applyInverseTransform((RigidBodyTransform) transform); } else { translation.setToZero(); translation.applyInverseTransform(transform); // Let's first apply the rotation onto the CoM and the mass moment of inertia: momentOfInertia.applyInverseTransform(transform); centerOfMassOffset.applyInverseTransform(transform); // Now we can simply apply the translation on the CoM and mass moment of inertia: MecanoTools.translateMomentOfInertia(mass, centerOfMassOffset, true, translation, momentOfInertia); centerOfMassOffset.add(translation); } }
private void setTimeRangeToZero(MomentumTrajectoryMessage momentumTrajectoryMessage, double t0, double tf) { EuclideanTrajectoryMessage angularMomentumTrajectory = momentumTrajectoryMessage.getAngularMomentumTrajectory(); Object<EuclideanTrajectoryPointMessage> taskspaceTrajectoryPoints = angularMomentumTrajectory.getTaskspaceTrajectoryPoints(); for (int i = 0; i < taskspaceTrajectoryPoints.size(); i++) { EuclideanTrajectoryPointMessage trajectoryPoint = taskspaceTrajectoryPoints.get(i); if(trajectoryPoint.getTime() > tf) { break; } else if(trajectoryPoint.getTime() > t0) { trajectoryPoint.getPosition().setToZero(); trajectoryPoint.getLinearVelocity().setToZero(); } } }
position.setToZero(); for (Vector3D velocity : intermediateVelocities) velocity.setToZero();
intersectionWithPlane.setToZero(); intersectionWithPlane.set(intersectionInPlaneFrame2D); intersectionWithPlane.applyTransform(regionToWorld);
/** * Set this command to the contents of the z height of the pelvis trajectory command Copies the z * points, velocities, and the linear z weight and frame * * @param command the other command */ public void set(PelvisTrajectoryCommand command) { clear(command.getSE3Trajectory().getDataFrame()); euclideanTrajectory.setQueueableCommandVariables(command.getSE3Trajectory()); WeightMatrix3D weightMatrix = command.getSE3Trajectory().getWeightMatrix().getLinearPart(); double zAxisWeight = weightMatrix.getZAxisWeight(); WeightMatrix3D currentWeightMatrix = euclideanTrajectory.getWeightMatrix(); currentWeightMatrix.setZAxisWeight(zAxisWeight); currentWeightMatrix.setWeightFrame(weightMatrix.getWeightFrame()); for (int i = 0; i < command.getSE3Trajectory().getNumberOfTrajectoryPoints(); i++) { FrameSE3TrajectoryPoint trajectoryPoint = command.getSE3Trajectory().getTrajectoryPoint(i); double time = trajectoryPoint.getTime(); double position = trajectoryPoint.getPositionZ(); double velocity = trajectoryPoint.getLinearVelocityZ(); tempPoint.setToZero(); tempPoint.setZ(position); tempVector.setToZero(); tempVector.setZ(velocity); euclideanTrajectory.addTrajectoryPoint(time, tempPoint, tempVector); } enableUserPelvisControl = true; enableUserPelvisControlDuringWalking = command.isEnableUserPelvisControlDuringWalking(); }