Inverse Kinematics Solver C++
Posted: Tue Jul 20, 2021 9:53 am
Hello, I am trying to use the Inverse Kinematics solver through the c++ API and I am wondering how to go about doing so since I am not an expert at c++ and I could not find any examples that use the Inverse Kinematics Solver.
Essentially, what I want to do is
1) Setup the solver with osim model and marker weights
2) Input the experimental markers 1 frame at a time (not altogether using a .trc file)
3) Solve for and get the optimized coordinates obtained from the solver
I have written some of the code but there may be missing/incorrect steps
Essentially, what I want to do is
1) Setup the solver with osim model and marker weights
2) Input the experimental markers 1 frame at a time (not altogether using a .trc file)
3) Solve for and get the optimized coordinates obtained from the solver
I have written some of the code but there may be missing/incorrect steps
Code: Select all
#include <OpenSim/OpenSim.h>
#include "OpenSim/Common/STOFileAdapter.h"
#include <ctime> // clock(), clock_t, CLOCKS_PER_SEC
#include "IK_ID.h"
using namespace OpenSim;
using namespace SimTK;
using namespace std;
int main()
{
// Create a new OpenSim model.
Model osimModel("C:/Users/sanke/Documents/gatech/RoboticsResearch/Arm_IK_ID3.osim");
//set for marker weights
Set<MarkerWeight> *marker_weights = new Set<MarkerWeight>();
//marker trc file
std::string marker_file("C:/Users/sanke/Documents/gatech/RoboticsResearch/OpenSim/4.1/Models/UpperArmPilot9/Session1/trial_feedback03.trc");
//markers reference
MarkersReference markers_experimental(marker_file, *marker_weights, Units(Units::Meters));
//marker weights
Set<MarkerWeight> marker_weights_exp = markers_experimental.upd_marker_weights();
marker_weights_exp.insert(0, MarkerWeight("RSHO", 1));
marker_weights_exp.insert(1, MarkerWeight("RELB", 1));
marker_weights_exp.insert(2, MarkerWeight("RELA", 1));
marker_weights_exp.insert(3, MarkerWeight("RUPA", 1));
marker_weights_exp.insert(4, MarkerWeight("RWRB", 1));
marker_weights_exp.insert(5, MarkerWeight("RWRA", 1));
marker_weights_exp.insert(6, MarkerWeight("RFRM", 1));
//coordinate reference (should be empty)
SimTK::Array_< CoordinateReference > coordinate_reference;
//Create solver
InverseKinematicsSolver solver(osimModel, markers_experimental, coordinate_reference);
//initialize system
SimTK::State si = osimModel.initSystem();
//solve
solver.assemble(si);
solver.track(si);
// End of main() routine.
return 0;
}