C++ SimTK::Vec3方法代码示例

//_____________________________________________________________________________
// Allocate and initialize properties.
void PointActuator::constructProperties()
{
    constructProperty_body();
    constructProperty_point(Vec3(0)); // origin
    constructProperty_point_is_global(false);
    constructProperty_direction(Vec3(1,0,0)); // arbitrary
    constructProperty_force_is_global(false);
    constructProperty_optimal_force(1.0);
}

//_____________________________________________________________________________
// Construct and initialize properties.
void PointToPointActuator::constructProperties()
{
    constructProperty_bodyA();
    constructProperty_bodyB();
    constructProperty_points_are_global(false);
    constructProperty_pointA(Vec3(0));  // origin
    constructProperty_pointB(Vec3(0));
    constructProperty_optimal_force(1.0);
}

void ContactGeometry::constructProperties()
{
    constructProperty_location(Vec3(0));
    constructProperty_orientation(Vec3(0));
    constructProperty_display_preference(1);

    Array<double> defaultColor(1.0, 3); //color default to 0, 1, 1
    defaultColor[0] = 0.0; 
    constructProperty_color(defaultColor);
}

// [Step 2, Task C]
Device* buildDevice() {
    using SimTK::Vec3;
    using SimTK::Inertia;

    // Create the device.
    auto device = new Device();
    device->setName("device");

    // The device's mass is distributed between two identical cuffs that attach
    // to the hopper via WeldJoints (to be added below).
    double deviceMass = 2.0;
    auto cuffA = new Body("cuffA", deviceMass/2., Vec3(0), Inertia(0.5));
    //TODO: Repeat for cuffB.

    //TODO: Add the cuff Components to the device.

    // Attach a sphere to each cuff for visualization.
    auto sphere = new Sphere(0.01);
    sphere->setName("sphere");
    sphere->setColor(SimTK::Red);
    sphere->setFrame(*cuffA); cuffA->attachGeometry(sphere);
    //sphere->setFrame(*cuffB); cuffB->attachGeometry(sphere->clone());

    // Create a WeldJoint to anchor cuffA to the hopper.
    auto anchorA = new WeldJoint();
    anchorA->setName("anchorA");
    //TODO: Connect the "child_frame" (a PhysicalFrame) Connector of anchorA to
    //      cuffA. Note that only the child frame is connected now; the parent
    //      frame will be connected in exampleHopperDevice.cpp.

    //TODO: Add anchorA to the device.

    //TODO: Create a WeldJoint to anchor cuffB to the hopper. Connect the
    //      "child_frame" Connector of anchorB to cuffB and add anchorB to the
    //      device.

    // Attach a PathActuator between the two cuffs.
    auto pathActuator = new PathActuator();
    pathActuator->setName("cableAtoB");
    pathActuator->set_optimal_force(OPTIMAL_FORCE);
    pathActuator->addNewPathPoint("pointA", *cuffA, Vec3(0));
    //pathActuator->addNewPathPoint("pointB", *cuffB, Vec3(0));
    device->addComponent(pathActuator);

    // Create a PropMyoController.
    auto controller = new PropMyoController();
    controller->setName("controller");
    controller->set_gain(GAIN);

    //TODO: Connect the controller's "actuator" Connector to pathActuator.

    //TODO: Add the controller to the device.

    return device;
}

// get the path as PointForceDirections directions 
// CAUTION: the return points are heap allocated; you must delete them yourself! 
// (TODO: that is really lame)
void GeometryPath::
getPointForceDirections(const SimTK::State& s, 
                        OpenSim::Array<PointForceDirection*> *rPFDs) const
{
    int i;
    PathPoint* start;
    PathPoint* end;
    const OpenSim::PhysicalFrame* startBody;
    const OpenSim::PhysicalFrame* endBody;
    const Array<PathPoint*>& currentPath = getCurrentPath(s);

    int np = currentPath.getSize();
    rPFDs->ensureCapacity(np);
    
    for (i = 0; i < np; i++) {
        PointForceDirection *pfd = 
            new PointForceDirection(currentPath[i]->getLocation(), 
                                    currentPath[i]->getBody(), Vec3(0));
        rPFDs->append(pfd);
    }

    for (i = 0; i < np-1; i++) {
        start = currentPath[i];
        end = currentPath[i+1];
        startBody = &start->getBody();
        endBody = &end->getBody();

        if (startBody != endBody)
        {
            Vec3 posStart, posEnd;
            Vec3 direction(0);

            // Find the positions of start and end in the inertial frame.
            //engine.getPosition(s, start->getBody(), start->getLocation(), posStart);
            posStart = start->getLocationInGround(s);
            
            //engine.getPosition(s, end->getBody(), end->getLocation(), posEnd);
            posEnd = end->getLocationInGround(s);

            // Form a vector from start to end, in the inertial frame.
            direction = (posEnd - posStart);

            // Check that the two points are not coincident.
            // This can happen due to infeasible wrapping of the path,
            // when the origin or insertion enters the wrapping surface.
            // This is a temporary fix, since the wrap algorithm should
            // return NaN for the points and/or throw an Exception- aseth
            if (direction.norm() < SimTK::SignificantReal){
                direction = direction*SimTK::NaN;
            }
            else{
                direction = direction.normalize();
            }

            // Get resultant direction at each point 
            rPFDs->get(i)->addToDirection(direction);
            rPFDs->get(i+1)->addToDirection(-direction);
        }
    }
}

//_____________________________________________________________________________
// Construct and initialize properties.
void TorqueActuator::constructProperties()
{
	setAuthors("Ajay Seth, Matt DeMers");
    constructProperty_bodyA();
    constructProperty_bodyB();
    constructProperty_torque_is_global(true);
	constructProperty_axis(Vec3(0,0,1)); // z direction
    constructProperty_optimal_force(1.0);
}

/**
 * Get the inertia matrix of the body.
 *
 * @return 3x3 inertia matrix.
 */
const SimTK::Inertia& Body::getInertia() const
{
    // Has not been set programmatically
    if (_inertia.isNaN()){
        // initialize from properties
        const double& m = getMass();
        // if mass is zero, non-zero inertia makes no sense
        if (-SimTK::SignificantReal <= m && m <= SimTK::SignificantReal){
            // force zero inertia
            cout<<"Body '"<<getName()<<"' is massless but nonzero inertia provided.";
            cout<<" Inertia reset to zero. "<<"Otherwise provide nonzero mass."<< endl;
            _inertia = SimTK::Inertia(0);
        }
        else{
            const SimTK::Vec6& Ivec = get_inertia();
            try {
                _inertia = SimTK::Inertia(Ivec.getSubVec<3>(0), Ivec.getSubVec<3>(3));
            } 
            catch (const std::exception& ex){
                // Should throw an Exception but we have models we have released with
                // bad inertias. E.g. early gait23 models had an error in the inertia
                // of the toes Body. We cannot allow failures with our models so 
                // raise a warning and do something sensible with the values at hand.
                cout << "WARNING: Body " + getName() + " has invalid inertia. " << endl;
                cout << ex.what() << endl;

                // get some aggregate value for the inertia based on existing values
                double diag = Ivec.getSubVec<3>(0).norm()/sqrt(3.0);

                // and then assume a spherical shape.
                _inertia = SimTK::Inertia(Vec3(diag), Vec3(0));
                
                cout << getName() << " Body's inertia being reset to:" << endl;
                cout << _inertia << endl;
            }
        }
    }
    return _inertia;
}

Vec3 PrescribedForce::getPointAtTime(double aTime) const
{
    const FunctionSet& pointFunctions = getPointFunctions();

    if (pointFunctions.getSize() != 3)
        return Vec3(0);

    const SimTK::Vector timeAsVector(1, aTime);
    const Vec3 point(pointFunctions[0].calcValue(timeAsVector), 
                     pointFunctions[1].calcValue(timeAsVector), 
                     pointFunctions[2].calcValue(timeAsVector));
    return point;
}

Vec3 PrescribedForce::getTorqueAtTime(double aTime) const
{
    const FunctionSet& torqueFunctions = getTorqueFunctions();

    if (torqueFunctions.getSize() != 3)
        return Vec3(0);

    const SimTK::Vector timeAsVector(1, aTime);
    const Vec3 torque(torqueFunctions[0].calcValue(timeAsVector), 
                      torqueFunctions[1].calcValue(timeAsVector), 
                      torqueFunctions[2].calcValue(timeAsVector));
    return torque;
}

void PrescribedForce::computeForce(const SimTK::State& state, 
                              SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
                              SimTK::Vector& generalizedForces) const
{
    const bool pointIsGlobal = get_pointIsGlobal();
    const bool forceIsGlobal = get_forceIsGlobal();
    const FunctionSet& forceFunctions = getForceFunctions();
    const FunctionSet& pointFunctions = getPointFunctions();
    const FunctionSet& torqueFunctions = getTorqueFunctions();

    double time = state.getTime();
    SimTK::Vector  timeAsVector(1, time);

    const bool hasForceFunctions  = forceFunctions.getSize()==3;
    const bool hasPointFunctions  = pointFunctions.getSize()==3;
    const bool hasTorqueFunctions = torqueFunctions.getSize()==3;

    const PhysicalFrame& frame =
        getConnector<PhysicalFrame>("frame").getConnectee();
    const Ground& gnd = getModel().getGround();
    if (hasForceFunctions) {
        Vec3 force(forceFunctions[0].calcValue(timeAsVector), 
                   forceFunctions[1].calcValue(timeAsVector), 
                   forceFunctions[2].calcValue(timeAsVector));
        if (!forceIsGlobal)
            force = frame.expressVectorInAnotherFrame(state, force, gnd);

        Vec3 point(0); // Default is body origin.
        if (hasPointFunctions) {
            // Apply force to a specified point on the body.
            point = Vec3(pointFunctions[0].calcValue(timeAsVector), 
                         pointFunctions[1].calcValue(timeAsVector), 
                         pointFunctions[2].calcValue(timeAsVector));
            if (pointIsGlobal)
                point = gnd.findLocationInAnotherFrame(state, point, frame);

        }
        applyForceToPoint(state, frame, point, force, bodyForces);
    }
    if (hasTorqueFunctions){
        Vec3 torque(torqueFunctions[0].calcValue(timeAsVector), 
                    torqueFunctions[1].calcValue(timeAsVector), 
                    torqueFunctions[2].calcValue(timeAsVector));
        if (!forceIsGlobal)
            torque = frame.expressVectorInAnotherFrame(state, torque, gnd);

         applyTorque(state, frame, torque, bodyForces);
    }
}

/**
 * Perform some set up functions that happen after the
 * object has been deserialized or copied.
 *
 * @param aEngine dynamics engine containing this wrap object.
 * @param aBody body containing this wrap object.
 */
void WrapObject::connectToModelAndBody(Model& aModel, OpenSim::Body& aBody)
{
   _body = &aBody;
   _model = &aModel;

	setupQuadrant();

	SimTK::Rotation rot;
	rot.setRotationToBodyFixedXYZ(Vec3(_xyzBodyRotation[0], _xyzBodyRotation[1], _xyzBodyRotation[2]));
	_pose.set(rot, _translation);

	// Object is visible (has displayer) and depends on body it's attached to.
	_body->updDisplayer()->addDependent(getDisplayer());
	_displayer.setTransform(_pose);
	_displayer.setOwner(this);
}

/**
 * This method creates a SimmMotionTrial instance with the markerFile and
 * timeRange parameters. It also creates a Storage instance with the
 * coordinateFile parameter. Then it updates the coordinates and markers in
 * the model, if specified. Then it does IK to fit the model to the static
 * pose. Then it uses the current model pose to relocate all non-fixed markers
 * according to their locations in the SimmMotionTrial. Then it writes the
 * output files selected by the user.
 *
 * @param aModel the model to use for the marker placing process.
 * @return Whether the marker placing process was successful or not.
 */
bool MarkerPlacer::processModel(Model* aModel,
        const string& aPathToSubject) const {

    if(!getApply()) return false;

    cout << endl << "Step 3: Placing markers on model" << endl;

    if (_timeRange.getSize()<2) 
        throw Exception("MarkerPlacer::processModel, time_range is unspecified.");

    /* Load the static pose marker file, and average all the
    * frames in the user-specified time range.
    */
    TimeSeriesTableVec3 staticPoseTable{aPathToSubject + _markerFileName};
    const auto& timeCol = staticPoseTable.getIndependentColumn();

    // Users often set a time range that purposely exceeds the range of
    // their data with the mindset that all their data will be used.
    // To allow for that, we have to narrow the provided range to data
    // range, since the TimeSeriesTable will correctly throw that the 
    // desired time exceeds the data range.
    if (_timeRange[0] < timeCol.front())
        _timeRange[0] = timeCol.front();
    if (_timeRange[1] > timeCol.back())
        _timeRange[1] = timeCol.back();

    const auto avgRow = staticPoseTable.averageRow(_timeRange[0],
                                                   _timeRange[1]);
    for(size_t r = staticPoseTable.getNumRows(); r-- > 0; )
        staticPoseTable.removeRowAtIndex(r);
    staticPoseTable.appendRow(_timeRange[0], avgRow);
    
    OPENSIM_THROW_IF(!staticPoseTable.hasTableMetaDataKey("Units"),
                     Exception,
                     "MarkerPlacer::processModel -- Marker file does not have "
                     "'Units'.");
    Units
    staticPoseUnits{staticPoseTable.getTableMetaData<std::string>("Units")};
    double scaleFactor = staticPoseUnits.convertTo(aModel->getLengthUnits());
    OPENSIM_THROW_IF(SimTK::isNaN(scaleFactor),
                     Exception,
                     "Model has unspecified units.");
    if(std::fabs(scaleFactor - 1) >= SimTK::Eps) {
        for(unsigned r = 0; r < staticPoseTable.getNumRows(); ++r)
            staticPoseTable.updRowAtIndex(r) *= scaleFactor;

        staticPoseUnits = aModel->getLengthUnits();
        staticPoseTable.removeTableMetaDataKey("Units");
        staticPoseTable.addTableMetaData("Units",
                                         staticPoseUnits.getAbbreviation());
    }
    
    MarkerData* staticPose = new MarkerData(aPathToSubject + _markerFileName);
    staticPose->averageFrames(_maxMarkerMovement, _timeRange[0], _timeRange[1]);
    staticPose->convertToUnits(aModel->getLengthUnits());

    /* Delete any markers from the model that are not in the static
     * pose marker file.
     */
    aModel->deleteUnusedMarkers(staticPose->getMarkerNames());

    // Construct the system and get the working state when done changing the model
    SimTK::State& s = aModel->initSystem();
    s.updTime() = _timeRange[0];
    
    // Create references and WeightSets needed to initialize InverseKinemaicsSolver
    Set<MarkerWeight> markerWeightSet;
    _ikTaskSet.createMarkerWeightSet(markerWeightSet); // order in tasks file
    // MarkersReference takes ownership of marker data (staticPose)
    MarkersReference markersReference(staticPoseTable, &markerWeightSet);
    SimTK::Array_<CoordinateReference> coordinateReferences;

    // Load the coordinate data
    // create CoordinateReferences for Coordinate Tasks
    FunctionSet *coordFunctions = NULL;
    // bool haveCoordinateFile = false;
    if(_coordinateFileName != "" && _coordinateFileName != "Unassigned"){
        Storage coordinateValues(aPathToSubject + _coordinateFileName);
        aModel->getSimbodyEngine().convertDegreesToRadians(coordinateValues);
        // haveCoordinateFile = true;
        coordFunctions = new GCVSplineSet(5,&coordinateValues);
    }
    
    int index = 0;
    for(int i=0; i< _ikTaskSet.getSize(); i++){
        IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i]);
        if (coordTask && coordTask->getApply()){
            std::unique_ptr<CoordinateReference> coordRef{};
//.........这里部分代码省略.........

/**
 * Average all the frames between aStartTime and
 * aEndTime (inclusive) and store the result in the first
 * frame. All other frames are deleted. The time and frame
 * number of this one remaining frame are copied from the
 * startIndex frame. The aThreshold parameter is for printing
 * a warning if any marker moves more than that amount in
 * the averaged frames. aThreshold is specified by the user,
 * and is assumed to be in the units of the marker data.
 *
 * @param aThreshold amount of marker movement that is allowed for averaging.
 * @param aStartTime start time of frame range to average.
 * @param aEndTime end time of frame range to average.
 */
void MarkerData::averageFrames(double aThreshold, double aStartTime, double aEndTime)
{
	if (_numFrames < 2)
		return;

	int startIndex = 0, endIndex = 1;
	double *minX = NULL, *minY = NULL, *minZ = NULL, *maxX = NULL, *maxY = NULL, *maxZ = NULL;

	findFrameRange(aStartTime, aEndTime, startIndex, endIndex);
	MarkerFrame *averagedFrame = new MarkerFrame(*_frames[startIndex]);

	/* If aThreshold is greater than zero, then calculate
	 * the movement of each marker so you can check if it
	 * is greater than aThreshold.
	 */
	if (aThreshold > 0.0)
	{
		minX = new double [_numMarkers];
		minY = new double [_numMarkers];
		minZ = new double [_numMarkers];
		maxX = new double [_numMarkers];
		maxY = new double [_numMarkers];
		maxZ = new double [_numMarkers];
		for (int i = 0; i < _numMarkers; i++)
		{
			minX[i] = minY[i] = minZ[i] =  SimTK::Infinity;
			maxX[i] = maxY[i] = maxZ[i] = -SimTK::Infinity;
		}
	}

	/* Initialize all the averaged marker locations to 0,0,0. Then
	 * loop through the frames to be averaged, adding each marker location
	 * to averagedFrame. Keep track of the min/max XYZ for each marker
	 * so you can compare it to aThreshold when you're done.
	 */
	for (int i = 0; i < _numMarkers; i++)
	{
		int numFrames = 0;
		Vec3& avePt = averagedFrame->updMarker(i);
		avePt = Vec3(0);

		for (int j = startIndex; j <= endIndex; j++)
		{
			Vec3& pt = _frames[j]->updMarker(i);
			if (!pt.isNaN())
			{
				Vec3& coords = pt; //.get();
				avePt += coords;
				numFrames++;
				if (aThreshold > 0.0)
				{
					if (coords[0] < minX[i])
						minX[i] = coords[0];
					if (coords[0] > maxX[i])
						maxX[i] = coords[0];
					if (coords[1] < minY[i])
						minY[i] = coords[1];
					if (coords[1] > maxY[i])
						maxY[i] = coords[1];
					if (coords[2] < minZ[i])
						minZ[i] = coords[2];
					if (coords[2] > maxZ[i])
						maxZ[i] = coords[2];
				}
			}
		}

		/* Now divide by the number of frames to get the average. */
		if (numFrames > 0)
			avePt /= (double)numFrames;
		else
			avePt = Vec3(SimTK::NaN) ;//(SimTK::NaN, SimTK::NaN, SimTK::NaN);
	}

	/* Store the indices from the file of the first frame and
	 * last frame that were averaged, so you can report them later.
	 */
	int startUserIndex = _frames[startIndex]->getFrameNumber();
	int endUserIndex = _frames[endIndex]->getFrameNumber();

	/* Now delete all the existing frames and insert the averaged one. */
	_frames.clearAndDestroy();
	_frames.append(averagedFrame);
	_numFrames = 1;
	_firstFrameNumber = _frames[0]->getFrameNumber();

//.........这里部分代码省略.........

/**
 * This method creates a SimmMotionTrial instance with the markerFile and
 * timeRange parameters. It also creates a Storage instance with the
 * coordinateFile parameter. Then it updates the coordinates and markers in
 * the model, if specified. Then it does IK to fit the model to the static
 * pose. Then it uses the current model pose to relocate all non-fixed markers
 * according to their locations in the SimmMotionTrial. Then it writes the
 * output files selected by the user.
 *
 * @param aModel the model to use for the marker placing process.
 * @return Whether the marker placing process was successful or not.
 */
bool MarkerPlacer::processModel(Model* aModel, const string& aPathToSubject)
{
    if(!getApply()) return false;

    cout << endl << "Step 3: Placing markers on model" << endl;

    /* Load the static pose marker file, and average all the
     * frames in the user-specified time range.
     */
    MarkerData staticPose(aPathToSubject + _markerFileName);
    if (_timeRange.getSize()<2) 
        throw Exception("MarkerPlacer::processModel, time_range is unspecified.");

    staticPose.averageFrames(_maxMarkerMovement, _timeRange[0], _timeRange[1]);
    staticPose.convertToUnits(aModel->getLengthUnits());

    /* Delete any markers from the model that are not in the static
     * pose marker file.
     */
    aModel->deleteUnusedMarkers(staticPose.getMarkerNames());

    // Construct the system and get the working state when done changing the model
    SimTK::State& s = aModel->initSystem();
    
    // Create references and WeightSets needed to initialize InverseKinemaicsSolver
    Set<MarkerWeight> markerWeightSet;
    _ikTaskSet.createMarkerWeightSet(markerWeightSet); // order in tasks file
    MarkersReference markersReference(staticPose, &markerWeightSet);
    SimTK::Array_<CoordinateReference> coordinateReferences;

    // Load the coordinate data
    // create CoordinateReferences for Coordinate Tasks
    FunctionSet *coordFunctions = NULL;
    bool haveCoordinateFile = false;
    if(_coordinateFileName != "" && _coordinateFileName != "Unassigned"){
        Storage coordinateValues(aPathToSubject + _coordinateFileName);
        aModel->getSimbodyEngine().convertDegreesToRadians(coordinateValues);
        haveCoordinateFile = true;
        coordFunctions = new GCVSplineSet(5,&coordinateValues);
    }
    
    int index = 0;
    for(int i=0; i< _ikTaskSet.getSize(); i++){
        IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i]);
        if (coordTask && coordTask->getApply()){
            CoordinateReference *coordRef = NULL;
            if(coordTask->getValueType() == IKCoordinateTask::FromFile){
                index = coordFunctions->getIndex(coordTask->getName(), index);
                if(index >= 0){
                    coordRef = new CoordinateReference(coordTask->getName(),coordFunctions->get(index));
                }
            }
            else if((coordTask->getValueType() == IKCoordinateTask::ManualValue)){
                Constant reference(Constant(coordTask->getValue()));
                coordRef = new CoordinateReference(coordTask->getName(), reference);
            }
            else{ // assume it should be held at its current/default value
                double value = aModel->getCoordinateSet().get(coordTask->getName()).getValue(s);
                Constant reference = Constant(value);
                coordRef = new CoordinateReference(coordTask->getName(), reference);
            }

            if(coordRef == NULL)
                throw Exception("MarkerPlacer: value for coordinate "+coordTask->getName()+" not found.");

            // We have a valid coordinate reference so now set its weight according to the task
            coordRef->setWeight(coordTask->getWeight());
            coordinateReferences.push_back(*coordRef);      
        }           
    }
    double constraintWeight = std::numeric_limits<SimTK::Real>::infinity();

    InverseKinematicsSolver ikSol(*aModel, markersReference,
                                  coordinateReferences, constraintWeight);
    ikSol.assemble(s);

    // Call realize Position so that the transforms are updated and  markers can be moved correctly
    aModel->getMultibodySystem().realize(s, SimTK::Stage::Position);
    // Report marker errors to assess the quality 
    int nm = markerWeightSet.getSize();
    SimTK::Array_<double> squaredMarkerErrors(nm, 0.0);
    SimTK::Array_<Vec3> markerLocations(nm, Vec3(0));
    double totalSquaredMarkerError = 0.0;
    double maxSquaredMarkerError = 0.0;
    int worst = -1;
    // Report in the same order as the marker tasks/weights
    ikSol.computeCurrentSquaredMarkerErrors(squaredMarkerErrors);
    for(int j=0; j<nm; ++j){
//.........这里部分代码省略.........

void BodyActuator::constructProperties()
{
    constructProperty_point(Vec3(0)); // origin
    constructProperty_point_is_global(false);
    constructProperty_spatial_force_is_global(true);
}