simtk.org

Code: Select all

const ContactForce& force = m_contactForces.getContactForce(state, i)

Code: Select all

ForceSet forces = _model->getForceSet();
OpenSim::Force& contact = forces.get(3);  // the elasticFoundationForce is in index 3
Array<std::string> Fn;
Array<double> F;
Fn = contact.getRecordLabels(s);
F = contact.getRecordValues(s);

Code: Select all

class PDController2 : public Controller
{
	OpenSim_DECLARE_CONCRETE_OBJECT(PDController2, Controller);

private:
	double kp;
	double kv;

public:

	PDController2::PDController2() : Controller() {
		setKp(100);
		setKv(20);
	}

	Array<double> PDController2::ForceValue(const State& s) const {
		ForceSet forces = _model->getForceSet();
		OpenSim::Force& contact = forces.get(3);

		Array<double> F;
		_model->getMultibodySystem().realize(s, SimTK::Stage::Dynamics);
		F = contact.getRecordValues(s);
		return F;
	};
	
	//_____________________________________________________________________________
	void PDController2::computeControls(const SimTK::State& s, SimTK::Vector &controls) const
	{
		double t = s.getTime();

		Array<double> contactForce = ForceValue(s);
		std::cout << "print force value:  " << contactForce << std::endl;
		
		double control_1 =0.5;
		double control_2 = 0.5;

		Vec2 control(control_1, control_2);

		Vector control1(1, control[0]);
		Vector control2(1, control[1]);

		j1_act.addInControls(control1, controls);
		j2_act.addInControls(control2, controls);

	}
};

int main()
{
	try {
		// Create an OpenSim model and set its name
		Model osimModel("TwoLinkArmModel.osim");
		osimModel.setUseVisualizer(true);
		
		PDController2 *controller = new PDController2();
		controller->setName("my_controller");
		controller->setActuators(osimModel.updActuators());
		osimModel.addController(controller);

		State& s = osimModel.initSystem();

		std::cout << "System Initialized" << std::endl;

		std::cout << "getMultibodySystem Done" << std::endl;

		SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
		integrator.setAccuracy(1.0e-3);
		Manager manager(osimModel, integrator);

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

		osimModel.updMatterSubsystem().setShowDefaultGeometry(true);
		Visualizer& viz = osimModel.updVisualizer().updSimbodyVisualizer();

		// Integrate from initial time to final time
		manager.setInitialTime(initialTime);
		manager.setFinalTime(finalTime);
		std::cout << "\n\nIntegrating from " << initialTime << " to " << finalTime << std::endl;
		manager.integrate(s);

	}
	catch (OpenSim::Exception ex)
	{
		std::cout << ex.getMessage() << std::endl;
		return 1;
	}
	catch (std::exception ex)
	{
		std::cout << ex.what() << std::endl;
		return 1;
	}
	catch (...)
	{
		std::cout << "UNRECOGNIZED EXCEPTION" << std::endl;
		return 1;
	}

	std::cout << "OpenSim example completed successfully.\n";
	std::cin.get();
	return 0;
}