void
TimeDerivative::output(const Task& task,
const DiscreteStateValueVector& discreteState,
const ContinousStateValueVector&,
PortValueList& portValues) const
{
real_type tOld = discreteState[*mOldTStateInfo](0, 0);
real_type dt = task.getTime() - tOld;
real_type dtMin = sqrt(Limits<real_type>::epsilon());
if (dt < dtMin) {
// For times very near at tOld, just use the old derivative
portValues[mOutputPort] = discreteState[*mDerivativeStateInfo];
} else {
// The numerical derivative
Matrix deriv = portValues[mInputPort] - discreteState[*mOldValueStateInfo];
deriv *= 1/dt;
if (dt < 2*dtMin) {
// For times where numerical derivative starts having a value beyond
// roundoff interpolate between the old and the numerical derivative
portValues[mOutputPort]
= interpolate(dt, dtMin, discreteState[*mDerivativeStateInfo],
2*dtMin, deriv);
} else {
// For times where numerical derivative provides valid values use the
// numerical derivative
portValues[mOutputPort] = deriv;
}
}
}
void
SimulationTime::output(const Task& task, const DiscreteStateValueVector&,
const ContinousStateValueVector&,
PortValueList& portValues) const
{
portValues[mOutputPort] = task.getTime();
}
void ProgramMenu::editTask(int id) {
string description, date, time;
int priority, severity;
Task* task = list->getTask(id);
cout << "Edytuj zadanie" << endl << endl;
cout << "Podaj date terminu: (" << task->getDate() << ")";
cin >> date;
task->setDate(date);
cout << "Podaj godzine terminu: (" << task->getTime() << ")";
cin >> time;
task->setTime(time);
cout << "Podaj opis zadania: (" << task->getDescription() << ")";
cin.ignore();
getline(cin, description);
task->setDescription(description);
cout << "Podaj priorytet (od 0 do 5): (" << task->getPriority() << ")";
cin >> priority;
while (priority > 5 || priority < 0) {
cout << "Podaj cyfre od 0 do 5" << endl;
cin >> priority;
}
task->setPriority(priority);
cout << "Podaj waznosc zadania (od 0 do 5): (" << task->getSeverity() << ")";
cin >> severity;
while (severity > 5 || severity < 0) {
cout << "Podaj cyfre od 0 do 5" << endl;
cin >> severity;
}
task->setSeverity(severity);
clear();
this->list->showList();
this->showMenu();
}
void
MobileRootJoint::derivative(const Task& task, const Environment& environment,
const DiscreteStateValueVector&,
const ContinousStateValueVector& continousState,
const ChildLink& childLink,
ContinousStateValueVector& derivatives) const
{
const CoordinateSystem& cs = childLink.getCoordinateSystem();
Vector3 position = continousState[*mPositionStateInfo];
Quaternion orientation = continousState[*mOrientationStateInfo];
Vector6 velocity = continousState[*mVelocityStateInfo];
Vector3 pDot = orientation.backTransform(velocity.getLinear());
// Compute the derivative term originating from the angular velocity.
// Correction term to keep the quaternion normalized.
// That is if |q| < 1 add a little radial component outward,
// if |q| > 1 add a little radial component inward
Quaternion q = orientation;
Vector3 angVel = velocity.getAngular();
Vector4 qderiv = LinAlg::derivative(q, angVel) + 1e1*(normalize(q) - q);
// Now the derivative of the relative velocity, that is take the
// spatial acceleration and subtract the inertial base velocity and the
// derivative of the 'joint matrix'.
Vector6 spatialAcceleration = environment.getAcceleration(task.getTime());
spatialAcceleration = motionTo(position, spatialAcceleration);
Vector3 angularBaseVelocity = environment.getAngularVelocity(task.getTime());
Vector6 pivel(angularMotionTo(position, angularBaseVelocity));
Vector6 Hdot = Vector6(cross(pivel.getAngular(), velocity.getAngular()),
cross(pivel.getAngular(), velocity.getLinear()) +
cross(pivel.getLinear(), velocity.getAngular()));
Vector6 velDeriv = childLink.getInertialAcceleration()
- spatialAcceleration - Hdot;
derivatives[*mPositionStateInfo] = pDot;
derivatives[*mOrientationStateInfo] = qderiv;
derivatives[*mVelocityStateInfo] = cs.rotToLocal(velDeriv);
}
void
TimeDerivative::init(const Task& task, DiscreteStateValueVector& discreteState,
ContinousStateValueVector&,
const PortValueList& portValues) const
{
Size sz = size(portValues[mInputPort]);
discreteState[*mDerivativeStateInfo].resize(sz(0), sz(1));
discreteState[*mDerivativeStateInfo].clear();
discreteState[*mOldValueStateInfo].resize(sz(0), sz(1));
discreteState[*mOldValueStateInfo] = portValues[mInputPort];
discreteState[*mOldTStateInfo].resize(1, 1);
discreteState[*mOldTStateInfo](0, 0) = task.getTime();
}
void
MobileRootJoint::velocity(const Task& task, const Environment& environment,
const ContinousStateValueVector& continousState,
ChildLink& childLink) const
{
Vector3 position = continousState[*mPositionStateInfo];
Quaternion orientation = continousState[*mOrientationStateInfo];
Vector6 velocity = continousState[*mVelocityStateInfo];
childLink.setCoordinateSystem(CoordinateSystem(position, orientation));
velocity = childLink.getCoordinateSystem().rotToReference(velocity);
Vector3 angularBaseVelocity = environment.getAngularVelocity(task.getTime());
Vector6 baseVelocity(angularMotionTo(position, angularBaseVelocity));
childLink.setVelocity(velocity);
childLink.setInertialVelocity(baseVelocity + velocity);
childLink.setForce(Vector6::zeros());
childLink.setInertia(SpatialInertia::zeros());
}
virtual void articulation(const Task& task)
{
// The coordinate system at the hub.
CoordinateSystem hubCoordinateSystem(getLink().getCoordinateSystem());
// Get the ground values in the hub coordinate system.
GroundValues groundValues =
getEnvironment().getGroundPlane(hubCoordinateSystem, task.getTime());
// Transform the plane equation to the local frame.
Plane lp = groundValues.plane;
// Get the intersection length.
real_type distHubGround = fabs(lp.getDist());
Vector3 down = -copysign(1, lp.getDist())*lp.getNormal();
real_type compressLength = mWheelContact->getWheelRadius() - distHubGround;
// Don't bother if we do not intersect the ground.
if (compressLength < 0)
return;
Vector3 contactPoint = distHubGround*down;
// The relative velocity of the ground wrt the contact point
Vector6 relVel = getLink().getLocalVelocity() - groundValues.vel;
// The velocity perpandicular to the plane.
// Positive when the contact spring is compressed,
// negative when decompressed.
real_type compressVel = - lp.scalarProjectToNormal(relVel.getLinear());
// Get a transform from the current frames coordinates into
// wheel coordinates.
// The wheel coordinates x axis is defined by the forward orientation
// of the wheel, the z axis points perpandicular to the ground
// plane downwards.
Vector3 forward = normalize(cross(mWheelContact->getAxis(), down));
Vector3 side = normalize(cross(down, forward));
// Transformed velocity to the ground plane
Vector2 wheelVel(dot(forward, relVel.getLinear()),
dot(side, relVel.getLinear()));
// The wheel rotation speed wrt ground
Vector3 rotVel = relVel.getAngular();
real_type omegaR = dot(rotVel, mWheelContact->getAxis()) * distHubGround;
// Log(Model,Error) << trans(groundValues.vel) << " "
// << trans(wheelVel) << " "
// << omegaR << " "
// << compressLength << " "
// << distHubGround << std::endl;
// Get the plane normal force.
real_type normForce = mWheelContact->computeNormalForce(compressLength,
compressVel,
mPortValueList);
// The normal force cannot get negative here.
normForce = max(static_cast<real_type>(0), normForce);
// Get the friction force.
Vector2 fricForce = mWheelContact->computeFrictionForce(normForce, wheelVel,
omegaR, groundValues.friction, mPortValueList);
// The resulting force is the sum of both.
// The minus sign is because of the direction of the surface normal.
Vector3 force = fricForce(0)*forward + fricForce(1)*side - normForce*down;
// We don't have an angular moment.
getLink().applyBodyForce(contactPoint, force);
}
