C++ Body::addDisplayGeometry方法代码示例

/**
 * Run a simulation of block sliding with contact on by two muscles sliding with contact 
 */
int main()
{

	try {
		// Create a new OpenSim model
		Model osimModel;
		osimModel.setName("osimModel");

		double Pi = SimTK::Pi;
		
		
		// Get the ground body
		OpenSim::Body& ground = osimModel.getGroundBody();
		ground.addDisplayGeometry("checkered_floor.vtp");

		// create linkage body
		double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06;
		
		Vec3 linkageDimensions(linkageDiameter, linkageLength, linkageDiameter);
		Vec3 linkageMassCenter(0,linkageLength/2,0);
		Inertia linkageInertia = Inertia::cylinderAlongY(linkageDiameter/2.0, linkageLength/2.0);

		OpenSim::Body* linkage1 = new OpenSim::Body("linkage1", linkageMass, linkageMassCenter, linkageMass*linkageInertia);

		// Graphical representation
		linkage1->addDisplayGeometry("cylinder.vtp");
		//This cylinder.vtp geometry is 1 meter tall, 1 meter diameter.  Scale and shift it to look pretty
		GeometrySet& geometry = linkage1->updDisplayer()->updGeometrySet();
		DisplayGeometry& thinCylinder = geometry[0];
		thinCylinder.setScaleFactors(linkageDimensions);
		thinCylinder.setTransform(Transform(Vec3(0.0,linkageLength/2.0,0.0)));
		linkage1->addDisplayGeometry("sphere.vtp");
		//This sphere.vtp is 1 meter in diameter.  Scale it.
		geometry[1].setScaleFactors(Vec3(0.1));
		
		// Creat a second linkage body
		OpenSim::Body* linkage2 = new OpenSim::Body(*linkage1);
		linkage2->setName("linkage2");

		// Creat a block to be the pelvis
		double blockMass = 20.0, blockSideLength = 0.2;
		Vec3 blockMassCenter(0);
		Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
		OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);
		block->addDisplayGeometry("block.vtp");
		//This block.vtp is 0.1x0.1x0.1 meters.  scale its appearance
		block->updDisplayer()->updGeometrySet()[0].setScaleFactors(Vec3(2.0));

		// Create 1 degree-of-freedom pin joints between the bodies to creat a kinematic chain from ground through the block
		
		Vec3 orientationInGround(0), locationInGround(0), locationInParent(0.0, linkageLength, 0.0), orientationInChild(0), locationInChild(0);

		PinJoint *ankle = new PinJoint("ankle", ground, locationInGround, orientationInGround, *linkage1, 
			locationInChild, orientationInChild);

		PinJoint *knee = new PinJoint("knee", *linkage1, locationInParent, orientationInChild, *linkage2,
			locationInChild, orientationInChild);

		PinJoint *hip = new PinJoint("hip", *linkage2, locationInParent, orientationInChild, *block,
			locationInChild, orientationInChild);
		
		double range[2] = {-SimTK::Pi*2, SimTK::Pi*2};
		CoordinateSet& ankleCoordinateSet = ankle->upd_CoordinateSet();
		ankleCoordinateSet[0].setName("q1");
		ankleCoordinateSet[0].setRange(range);

		CoordinateSet& kneeCoordinateSet = knee->upd_CoordinateSet();
		kneeCoordinateSet[0].setName("q2");
		kneeCoordinateSet[0].setRange(range);

		CoordinateSet& hipCoordinateSet = hip->upd_CoordinateSet();
		hipCoordinateSet[0].setName("q3");
		hipCoordinateSet[0].setRange(range);

		// Add the bodies to the model
		osimModel.addBody(linkage1);
		osimModel.addBody(linkage2);
		osimModel.addBody(block);

		// Define contraints on the model
		//  Add a point on line constraint to limit the block to vertical motion

		Vec3 lineDirection(0,1,0), pointOnLine(0,0,0), pointOnBlock(0);
		PointOnLineConstraint *lineConstraint = new PointOnLineConstraint(ground, lineDirection, pointOnLine, *block, pointOnBlock);
		osimModel.addConstraint(lineConstraint);

		// Add PistonActuator between the first linkage and the block
		Vec3 pointOnBodies(0);
		PistonActuator *piston = new PistonActuator();
		piston->setName("piston");
		piston->setBodyA(linkage1);
		piston->setBodyB(block);
		piston->setPointA(pointOnBodies);
		piston->setPointB(pointOnBodies);
		piston->setOptimalForce(200.0);
		piston->setPointsAreGlobal(false);

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

/**
* This test verifies the use of BodyActuator for applying spatial forces to a selected
* body. It checks if using a BodyActuator generates equivalent acceleration compared 
* to when applying the forces via mobilityForce.
*
* @author Soha Pouya
*/
void testBodyActuator()
{
	using namespace SimTK;
	// start timing
	std::clock_t startTime = std::clock();

	// Setup OpenSim model
	Model *model = new Model;

	// turn off gravity
	model->setGravity(Vec3(0));

	//OpenSim body 1: Ground
	OpenSim::Body& ground = model->getGroundBody();

	// OpenSim body 2: A Block
	// Geometrical/Inertial properties for the block
	double blockMass = 1.0, blockSideLength = 1;
	Vec3 blockMassCenter(0);
	Inertia blockInertia = blockMass*Inertia::brick(blockSideLength/2,
		blockSideLength/2, blockSideLength/2); // for the halves see doxygen for brick 

	OpenSim::Body *block = new OpenSim::Body("block", blockMass, 
											 blockMassCenter, blockInertia);

	// Add display geometry to the block to visualize in the GUI
	block->addDisplayGeometry("block.vtp");

	Vec3 locationInParent(0, blockSideLength / 2, 0), orientationInParent(0), 
		locationInBody(0), orientationInBody(0);
	FreeJoint *blockToGroundFree = new FreeJoint("blockToGroundBall", 
		ground, locationInParent, orientationInParent, 
		*block, locationInBody, orientationInBody);
	
	model->addBody(block);
	model->addJoint(blockToGroundFree);
	
	// specify magnitude and direction of applied force and torque
	double forceMag = 1.0;
	Vec3 forceAxis(1, 1, 1);
	Vec3 forceInG = forceMag * forceAxis;

	double torqueMag = 1.0;
	Vec3 torqueAxis(1, 1, 1);
	Vec3 torqueInG = torqueMag*torqueAxis;

	// ---------------------------------------------------------------------------
	// Use MobilityForces to Apply the given Torques and Forces to the body
	// ---------------------------------------------------------------------------
	State& state = model->initSystem();

	model->getMultibodySystem().realize(state, Stage::Dynamics);
	Vector_<SpatialVec>& bodyForces =
		model->getMultibodySystem().updRigidBodyForces(state, Stage::Dynamics);
	bodyForces.dump("Body Forces before applying 6D spatial force:");

	model->getMatterSubsystem().addInBodyTorque(state, block->getIndex(),
		torqueInG, bodyForces);
	model->getMatterSubsystem().addInStationForce(state, block->getIndex(),
		Vec3(0), forceInG, bodyForces);

	bodyForces.dump("Body Forces after applying 6D spatial force to the block");

	model->getMultibodySystem().realize(state, Stage::Acceleration);
	Vector udotBody = state.getUDot();
	udotBody.dump("Accelerations due to body forces");

	// clear body forces
	bodyForces *= 0;

	// update mobility forces
	Vector& mobilityForces = model->getMultibodySystem()
		.updMobilityForces(state, Stage::Dynamics);

	// Apply torques as mobility forces of the ball joint
	for (int i = 0; i<3; ++i){
		mobilityForces[i] = torqueInG[i];
		mobilityForces[i+3] = forceInG[i];
	}
	mobilityForces.dump("Mobility Forces after applying 6D spatial force to the block");


	model->getMultibodySystem().realize(state, Stage::Acceleration);
	Vector udotMobility = state.getUDot();
	udotMobility.dump("Accelerations due to mobility forces");

	// First make sure that accelerations are not zero accidentally
	ASSERT(udotMobility.norm() != 0.0 || udotBody.norm() != 0.0);
	// Then check if they are equal
	for (int i = 0; i<udotMobility.size(); ++i){
		ASSERT_EQUAL(udotMobility[i], udotBody[i], SimTK::Eps);
	}

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

/**
* This test verifies if using a BodyActuator generates equivalent result in the body
* acceleration compared to when using a combination of PointActuaor, TorqueActuaor 
* and BodyActuator. 
* It therefore also verifies model consistency when user defines and uses a 
* combination of these 3 actuators. 
* 
* @author Soha Pouya
*/
void testActuatorsCombination()
{
	using namespace SimTK;
	// start timing
	std::clock_t startTime = std::clock();

	// Setup OpenSim model
	Model *model = new Model;

	// turn off gravity
	model->setGravity(Vec3(0));

	// OpenSim bodies: 1) The ground
	OpenSim::Body& ground = model->getGroundBody();
	//ground.addDisplayGeometry("block.vtp");

	// OpenSim bodies: 2) A Block
	// Geometrical/Inertial properties for the block
	double blockMass = 1.0, blockSideLength = 1.0;
	Vec3 blockMassCenter(0);
	// Brick Inertia: for the halves see doxygen  
	Inertia blockInertia = blockMass*Inertia::brick(blockSideLength/2, 
									blockSideLength/2, blockSideLength/2);
	std::cout << "blockInertia: " << blockInertia << std::endl;

	OpenSim::Body *block = new OpenSim::Body("block", blockMass, 
									blockMassCenter, blockInertia);

	// Add display geometry to the block to visualize in the GUI
	block->addDisplayGeometry("block.vtp");

	// Make a FreeJoint from block to ground
	Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), //locationInParent(0, blockSideLength, 0)
		 locationInBody(0), orientationInBody(0);
	FreeJoint *blockToGroundFree = new FreeJoint("blockToGroundFreeJoint", 
				ground, locationInParent, orientationInParent, 
				*block, locationInBody, orientationInBody);

	// Add the body and joint to the model
	model->addBody(block);
	model->addJoint(blockToGroundFree);

	// specify magnitude and direction of desired force and torque vectors to apply
	double forceMag = 1.0;
	Vec3 forceAxis(1, 1, 1);
	SimTK::UnitVec3 forceUnitAxis(forceAxis); // to normalize
	Vec3 forceInG = forceMag * forceUnitAxis;

	double torqueMag = 1.0;
	Vec3 torqueAxis(1, 2, 1);
	SimTK::UnitVec3 torqueUnitAxis(torqueAxis); // to normalize
	Vec3 torqueInG = torqueMag*torqueUnitAxis;

	// needed to be called here once to build controller for body actuator
	State& state = model->initSystem();
	
	// ---------------------------------------------------------------------------
	// Add a set of PointActuator, TorqueActuator and BodyActuator to the model
	// ---------------------------------------------------------------------------
	// Create and add a body actuator to the model
	BodyActuator* bodyActuator1 = new BodyActuator(*block);
	bodyActuator1->setName("BodyAct1");
	bodyActuator1->set_point(Vec3(0, blockSideLength/2, 0));
	model->addForce(bodyActuator1);
	
	// Create and add a torque actuator to the model
	TorqueActuator* torqueActuator =
		new TorqueActuator(*block, ground, torqueUnitAxis, true);
	torqueActuator->setName("torqueAct");
	model->addForce(torqueActuator);

	// Create and add a point actuator to the model
	PointActuator* pointActuator =
		new PointActuator("block");
	pointActuator->setName("pointAct");
	pointActuator->set_direction(forceUnitAxis);
	pointActuator->set_point(Vec3(0, blockSideLength/2,0));
	model->addForce(pointActuator);

	// ------ build the model -----
	model->print("TestActuatorCombinationModel.osim");
	model->setUseVisualizer(false);

	// get a new system and state to reflect additions to the model
	State& state1 = model->initSystem();

	// ------------------- Provide control signals for bodyActuator ----------------
	// Get the default control vector of the model
	Vector modelControls = model->getDefaultControls();

	// Spedicfy a vector of control signals for desired torques and forces
//.........这里部分代码省略.........

void testClutchedPathSpring()
{
	using namespace SimTK;

	// start timing
	std::clock_t startTime = std::clock();

	double mass = 1;
	double stiffness = 100;
	double dissipation = 0.3;
	double start_h = 0.5;
	double ball_radius = 0.25;

	double omega = sqrt(stiffness/mass);

	// Setup OpenSim model
	Model* model = new Model;
	model->setName("ClutchedPathSpringModel");
	model->setGravity(gravity_vec);

	//OpenSim bodies
    OpenSim::Body* ground = &model->getGroundBody();
	
	// body that acts as the pulley that the path wraps over
	OpenSim::Body* pulleyBody =
		new OpenSim::Body("PulleyBody", mass ,Vec3(0),  mass*Inertia::brick(0.1, 0.1, 0.1));
	
	// body the path spring is connected to at both ends
	OpenSim::Body* block =
		new OpenSim::Body("block", mass ,Vec3(0),  mass*Inertia::brick(0.2, 0.1, 0.1));
	block->addDisplayGeometry("box.vtp");
	block->scale(Vec3(0.2, 0.1, 0.1), false);

	double dh = mass*gravity_vec(1)/stiffness;
	
	WrapCylinder* pulley = new WrapCylinder();
	pulley->setRadius(0.1);
	pulley->setLength(0.05);

	// Add the wrap object to the body, which takes ownership of it
	pulleyBody->addWrapObject(pulley);

	// Add joints
	WeldJoint* weld = 
		new WeldJoint("weld", *ground, Vec3(0, 1.0, 0), Vec3(0), *pulleyBody, Vec3(0), Vec3(0));
	
	SliderJoint* slider =
		new SliderJoint("slider", *ground, Vec3(0), Vec3(0,0,Pi/2),*block, Vec3(0), Vec3(0,0,Pi/2));

	double positionRange[2] = {-10, 10};
	// Rename coordinates for a slider joint
	CoordinateSet &slider_coords = slider->upd_CoordinateSet();
	slider_coords[0].setName("block_h");
	slider_coords[0].setRange(positionRange);
	slider_coords[0].setMotionType(Coordinate::Translational);

	model->addBody(pulleyBody);
	model->addJoint(weld);

	model->addBody(block);
	model->addJoint(slider);

	ClutchedPathSpring* spring = 
		new ClutchedPathSpring("clutch_spring", stiffness, dissipation, 0.01);

	spring->updGeometryPath().appendNewPathPoint("origin", *block, Vec3(-0.1, 0.0 ,0.0));
	
	int N = 10;
	for(int i=1; i<N; ++i){
		double angle = i*Pi/N;
		double x = 0.1*cos(angle);
		double y = 0.1*sin(angle);
		spring->updGeometryPath().appendNewPathPoint("", *pulleyBody, Vec3(-x, y ,0.0));
	}

	spring->updGeometryPath().appendNewPathPoint("insertion", *block, Vec3(0.1, 0.0 ,0.0));

	// BUG in defining wrapping API requires that the Force containing the GeometryPath be
	// connected to the model before the wrap can be added
	model->addForce(spring);

	PrescribedController* controller = new PrescribedController();
	controller->addActuator(*spring);
	
	// Control greater than 1 or less than 0 should be treated as 1 and 0 respectively.
	double     timePts[4] = {0.0,  5.0, 6.0, 10.0};
	double clutchOnPts[4] = {1.5, -2.0, 0.5,  0.5};

	PiecewiseConstantFunction* controlfunc = 
		new PiecewiseConstantFunction(4, timePts, clutchOnPts);

	controller->prescribeControlForActuator("clutch_spring", controlfunc);
	model->addController(controller);

	model->print("ClutchedPathSpringModel.osim");

	//Test deserialization
	delete model;
	model = new Model("ClutchedPathSpringModel.osim");

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

int main(int argc, char **argv) {
  cout << "--------------------------------------------------------------------------------" << endl;
  cout << " Multi-Body System Benchmark in OpenSim" << endl;
  cout << " Benchmark reference url: http://lim.ii.udc.es/mbsbenchmark/" << endl;
  cout << " Problem A03: Andrew's Mechanism Model Creator" << endl;
  cout << " Copyright (C) 2013-2015 Luca Tagliapietra, Michele Vivian, Elena Ceseracciu, and Monica Reggiani" << endl;
  cout << "--------------------------------------------------------------------------------" << endl;

  if (argc != 2){
    cout << " ******************************************************************************" << endl;
    cout << " Multi-Body System Benchmark in OpenSim: Creator for Model A03" << endl;
    cout << " Usage: ./AndrewsMechanismCreateModel dataDirectory" << endl;
    cout << "       dataDirectory must contain a vtpFiles folder" << endl;
    cout << " ******************************************************************************" << endl;
    exit(EXIT_FAILURE);
  }

  const std::string dataDir = argv[1];
  cout << "Data directory: " + dataDir << endl;

  OpenSim::Model andrewsMechanism;
  andrewsMechanism.setName("Andrew's Mechanism");
  andrewsMechanism.setAuthors("L.Tagliapietra, M. Vivian, M.Reggiani");

  // Get a reference to the model's ground body
  OpenSim::Body& ground = andrewsMechanism.getGroundBody();
  andrewsMechanism.setGravity(gravityVector);

  //******************************
  // Create OF element
  //******************************
  SimTK::Inertia OFbarInertia(0.1,0.1,OFinertia);
  OpenSim::Body *OF = new OpenSim::Body("OF", OFmass, OFMassCenter, OFbarInertia);

  //Set transformation for visualization pourpose
  SimTK::Rotation rot(SimTK::Pi/2, SimTK::UnitVec3(0,0,1));
  SimTK::Transform trans = SimTK::Transform(rot);

  //Set visualization properties
  OF->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visOF = OF->updDisplayer();
  visOF -> updTransform() =  trans;
  visOF -> setScaleFactors(SimTK::Vec3(0.001,OFlength, 0.001));
  visOF -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  SimTK::Vec3 orientationInParent(0), orientationInBody(0);
  OpenSim::PinJoint *OJoint = new OpenSim::PinJoint("joint_O", ground, SimTK::Vec3(0), orientationInParent, *OF, SimTK::Vec3(-OFlength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& OCoordinateSet = OJoint -> upd_CoordinateSet();
  OCoordinateSet[0].setName("joint_O");
  OCoordinateSet[0].setDefaultValue(OAngleAtZero);
  andrewsMechanism.addBody(OF);

  //********************************
  // Create FE element
  //********************************
  SimTK::Inertia EFbarInertia(0.1,0.1,EFinertia);
  OpenSim::Body *EF = new OpenSim::Body("EF", EFmass, EFMassCenter, EFbarInertia);

  //Set visualization properties
  EF->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visEF = EF->updDisplayer();
  visEF -> updTransform() =  trans;
  visEF -> setScaleFactors(SimTK::Vec3(0.001,EFlength, 0.001));
  visEF -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *FJoint = new OpenSim::PinJoint("joint_F", *OF, SimTK::Vec3(OFlength/2,0,0), orientationInParent, *EF, SimTK::Vec3(EFlength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& FCoordinateSet = FJoint -> upd_CoordinateSet();
  FCoordinateSet[0].setName("joint_F");
  FCoordinateSet[0].setDefaultValue(FAngleAtZero);
  andrewsMechanism.addBody(EF);

  //********************************
  // Create EG element
  //********************************
  SimTK::Inertia GEbarInertia(0.1,0.1,GEinertia);
  OpenSim::Body *GE = new OpenSim::Body("GE", GEmass, GEMassCenter, GEbarInertia);

  //Set visualization properties
  GE->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visGE = GE->updDisplayer();
  visGE -> updTransform() =  trans;
  visGE -> setScaleFactors(SimTK::Vec3(0.001,GElength, 0.001));
  visGE -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *E1Joint = new OpenSim::PinJoint("joint_E1", *EF, SimTK::Vec3(-EFlength/2,0,0), orientationInParent, *GE, SimTK::Vec3(GElength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& E1CoordinateSet = E1Joint -> upd_CoordinateSet();
  E1CoordinateSet[0].setName("joint_E1");
  E1CoordinateSet[0].setDefaultValue(E1AngleAtZero);
  andrewsMechanism.addBody(GE);

  //********************************
  // Create AG element
  //********************************
  SimTK::Inertia AGbarInertia(0.1,0.1,AGinertia);
  OpenSim::Body *AG = new OpenSim::Body("AG", AGmass, AGMassCenter, AGbarInertia);

  //Set visualization properties
  AG->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visAG = AG->updDisplayer();
  visAG -> updTransform() =  trans;
//.........这里部分代码省略.........

/*==============================================================================  
    Main test driver to be used on any muscle model (derived from Muscle) so new 
    cases should be easy to add currently, the test only verifies that the work 
    done by the muscle corresponds to the change in system energy.

    TODO: Test will fail wih prescribe motion until the work done by this 
    constraint is accounted for.
================================================================================
*/
void simulateMuscle(
        const Muscle &aMuscModel, 
        double startX, 
        double act0, 
        const Function *motion,  // prescribe motion of free end of muscle
        const Function *control, // prescribed excitation signal to the muscle
        double integrationAccuracy,
        int testType,
        double testTolerance,
        bool printResults)
{
    string prescribed = (motion == NULL) ? "." : " with Prescribed Motion.";

    cout << "\n******************************************************" << endl;
    cout << "Test " << aMuscModel.getConcreteClassName() 
         << " Model" << prescribed << endl;
    cout << "******************************************************" << endl;
    using SimTK::Vec3;

//==========================================================================
// 0. SIMULATION SETUP: Create the block and ground
//==========================================================================

    // Define the initial and final simulation times
    double initialTime = 0.0;
    double finalTime = 4.0;
    
    //Physical properties of the model
    double ballMass = 10;
    double ballRadius = 0.05;
    double anchorWidth = 0.1;

    // Create an OpenSim model
    Model model;

    double optimalFiberLength = aMuscModel.getOptimalFiberLength();
    double pennationAngle     = aMuscModel.getPennationAngleAtOptimalFiberLength();
    double tendonSlackLength  = aMuscModel.getTendonSlackLength();

    // Use a copy of the muscle model passed in to add path points later
    PathActuator *aMuscle = aMuscModel.clone();

    // Get a reference to the model's ground body
    Body& ground = model.getGroundBody();
    ground.addDisplayGeometry("box.vtp");
    ground.updDisplayer()
        ->setScaleFactors(Vec3(anchorWidth, anchorWidth, 2*anchorWidth));

    OpenSim::Body * ball = new OpenSim::Body("ball", 
                        ballMass , 
                        Vec3(0),  
                        ballMass*SimTK::Inertia::sphere(ballRadius));
    
    ball->addDisplayGeometry("sphere.vtp");
    ball->updDisplayer()->setScaleFactors(Vec3(2*ballRadius));
    // ball connected  to ground via a slider along X
    double xSinG = optimalFiberLength*cos(pennationAngle)+tendonSlackLength;

    SliderJoint* slider = new SliderJoint( "slider", 
                        ground, 
                        Vec3(anchorWidth/2+xSinG, 0, 0), 
                        Vec3(0), 
                        *ball, 
                        Vec3(0), 
                        Vec3(0));

    CoordinateSet& jointCoordinateSet = slider->upd_CoordinateSet();
        jointCoordinateSet[0].setName("tx");
        jointCoordinateSet[0].setDefaultValue(1.0);
        jointCoordinateSet[0].setRangeMin(0); 
        jointCoordinateSet[0].setRangeMax(1.0);
    
    if(motion != NULL){
        jointCoordinateSet[0].setPrescribedFunction(*motion);
        jointCoordinateSet[0].setDefaultIsPrescribed(true);
    }
    // add ball to model
    model.addBody(ball);
	model.addJoint(slider);


//==========================================================================
// 1. SIMULATION SETUP: Add the muscle
//==========================================================================

    //Attach the muscle
    const string &actuatorType = aMuscle->getConcreteClassName();
    aMuscle->setName("muscle");
    aMuscle->addNewPathPoint("muscle-box", ground, Vec3(anchorWidth/2,0,0));
    aMuscle->addNewPathPoint("muscle-ball", *ball, Vec3(-ballRadius,0,0));
    
//.........这里部分代码省略.........

/**
 * Create a model that does nothing. 
 */
int main()
{
	try {

		///////////////////////////////////////////
		// DEFINE BODIES AND JOINTS OF THE MODEL //
		///////////////////////////////////////////

		// Create an OpenSim model and set its name
		Model osimModel;
		osimModel.setName("tugOfWar");

		// GROUND BODY

		// Get a reference to the model's ground body
		OpenSim::Body& ground = osimModel.getGroundBody();

		// Add display geometry to the ground to visualize in the GUI
		ground.addDisplayGeometry("ground.vtp");
		ground.addDisplayGeometry("anchor1.vtp");
		ground.addDisplayGeometry("anchor2.vtp");

		// BLOCK BODY

		// Specify properties of a 20 kg, 0.1 m^3 block body
		double blockMass = 20.0, blockSideLength = 0.1;
		Vec3 blockMassCenter(0);
		Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);

		// Create a new block body with the specified properties
		OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);

		// Add display geometry to the block to visualize in the GUI
		block->addDisplayGeometry("block.vtp");

		// FREE JOINT

		// Create a new free joint with 6 degrees-of-freedom (coordinates) between the block and ground bodies
		Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), locationInBody(0), orientationInBody(0);
		FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, locationInParent, orientationInParent, *block, locationInBody, orientationInBody);
		
		// Get a reference to the coordinate set (6 degrees-of-freedom) between the block and ground bodies
		CoordinateSet& jointCoordinateSet = blockToGround->upd_CoordinateSet();

		// Set the angle and position ranges for the coordinate set
		double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
		double positionRange[2] = {-1, 1};
		jointCoordinateSet[0].setRange(angleRange);
		jointCoordinateSet[1].setRange(angleRange);
		jointCoordinateSet[2].setRange(angleRange);
		jointCoordinateSet[3].setRange(positionRange);
		jointCoordinateSet[4].setRange(positionRange);
		jointCoordinateSet[5].setRange(positionRange);

		// Add the block body to the model
		osimModel.addBody(block);

		///////////////////////////////////////////////
		// DEFINE THE SIMULATION START AND END TIMES //
		///////////////////////////////////////////////

		// Define the initial and final simulation times
		double initialTime = 0.0;
		double finalTime = 3.00;

		/////////////////////////////////////////////
		// DEFINE CONSTRAINTS IMPOSED ON THE MODEL //
		/////////////////////////////////////////////

		// Specify properties of a constant distance constraint to limit the block's motion
		double distance = 0.2;
		Vec3 pointOnGround(0, blockSideLength/2 ,0);
		Vec3 pointOnBlock(0, 0, 0);

		// Create a new constant distance constraint
		ConstantDistanceConstraint *constDist = new ConstantDistanceConstraint(ground, 
										pointOnGround, *block, pointOnBlock, distance);

		// Add the new point on a line constraint to the model
		osimModel.addConstraint(constDist);

		///////////////////////////////////////
		// DEFINE FORCES ACTING ON THE MODEL //
		///////////////////////////////////////

		// GRAVITY
		// Obtaine the default acceleration due to gravity
		Vec3 gravity = osimModel.getGravity();
	

		// MUSCLE FORCES
		// Create two new muscles with identical properties
		double maxIsometricForce = 1000.0, optimalFiberLength = 0.25, tendonSlackLength = 0.1, pennationAngle = 0.0; 
		Thelen2003Muscle *muscle1 = new Thelen2003Muscle("muscle1",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);
		Thelen2003Muscle *muscle2 = new Thelen2003Muscle("muscle2",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);

		// Specify the paths for the two muscles
//.........这里部分代码省略.........

/**
 * Run a simulation of block sliding with contact on by two muscles sliding with contact 
 */
int main()
{
    clock_t startTime = clock();

	try {
		//////////////////////
		// MODEL PARAMETERS //
		//////////////////////

		// Specify body mass of a 20 kg, 0.1m sides of cubed block body
		double blockMass = 20.0, blockSideLength = 0.1;

		// Constant distance of constraint to limit the block's motion
		double constantDistance = 0.2;

		// Contact parameters
		double stiffness = 1.0e7, dissipation = 0.1, friction = 0.2, viscosity=0.01;

		///////////////////////////////////////////
		// DEFINE BODIES AND JOINTS OF THE MODEL //
		///////////////////////////////////////////

		// Create an OpenSim model and set its name
		Model osimModel;
		osimModel.setName("tugOfWar");

		// GROUND BODY
		// Get a reference to the model's ground body
		OpenSim::Body& ground = osimModel.getGroundBody();

		// Add display geometry to the ground to visualize in the Visualizer and GUI
		// add a checkered floor
		ground.addDisplayGeometry("checkered_floor.vtp");
		// add anchors for the muscles to be fixed too
		ground.addDisplayGeometry("block.vtp");
		ground.addDisplayGeometry("block.vtp");

		// block is 0.1 by 0.1 by 0.1m cube and centered at origin. 
		// transform anchors to be placed at the two extremes of the sliding block (to come)
		GeometrySet& geometry = ground.updDisplayer()->updGeometrySet();
		DisplayGeometry& anchor1 = geometry[1];
		DisplayGeometry& anchor2 = geometry[2];
		// scale the anchors
		anchor1.setScaleFactors(Vec3(5, 1, 1));
		anchor2.setScaleFactors(Vec3(5, 1, 1));
		// reposition the anchors
		anchor1.setTransform(Transform(Vec3(0, 0.05, 0.35)));
		anchor2.setTransform(Transform(Vec3(0, 0.05, -0.35)));

		// BLOCK BODY
		Vec3 blockMassCenter(0);
		Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);

		// Create a new block body with the specified properties
		OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);

		// Add display geometry to the block to visualize in the GUI
		block->addDisplayGeometry("block.vtp");

		// FREE JOINT

		// Create a new free joint with 6 degrees-of-freedom (coordinates) between the block and ground bodies
		Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), locationInBody(0), orientationInBody(0);
		FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, locationInParent, orientationInParent, *block, locationInBody, orientationInBody);
		
		// Get a reference to the coordinate set (6 degrees-of-freedom) between the block and ground bodies
		CoordinateSet& jointCoordinateSet = blockToGround->upd_CoordinateSet();

		// Set the angle and position ranges for the coordinate set
		double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
		double positionRange[2] = {-1, 1};
		jointCoordinateSet[0].setRange(angleRange);
		jointCoordinateSet[1].setRange(angleRange);
		jointCoordinateSet[2].setRange(angleRange);
		jointCoordinateSet[3].setRange(positionRange);
		jointCoordinateSet[4].setRange(positionRange);
		jointCoordinateSet[5].setRange(positionRange);

		// GRAVITY
		// Obtaine the default acceleration due to gravity
		Vec3 gravity = osimModel.getGravity();

		// Define non-zero default states for the free joint
		jointCoordinateSet[3].setDefaultValue(constantDistance); // set x-translation value
		double h_start = blockMass*gravity[1]/(stiffness*blockSideLength*blockSideLength);
		jointCoordinateSet[4].setDefaultValue(h_start); // set y-translation which is height

		// Add the block and joint to the model
		osimModel.addBody(block);
		osimModel.addJoint(blockToGround);

		///////////////////////////////////////////////
		// DEFINE THE SIMULATION START AND END TIMES //
		///////////////////////////////////////////////

		// Define the initial and final simulation times
		double initialTime = 0.0;
//.........这里部分代码省略.........