@Override public void setAngularVelocityToZero() { angularVelocity.setToZero(); }
@Override public void clear() { load = false; coefficientOfFriction = 0.0; bodyFrameToContactFrame.setToZero(); contactNormalInWorldFrame.setToZero(); }
/** * 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; }
/** {@inheritDoc} */ @Override public void setToZero() { super.setToZero(); radii.setToZero(); }
/** * 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); }
public void clearInitialConstraint() { initialConstraintOrientationError.setToZero(); initialConstraintAngularVelocityError.setToZero(); tempOrientation.set(initialConstraintOrientationError); tempAngularVelocity.set(initialConstraintAngularVelocityError); initialConstraintTrajectory.setTrajectoryTime(0.0); initialConstraintTrajectory.setInitialConditions(tempOrientation, tempAngularVelocity); initialConstraintTrajectory.setFinalConditions(tempOrientation, tempAngularVelocity); initialConstraintTrajectory.initialize(); }
public void clearFinalConstraint() { finalConstraintOrientationError.setToZero(); finalConstraintAngularVelocityError.setToZero(); tempOrientation.set(finalConstraintOrientationError); tempAngularVelocity.set(finalConstraintAngularVelocityError); finalConstraintTrajectory.setTrajectoryTime(0.0); finalConstraintTrajectory.setInitialConditions(tempOrientation, tempAngularVelocity); finalConstraintTrajectory.setFinalConditions(tempOrientation, tempAngularVelocity); finalConstraintTrajectory.initialize(); }
@Override protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack) { RigidBodyTransform transformToRoot = originalFrame.getTransformToRoot(); if (previousRotation.containsNaN() || previousTranslation.containsNaN()) { transformToRoot.get(previousRotation, previousTranslation); angularVelocity.setToZero(); linearVelocity.setToZero(); } else { transformToRoot.get(rotation, translation); rotation.multiplyConjugateOther(previousRotation); rotation.getRotationVector(angularVelocity); angularVelocity.scale(1.0 / updateDT); linearVelocity.sub(translation, previousTranslation); linearVelocity.scale(1.0 / updateDT); transformToRoot.get(previousRotation, previousTranslation); transformToRoot.inverseTransform(angularVelocity); transformToRoot.inverseTransform(linearVelocity); } twistRelativeToParentToPack.setIncludingFrame(this, getParent(), this, angularVelocity, linearVelocity); } }
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(); } } }
point.setTime(time + timeCorruption); point.getPosition().setToZero(); point.getLinearVelocity().setToZero(); point.getLinearVelocity().setToZero(); point.setTime(currentTime + time + dtCorruption); point.getPosition().setToZero(); point.getLinearVelocity().setToZero(); point.getLinearVelocity().setToZero();
angularVelocity.setToZero(); SO3TrajectoryPointMessage trajectoryPoint = so3Trajectory.getTaskspaceTrajectoryPoints().add(); trajectoryPoint.setTime(time);
waypointA.getPosition().addZ(stepHeight + 0.05); waypointA.getOrientation().set(footOrientation); waypointA.getLinearVelocity().setToZero(); waypointA.getAngularVelocity().setToZero(); waypointB.getPosition().addZ(-lastPortion * swingTime * touchdownVelocity + 0.03); waypointB.getOrientation().set(footOrientation); waypointB.getLinearVelocity().setToZero(); waypointB.getAngularVelocity().setToZero(); waypointC.getOrientation().set(footOrientation); waypointC.getLinearVelocity().set(0.0, 0.0, touchdownVelocity); waypointC.getAngularVelocity().setToZero(); touchdown.getOrientation().set(footOrientation); touchdown.getLinearVelocity().set(0.0, 0.0, touchdownVelocity); touchdown.getAngularVelocity().setToZero();
/** * 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(); }