//! 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;
}
Пример #2
0
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);
}
Пример #8
0
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();
}