//! Modify item at position \a index. If \a fromCurrentItem is true, the
//! item is copied from _currentItem, otherwise the item property corresponding
//! to the property field \a field is modified.
//! Return true if item has been modified.
bool ColoredMarkerListContainer::modifyItem(MLssize_t index, Field *field, bool fromCurrentItem)
{
ML_TRACE_IN("bool ColoredMarkerListContainer::modifyItem(MLssize_t index, Field *field, bool fromCurrentItem)");
ML_TRY{
bool modified = true;
ColoredMarker *marker=NULL;
// Valid list and item?
if (_listPtr && (index >= 0) && (index < mlrange_cast<MLssize_t>(_listPtr->getSize())) )
{
// Modify from property field?
if (!fromCurrentItem)
{
// Pointer to list item
marker = &(*_listPtr)[mlrange_cast<size_t>(index)];
// Position fields
if ((field == _fldPos3D) || (field == _fldPosC) || (field == _fldPosT) || (field == _fldPosU))
{
marker->pos = Vector6(_fldPos3D->getVector3Value(),0,0,0);
marker->c() = _fldPosC->getFloatValue();
marker->t() = _fldPosT->getFloatValue();
marker->u() = _fldPosU->getFloatValue();
}
else if (field == _fldColor || (field == _fldAlpha))
{
// ImageVector field
marker->rgba() = Vector4(_fldColor->getVector3Value(),_fldAlpha->getFloatValue());
}
else if (field == _fldType)
{
// Type field
marker->type = _fldType->getIntValue();
}
else
{
// Not an ColoredMarker-specific field, pass call to base class
modified = ListContainerTemplate<ColoredMarkerList>::modifyItem(index, field, fromCurrentItem);
}
}
else
{
// Modify from _currentItem, handled in base class
modified = ListContainerTemplate<ColoredMarkerList>::modifyItem(index, field, fromCurrentItem);
}
} else
{
modified = false;
}
return modified;
}
ML_CATCH_RETHROW;
}
void
Contact::output(const TaskInfo& taskInfo)
{
if (nonZeroIntersection(taskInfo.getSampleTimeSet(),
SampleTime::PerTimestep)) {
Log(Model, Debug) << "Contact::output(): \"" << getName()
<< "\" computing ground plane below" << endl;
getGround(taskInfo.getTime());
}
// Transform the plane equation to the local frame.
Plane lp = mMountFrame->planeFromRef(mGroundVal.plane);
// Get the intersection length.
real_type compressLength = lp.getDist(Vector3::zeros());
// Don't bother if we do not intersect the ground.
if (compressLength < 0) {
setForce(Vector6::zeros());
return;
}
// The velocity of the ground patch in the current frame.
Vector6 groundVel(mMountFrame->rotFromRef(mGroundVal.vel.getAngular()),
mMountFrame->rotFromRef(mGroundVal.vel.getLinear()));
groundVel -= mMountFrame->getRefVel();
// Now get the relative velocity of the ground wrt the contact point
Vector3 relVel = - groundVel.getLinear();
// The velocity perpandicular to the plane.
// Positive when the contact spring is compressed,
// negative when decompressed.
real_type compressVel = - lp.scalarProjectToNormal(relVel);
// The in plane velocity.
Vector3 sVel = lp.projectToPlane(relVel);
// Get the plane normal force.
real_type normForce = computeNormalForce(compressLength, compressVel);
// The normal force cannot get negative here.
normForce = max(static_cast<real_type>(0), normForce);
// Get the friction force.
Vector3 fricForce = computeFrictionForce(normForce, sVel, lp.getNormal(),
mGroundVal.friction);
// The resulting force is the sum of both.
// The minus sign is because of the direction of the surface normal.
Vector3 force = fricForce - normForce*lp.getNormal();
// We don't have an angular moment.
setForce(Vector6(Vector3::zeros(), force));
}
// Convert to XMarkerList
ml::XMarkerList ColoredMarkerList::toXMarkerList()
{
ml::XMarkerList result;
for(ml::ColoredMarkerList::iterator thisElement = this->begin(); thisElement != this->end(); ++thisElement)
{
ml::XMarker newMarker(Vector6(thisElement->x(),thisElement->y(),thisElement->z(),thisElement->c(),thisElement->t(),thisElement->u()), (int)thisElement->type);
newMarker.setName(thisElement->name());
result.push_back(newMarker);
}
return result;
}
//! Copy the values of the templates fields to \a _currentItem.
void ColoredMarkerListContainer::copyTemplateToCurrent()
{
ML_TRACE_IN("void ColoredMarkerListContainer::copyTemplateToCurrent()");
ML_TRY{
// ColoredMarker-specific fields
_currentItem.pos = Vector6(_fldNewPos3D->getVector3Value(), _fldNewPosC->getFloatValue(), _fldNewPosT->getFloatValue(), _fldNewPosU->getFloatValue());
_currentItem.color = Vector4(_fldNewColor->getVector3Value(), _fldNewAlpha->getFloatValue());
_currentItem.type = _fldNewType->getIntValue();
// Base class fields
ListContainerTemplate<ColoredMarkerList>::copyTemplateToCurrent();
}
ML_CATCH_RETHROW;
}
//! Initialize the list item at position \a index. This method is called by
//! insertItem() if the \a fromCurrentItem argument is false.
//! Moreover it is called when fldInit is touched.
void ColoredMarkerListContainer::initItem(MLssize_t index)
{
ML_TRACE_IN("void ColoredMarkerListContainer::initItem(MLssize_t index)");
ML_TRY{
ColoredMarker *marker = &(*_listPtr)[mlrange_cast<size_t>(index)];
// ColoredMarker-specific fields
marker->pos = Vector6(0);
marker->color = Vector4(0);
marker->type = 0;
// Base class fields
ListContainerTemplate<ColoredMarkerList>::initItem(index);
}
ML_CATCH_RETHROW;
}
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);
}
PrismaticJoint::Matrix6N
PrismaticJoint::getJointMatrix() const
{
return Vector6(Vector3::zeros(), mAxis);
}
Vector6
Pacejka89::getForce(const real_type& rho, const real_type& rhoDot,
const real_type& alpha, const real_type& kappa,
const real_type& gamma, const real_type& phi) const
{
// Pacejka suggests this correction to avoid singularities at zero speed
const real_type e_v = 1e-2;
// The normal force
real_type Fz = rho*mSpringConstant + mDampingConstant*rhoDot;
// The normal force cannot get negative here.
Fz = max(real_type(0), Fz);
// Pacejka's equations tend to provide infinitesimal stiff tires at low
// normal forces. Limit the input to the magic formula to a minimal
// normal load and rescale the output force according to the original
// load.
real_type FzRatio = 1;
if (Fz < mFzMin) {
FzRatio = Fz/mFzMin;
Fz = mFzMin;
}
// The normal force is mean to be in kN
Fz *= 1e-3;
//
// Longitudinal force (Pure longitudinal slip)
//
// Shape factor
real_type Cx = mB0;
// Peak factor
real_type Dx = (mB1*Fz + mB2)*Fz;
// BCD
real_type BCDx = (mB3*Fz + mB4)*Fz*exp(-mB5*Fz);
// Stiffness factor
real_type Bx = BCDx/(Cx*Dx + e_v);
// Horizonal shift
real_type SHx = (mB9*Fz + mB10);
// Vertical shift
real_type SVx = 0;
// Shifted longitudinal slip
// FIXME is this really in % ??? Also the other old model!!!
// real_type kappax = kappa + SHx;
real_type kappax = 100*kappa + SHx;
// Curvature factor
real_type Ex = (mB6*Fz + mB7)*Fz + mB8;
// See P175 Note on Fig 4.10: Clamp E <= 1 to avoid unrealistic behaviour
Ex = min(Ex, real_type(1));
// The resulting longitudinal force
real_type Bxk = Bx*kappax;
real_type Fx = Dx*sin(Cx*atan(Bxk - Ex*(Bxk - atan(Bxk)))) + SVx;
//
// Lateral force (Pure side slip)
//
// Shape factor
real_type Cy = mA0;
// Peak factor
real_type Dy = Fz*(mA1*Fz + mA2);
// BCD
real_type BCDy = mA3*sin(atan(Fz/mA4)*2)*(1 - mA5*fabs(gamma));
// Stiffness factor
real_type By = BCDy/(Cy*Dy + e_v);
// Horizonal shift
real_type SHy = mA9*Fz + mA10 + mA8*gamma;
// Vertical shift
real_type SVy = mA11*Fz*gamma + mA12*Fz + mA13;
// Shifted lateral slip
real_type alphay = rad2deg*alpha + SHy;
// Curvature factor
real_type Ey = mA6*Fz + mA7;
// See P175 Note on Fig 4.10: Clamp E <= 1 to avoid unrealistic behaviour
Ey = min(Ey, real_type(1));
// The resulting lateral force
real_type Bya = By*alphay;
real_type Fy = Dy*sin(Cy*atan(Bya - Ey*(Bya - atan(Bya)))) + SVy;
//
// Self aligning torque
//
// Shape factor
real_type Cz = mC0;
// Peak factor
real_type Dz = (mC1*Fz + mC2)*Fz;
// BCD
real_type BCDz = (mC3*Fz + mC4)*Fz*(1 - mC6*fabs(gamma))*exp(-mC5*Fz);
// Stiffness factor
real_type Bz = BCDz/(Cz*Dz + e_v);
// Horizonal shift
real_type SHz = mC11*gamma + mC12*Fz + mC13;
// Vertical shift
real_type SVz = (mC14*Fz + mC15)*Fz*gamma + mC16*Fz + mC17;
// Shifted lateral slip
real_type alphaz = rad2deg*alpha + SHz;
// Curvature factor
real_type Ez = ((mC7*Fz + mC8)*Fz + mC9)*(1 - mC10*fabs(gamma));
// See P175 Note on Fig 4.10: Clamp E <= 1 to avoid unrealistic behaviour
Ez = min(Ez, real_type(1));
// The resulting lateral force
real_type Bza = Bz*alphaz;
real_type Mz = Dz*sin(Cz*atan(Bza - Ez*(Bza - atan(Bza)))) + SVz;
return FzRatio*Vector6(Vector3(0, 0, Mz), Vector3(Fx, Fy, 1e3*Fz));
}
void LoadPointLineGeometry::_addToCache()
{
// Get highest type ID from posittions and connections
int highestTypeID = 0;
std::map<int, int> typeIDMap;
// Step 1: Scan all positions and get all type ID from them
for (XMarkerList::const_iterator it = _outCachedPositionsList.cbegin(); it != _outCachedPositionsList.cend(); ++it)
{
XMarker thisMarker = *it;
if (thisMarker.type > highestTypeID)
{
highestTypeID = thisMarker.type;
}
}
for (IndexPairList::const_iterator it = _outCachedConnectionsList.cbegin(); it != _outCachedConnectionsList.cend(); ++it)
{
IndexPair thisPair = *it;
if (thisPair.type > highestTypeID)
{
highestTypeID = thisPair.type;
}
}
// Add positions
for (XMarkerList::const_iterator it = _outPositionsList.cbegin(); it != _outPositionsList.cend(); ++it)
{
XMarker inMarker = *it;
int inMarkerType = inMarker.type;
int newMarkerType = inMarker.type;
// Check if type ID must be modified
if (inMarkerType <= highestTypeID)
{
std::map<int, int>::iterator findIt = typeIDMap.find(inMarkerType);
if (findIt == typeIDMap.end())
{
// inMarkerType has not yet been added to map, so add it now
newMarkerType = highestTypeID + 1;
typeIDMap[inMarkerType] = newMarkerType;
highestTypeID++;
}
else
{
newMarkerType = typeIDMap[inMarkerType];
}
}
XMarker newCachedMarker(Vector6(inMarker.x(), inMarker.y(), inMarker.z(), inMarker.c(), inMarker.t(), inMarker.u()), newMarkerType, inMarker.name());
_outCachedPositionsList.appendItem(newCachedMarker);
}
// Add connections
for (IndexPairList::const_iterator it = _outConnectionsList.cbegin(); it != _outConnectionsList.cend(); ++it)
{
IndexPair inPair = *it;
MLint inPairType = inPair.type;
MLint newPairType = inPair.type;
// Check if type ID must be modified
if (inPairType <= highestTypeID)
{
std::map<int, int>::iterator findIt = typeIDMap.find(inPairType);
if (findIt == typeIDMap.end())
{
// newPairType has not yet been added to map -> skip this connection!
inPairType = ML_INT_MIN;
}
else
{
newPairType = typeIDMap[inPairType];
}
}
if (inPairType > ML_INT_MIN)
{
IndexPair newCachedPair(inPair.index1, inPair.index2, newPairType, inPair.name());
_outCachedConnectionsList.appendItem(newCachedPair);
}
}
// Deselect items
_outCachedPositionsList.selectItemAt(-1);
_outCachedConnectionsList.selectItemAt(-1);
// Notify observers
_outCachedPositionsListFld->touch();
_outCachedConnectionsListFld->touch();
}
void LoadPointLineGeometry::_updateOutputData()
{
_outPositionsList.clearList();
_outConnectionsList.clearList();
std::string newSetFilter = "";
std::string newSetName = "";
int newSetType = 0;
std::string filterString = _filterFld->getStringValue();
std::string numberDelimiter = _numberDelimiterFld->getStringValue();
if (_numberDelimiterSpaceFld->getBoolValue())
{
numberDelimiter = " ";
}
std::string decimalSeparator = _decimalSeparatorFld->getStringValue();
for(StringVector::iterator it = _inputFileLinesVector.begin(); it != _inputFileLinesVector.end(); ++it)
{
std::string thisLine = *it;
StringVector thisDataLineComponents = mlPDF::PDFTools::stringSplit(thisLine, " ", false);
size_t thisDataLineNumComponents = thisDataLineComponents.size();
if (thisDataLineNumComponents > 0)
{
// Check if new set definition is started
if (thisDataLineNumComponents > 1) // Must be at least a a <Tag> <Type> {<Name>} pair/triple
{
std::string firstComponent = thisDataLineComponents[0];
if (isalpha(firstComponent[0]))
{
newSetFilter = firstComponent;
newSetType = mlPDF::stringToInt(thisDataLineComponents[1]);
if (thisDataLineNumComponents > 2)
{
newSetName = "";
// Append all remaining fragments to the name
for (size_t i = 2; i < thisDataLineNumComponents; i++)
{
if (newSetName != "")
{
newSetName += " ";
}
newSetName += thisDataLineComponents[i];
}
}
else
{
newSetName = "";
}
continue;
}
}
if ((filterString == "") || (filterString == newSetFilter))
{
// If it is a list of connections
if (thisDataLineNumComponents == 2)
{
// Add point (only the first 3 components)
int start = mlPDF::stringToInt(thisDataLineComponents[0]);
int end = mlPDF::stringToInt(thisDataLineComponents[1]);
IndexPair newConnection(start, end, newSetType, newSetName.c_str());
_outConnectionsList.appendItem(newConnection);
}
// If it is a list of positions
else if (thisDataLineNumComponents > 2)
{
// Add point (only the first 3 components)
double x = mlPDF::stringToDouble(thisDataLineComponents[0]);
double y = mlPDF::stringToDouble(thisDataLineComponents[1]);
double z = mlPDF::stringToDouble(thisDataLineComponents[2]);
XMarker newPosition(Vector6(x, y, z, 0, 0, 0), newSetType, newSetName.c_str());
_outPositionsList.appendItem(newPosition);
}
}
} // if (thisDataLineNumComponents > 0)
} // for(StringVector::iterator it
// Deselect items
_outPositionsList.selectItemAt(-1);
_outConnectionsList.selectItemAt(-1);
_createFibers();
if (_autoAddToCacheFld->getBoolValue())
{
_addToCache();
}
// Set validation fields
_positionsLoadedFld->setBoolValue(_outPositionsList.size() > 0);
_connectionsLoadedFld->setBoolValue(_outConnectionsList.size() > 0);
// Update output
_outPositionsListFld->touch();
_outConnectionsListFld->touch();
_outFibersFld->touch();
}
