int
main(int argc, char** argv)
{
if (argc < 2)
{
std::cout << "Usage: rlJacobianDemo KINEMATICSFILE Q1 ... Qn QD1 ... QDn" << std::endl;
return 1;
}
try
{
boost::shared_ptr< rl::kin::Kinematics > kinematics(rl::kin::Kinematics::create(argv[1]));
rl::math::Vector q(kinematics->getDof());
for (std::ptrdiff_t i = 0; i < q.size(); ++i)
{
q(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2]);
}
rl::math::Vector qdot(kinematics->getDof());
for (std::ptrdiff_t i = 0; i < qdot.size(); ++i)
{
qdot(i) = boost::lexical_cast< rl::math::Real >(argv[q.size() + i + 2]);
}
kinematics->setPosition(q);
kinematics->updateFrames();
kinematics->updateJacobian();
std::cout << "J=" << std::endl << kinematics->getJacobian() << std::endl;
rl::math::Vector xdot(kinematics->getOperationalDof() * 6);
kinematics->forwardVelocity(qdot, xdot);
std::cout << "xdot=" << std::endl;
for (std::size_t i = 0; i < kinematics->getOperationalDof(); ++i)
{
std::cout << i << " " << xdot.transpose() << std::endl;
}
kinematics->updateJacobianInverse(1.0e-9f);
std::cout << "invJ=" << std::endl << kinematics->getJacobianInverse() << std::endl;
kinematics->inverseVelocity(xdot, qdot);
std::cout << "qdot=" << std::endl << qdot.transpose() << std::endl;
}
catch (const std::exception& e)
{
std::cout << e.what() << std::endl;
return 1;
}
return 0;
}
void GeneralisedCoordinatesFromStorageFile::updateKinematicsSplines(double fc) {
//reinitialise because I modify the storage with padding and filtering..
OpenSim::Storage kinematics(kinematicsStorageFilename_);
if (fc > 0) {
kinematics.pad(kinematics.getSize() / 2.);
kinematics.lowpassIIR(fc);
}
if (kinematics.isInDegrees()){
model_.getSimbodyEngine().convertDegreesToRadians(kinematics);
}
// Create differentiable splines of the coordinate data
OpenSim::GCVSplineSet kinematicsSplines(5, &kinematics);
//Functions must correspond to model coordinates and their order for the solver
OpenSim::Array<string> coordinateNamesOS;
model_.getCoordinateSet().getNames(coordinateNamesOS);
for (size_t i(0); i < coordinateNamesOS.getSize(); ++i){
if (kinematicsSplines.contains(coordinateNamesOS.get(i))){
kinematicsSplines.insert(i, kinematicsSplines.get(coordinateNamesOS.get(i)));
}
else{
kinematicsSplines.insert(i, OpenSim::Constant(model_.getCoordinateSet().get(i).getDefaultValue()));
}
}
if (kinematicsSplines.getSize() > coordinateNamesOS.getSize()){
kinematicsSplines.setSize(coordinateNamesOS.getSize());
}
splines_ = kinematicsSplines;
}
double CTRCalibration::ComputeErrorOnValidationSet(double& max_error, int& max_ID)
{
CTR* robot = CTRFactory::buildCTR("");
SetParamsToCTR(robot, m_params);
MechanicsBasedKinematics kinematics(robot, m_numOfGridPoints);
kinematics.ActivateIVPJacobian();
double error = 0.0;
double tmp = 0.0;
double max_error_current = 0.0;
int counter = 0;
//for (int i = 0; i < m_validationSet.size(); i++)
for (int i = m_validationSet.size() -1; i >=0 ; i--)
{
tmp = ComputeSingleShapeError(robot, &kinematics, m_validationSet[i], max_error_current);
if (tmp < 0)
continue;
else
{
error += tmp;
counter++;
//::std::cout << i << "-th conf: " << tmp << ::std::endl;
::Eigen::Vector3d position;
kinematics.GetTipPosition(position);
m_stream_val << i << "\t" << tmp << "\t" << position.transpose() << ::std::endl;
}
if (max_error_current > max_error)
{
max_error = max_error_current;
max_ID = i;
//::std::cout << "id: " << i << ", conf: ";
//for (int k = 0 ; k < 5; k++)
// ::std::cout << dataset[i].GetConfiguration()[k] << " ";
//::std::cout << ::std::endl;
}
}
delete robot;
return ::std::sqrt(error/counter);
}
GeneralisedCoordinatesFromStorageFile::GeneralisedCoordinatesFromStorageFile(
GeneralisedCoordinatesQueue& outputGeneralisedCoordinatesQueue,
rtb::Concurrency::Latch& doneWithSubscriptions,
rtb::Concurrency::Latch& doneWithExecution,
const std::string& osimModelFilename,
const std::string& kinematicsStorageFilename,
double fc) :
GeneralisedCoordinatesFromX(outputGeneralisedCoordinatesQueue, doneWithSubscriptions, doneWithExecution),
model_(osimModelFilename),
kinematicsStorageFilename_(kinematicsStorageFilename) {
//for some mysterious reason, if I don't use c_str it doesn't work..
OpenSim::Storage kinematics(kinematicsStorageFilename_.c_str());
OpenSim::Array<double> timeColumnOS;
kinematics.getTimeColumn(timeColumnOS);
ArrayConverter::toStdVector(timeColumnOS, timeColumn_);
updateKinematicsSplines(fc);
OpenSim::Array<string> coordinateNamesOS;
model_.getCoordinateSet().getNames(coordinateNamesOS);
ArrayConverter::toStdVector(coordinateNamesOS, coordinateNamesFromOsimModel_);
mapper_.setNames(coordinateNamesFromOsimModel_, coordinateNamesFromOsimModel_);
}
void run()
{
// load model and data
OpenSim::Model model(m_model_path);
OpenSim::MarkerData imu_trc(m_imu_path);
SimTK::State& state = model.initSystem();
// setup
SimTK::Assembler ik(model.updMultibodySystem());
ik.setAccuracy(1e-5);
SimTK::Markers* markers = new SimTK::Markers();
SimTK::OrientationSensors* imus = new SimTK::OrientationSensors();
// add markers
addCustomMarkers(model, *markers, *imus);
// result storage
OpenSim::Kinematics kinematics(&model);
kinematics.setRecordAccelerations(true);
// move to initial target
ik.adoptAssemblyGoal(imus);
imus->moveOneObservation(m_humerus_ox, SimTK::Rotation(
SimTK::BodyOrSpaceType::BodyRotationSequence,
imu_trc.getFrame(0).getMarker(0)[0], SimTK::XAxis,
imu_trc.getFrame(0).getMarker(0)[1], SimTK::YAxis,
imu_trc.getFrame(0).getMarker(0)[2], SimTK::ZAxis));
imus->moveOneObservation(m_radius_ox, SimTK::Rotation(
SimTK::BodyOrSpaceType::BodyRotationSequence,
imu_trc.getFrame(0).getMarker(1)[0], SimTK::XAxis,
imu_trc.getFrame(0).getMarker(1)[1], SimTK::YAxis,
imu_trc.getFrame(0).getMarker(1)[2], SimTK::ZAxis));
// setup inverse kinematics
state.setTime(imu_trc.getFrame(0).getFrameTime());
ik.initialize(state);
ik.assemble(state);
kinematics.begin(state);
// loop for every observation frame (!!!must be same length)
for (int i = 1; i < imu_trc.getNumFrames(); ++i)
{
OpenSim::MarkerFrame osensor_frame = imu_trc.getFrame(i);
SimTK::Vec3 humerus_vec = osensor_frame.getMarker(0);
imus->moveOneObservation(m_humerus_ox, SimTK::Rotation(
SimTK::BodyOrSpaceType::BodyRotationSequence,
humerus_vec[0], SimTK::XAxis,
humerus_vec[1], SimTK::YAxis,
humerus_vec[2], SimTK::ZAxis));
SimTK::Vec3 radius_vec = osensor_frame.getMarker(1);
imus->moveOneObservation(m_radius_ox, SimTK::Rotation(
SimTK::BodyOrSpaceType::BodyRotationSequence,
radius_vec[0], SimTK::XAxis,
radius_vec[1], SimTK::YAxis,
radius_vec[2], SimTK::ZAxis));
// track
state.updTime() = osensor_frame.getFrameTime();
ik.track(state.getTime());
ik.updateFromInternalState(state);
// report
#ifdef DEBUG
std::cout << "Frame: " << i << " (t=" << state.getTime() << ")\n";
std::cout << "Error: " << ik.calcCurrentErrorNorm() << "\n";
std::flush(std::cout);
#endif
// store
kinematics.step(state, i);
}
kinematics.end(state);
// store results
kinematics.printResults(m_ik_result_path, "");
}
int
main(int argc, char** argv)
{
if (argc < 3)
{
std::cout << "testPrm SCENEFILE KINEMATICSFILE START1 ... STARTn GOAL1 ... GOALn" << std::endl;
return 1;
}
rl::sg::solid::Scene scene;
scene.load(argv[1]);
::boost::shared_ptr< ::rl::kin::Kinematics > kinematics(::rl::kin::Kinematics::create(argv[2]));
rl::plan::SimpleModel model;
model.kinematics = kinematics.get();
model.model = scene.getModel(0);
model.scene = &scene;
rl::plan::Prm planner;
rl::plan::UniformSampler sampler;
rl::plan::RecursiveVerifier verifier;
planner.model = &model;
planner.sampler = &sampler;
planner.verifier = &verifier;
sampler.model = &model;
verifier.delta = 1.0f * rl::math::DEG2RAD;
verifier.model = &model;
rl::math::Vector start(kinematics->getDof());
for (std::size_t i = 0; i < kinematics->getDof(); ++i)
{
start(i) = boost::lexical_cast< rl::math::Real >(argv[i + 3]);
}
planner.start = &start;
rl::math::Vector goal(kinematics->getDof());
for (std::size_t i = 0; i < kinematics->getDof(); ++i)
{
goal(i) = boost::lexical_cast< rl::math::Real >(argv[kinematics->getDof() + i + 3]);
}
planner.goal = &goal;
rl::util::Timer timer;
std::cout << "construct() ... " << std::endl;;
timer.start();
planner.construct(15);
timer.stop();
std::cout << "construct() " << timer.elapsed() * 1000.0f << " ms" << std::endl;
std::cout << "solve() ... " << std::endl;;
timer.start();
bool solved = planner.solve();
timer.stop();
std::cout << "solve() " << (solved ? "true" : "false") << " " << timer.elapsed() * 1000.0f << " ms" << std::endl;
return 0;
}
int
main(int argc, char** argv)
{
if (argc < 3)
{
std::cout << "Usage: rlRrtDemo SCENEFILE KINEMATICSFILE START1 ... STARTn GOAL1 ... GOALn" << std::endl;
return EXIT_FAILURE;
}
try
{
rl::sg::XmlFactory factory;
#if defined(RL_SG_SOLID)
rl::sg::solid::Scene scene;
#elif defined(RL_SG_BULLET)
rl::sg::bullet::Scene scene;
#elif defined(RL_SG_ODE)
rl::sg::ode::Scene scene;
#endif
factory.load(argv[1], &scene);
std::shared_ptr<rl::kin::Kinematics> kinematics(rl::kin::Kinematics::create(argv[2]));
rl::plan::SimpleModel model;
model.kin = kinematics.get();
model.model = scene.getModel(0);
model.scene = &scene;
rl::plan::KdtreeNearestNeighbors nearestNeighbors0(&model);
rl::plan::KdtreeNearestNeighbors nearestNeighbors1(&model);
rl::plan::RrtConCon planner;
rl::plan::UniformSampler sampler;
planner.model = &model;
planner.setNearestNeighbors(&nearestNeighbors0, 0);
planner.setNearestNeighbors(&nearestNeighbors1, 1);
planner.sampler = &sampler;
sampler.model = &model;
planner.delta = 1 * rl::math::DEG2RAD;
rl::math::Vector start(kinematics->getDof());
for (std::size_t i = 0; i < kinematics->getDof(); ++i)
{
start(i) = boost::lexical_cast<rl::math::Real>(argv[i + 3]) * rl::math::DEG2RAD;
}
planner.start = &start;
rl::math::Vector goal(kinematics->getDof());
for (std::size_t i = 0; i < kinematics->getDof(); ++i)
{
goal(i) = boost::lexical_cast<rl::math::Real>(argv[kinematics->getDof() + i + 3]) * rl::math::DEG2RAD;
}
planner.goal = &goal;
std::cout << "start: " << start.transpose() * rl::math::RAD2DEG << std::endl;;
std::cout << "goal: " << goal.transpose() * rl::math::RAD2DEG << std::endl;;
std::cout << "solve() ... " << std::endl;;
std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now();
bool solved = planner.solve();
std::chrono::steady_clock::time_point stopTime = std::chrono::steady_clock::now();
std::cout << "solve() " << (solved ? "true" : "false") << " " << std::chrono::duration_cast<std::chrono::duration<double>>(stopTime - startTime).count() * 1000 << " ms" << std::endl;
return EXIT_SUCCESS;
}
catch (const std::exception& e)
{
std::cerr << e.what() << std::endl;
return EXIT_FAILURE;
}
}
/*
* This is the constraint that makes sure we hit the ball
*/
void kinematics_eq_constr(unsigned m, double *result, unsigned n, const double *x, double *grad, void *data) {
static double ballPred[CART];
static double racketDesVel[CART];
static double racketDesNormal[CART];
static Matrix link_pos_des;
static Matrix joint_cog_mpos_des;
static Matrix joint_origin_pos_des;
static Matrix joint_axis_pos_des;
static Matrix Alink_des[N_LINKS+1];
static Matrix racketTransform;
static Matrix Jacobi;
static Vector qfdot;
static Vector xfdot;
static Vector normal;
static int firsttime = TRUE;
double T = x[2*DOF];
int N = T/TSTEP;
int i;
/* initialization of static variables */
if (firsttime) {
firsttime = FALSE;
link_pos_des = my_matrix(1,N_LINKS,1,3);
joint_cog_mpos_des = my_matrix(1,N_DOFS,1,3);
joint_origin_pos_des = my_matrix(1,N_DOFS,1,3);
joint_axis_pos_des = my_matrix(1,N_DOFS,1,3);
Jacobi = my_matrix(1,2*CART,1,N_DOFS);
racketTransform = my_matrix(1,4,1,4);
qfdot = my_vector(1,DOF);
xfdot = my_vector(1,2*CART);
normal = my_vector(1,CART);
for (i = 0; i <= N_LINKS; ++i)
Alink_des[i] = my_matrix(1,4,1,4);
// initialize the base variables
//taken from ParameterPool.cf
bzero((void *) &base_state, sizeof(base_state));
bzero((void *) &base_orient, sizeof(base_orient));
base_orient.q[_Q1_] = 1.0;
// the the default endeffector parameters
setDefaultEndeffector();
// homogeneous transform instead of using quaternions
racketTransform[1][1] = 1;
racketTransform[2][3] = 1;
racketTransform[3][2] = -1;
racketTransform[4][4] = 1;
}
if (grad) {
// compute gradient of kinematics = jacobian
//TODO:
grad[0] = 0.0;
grad[1] = 0.0;
grad[2] = 0.0;
}
// interpolate at time T to get the desired racket parameters
first_order_hold(ballPred,racketDesVel,racketDesNormal,T);
// extract state information from array to joint_des_state structure
for (i = 1; i <= DOF; i++) {
joint_des_state[i].th = x[i-1];
joint_des_state[i].thd = qfdot[i] = x[i-1+DOF];
}
/* compute the desired link positions */
kinematics(joint_des_state, &base_state, &base_orient, endeff,
joint_cog_mpos_des, joint_axis_pos_des, joint_origin_pos_des,
link_pos_des, Alink_des);
/* compute the racket normal */
mat_mult(Alink_des[6],racketTransform,Alink_des[6]);
for (i = 1; i <= CART; i++) {
normal[i] = Alink_des[6][i][3];
}
/* compute the jacobian */
jacobian(link_pos_des, joint_origin_pos_des, joint_axis_pos_des, Jacobi);
mat_vec_mult(Jacobi, qfdot, xfdot);
/* deviations from the desired racket frame */
for (i = 1; i <= CART; i++) {
//printf("xfdot[%d] = %.4f, racketDesVel[%d] = %.4f\n",i,xfdot[i],i,racketDesVel[i-1]);
//printf("normal[%d] = %.4f, racketDesNormal[%d] = %.4f\n",i,normal[i],i,racketDesNormal[i-1]);
result[i-1] = link_pos_des[6][i] - ballPred[i-1];
result[i-1 + CART] = xfdot[i] - racketDesVel[i-1];
result[i-1 + 2*CART] = normal[i] - racketDesNormal[i-1];
}
}
