diff --git a/addons/benchmark/SampleData.h b/addons/benchmark/SampleData.h index 3bc75c9..e4a45e9 100644 --- a/addons/benchmark/SampleData.h +++ b/addons/benchmark/SampleData.h @@ -15,6 +15,7 @@ struct SampleData { qdot = new RigidBodyDynamics::Math::VectorNd[count]; qddot = new RigidBodyDynamics::Math::VectorNd[count]; tau = new RigidBodyDynamics::Math::VectorNd[count]; + durations = data.durations; for (int si = 0; si < count; si++) { q[si] = data.q[si]; @@ -36,25 +37,32 @@ struct SampleData { RigidBodyDynamics::Math::VectorNd *qdot; RigidBodyDynamics::Math::VectorNd *qddot; RigidBodyDynamics::Math::VectorNd *tau; + RigidBodyDynamics::Math::VectorNd durations; void deleteData() { count = 0; - if (q) + if (q) { delete[] q; + } q = NULL; - if (qdot) + if (qdot) { delete[] qdot; + } qdot = NULL; - if (qddot) + if (qddot) { delete[] qddot; + } qddot = NULL; - if (tau) + if (tau) { delete[] tau; + } tau = NULL; + + durations.resize(0); } void fillRandom (int dof_count, int sample_count) { @@ -79,6 +87,8 @@ struct SampleData { tau[si][i] = (static_cast(rand()) / static_cast(RAND_MAX)) * 2. -1.; } } + + durations = RigidBodyDynamics::Math::VectorNd::Zero(count); } }; diff --git a/addons/benchmark/benchmark.cc b/addons/benchmark/benchmark.cc index 12002ba..519682a 100644 --- a/addons/benchmark/benchmark.cc +++ b/addons/benchmark/benchmark.cc @@ -6,6 +6,7 @@ #include #include #include +#include #include "rbdl/rbdl.h" #include "model_generator.h" @@ -41,40 +42,129 @@ bool benchmark_run_id_rnea = true; bool benchmark_run_crba = true; bool benchmark_run_nle = true; bool benchmark_run_calc_minv_times_tau = true; -bool benchmark_run_contacts = false; +bool benchmark_run_contacts = true; +bool benchmark_run_ik = true; -string model_file = ""; +bool json_output = false; + +string model_name; enum ContactsMethod { - ContactsMethodLagrangian = 0, - ContactsMethodRangeSpaceSparse, - ContactsMethodNullSpace, - ContactsMethodKokkevis + ConstraintsMethodDirect = 0, + ConstraintsMethodRangeSpaceSparse, + ConstraintsMethodNullSpace, + ConstraintsMethodKokkevis }; +struct BenchmarkRun { + string model_name; + int model_dof; + string benchmark; + int sample_count; + + double duration; + double avg; + double min; + double max; +}; + +vector benchmark_runs; + +void report_section (const char* section_name) { + if (!json_output) { + cout << "= " << section_name << " =" << endl; + } +} + +void register_run(const Model &model, const SampleData &data, const char *run_name) { + BenchmarkRun run; + run.benchmark = run_name; + run.model_name = model_name; + run.model_dof = model.dof_count; + run.sample_count = data.count; + + run.duration = data.durations.sum(); + run.avg = data.durations.mean(); + run.min = data.durations.minCoeff(); + run.max = data.durations.maxCoeff(); + + benchmark_runs.push_back(run); +} + +void report_run(const Model &model, const SampleData &data, + const char *run_name) { + register_run(model, data, run_name); + + if (!json_output) { + cout << "#DOF: " << setw(3) << model.dof_count + << " #samples: " << data.count + << " duration = " << setw(10) << data.durations.sum() << "(s)" + << " (~" << setw(10) << data.durations.mean() << "(s) per call)" << endl; + } +} + +void report_constraints_run(const Model &model, const SampleData &data, + const char *run_name) { + register_run(model, data, run_name); + + if (!json_output) { + cout << model_name << ": " + << " duration = " << setw(10) << data.durations.sum() << "(s)" + << " (~" << setw(10) << data.durations.mean() << "(s) per call)" << endl; + } +} + +/** Parses /proc/cpuinfo for the CPU model name. */ +string get_cpu_model_name () { + ifstream proc_cpu ("/proc/cpuinfo", ios_base::in); + if (!proc_cpu) { + cerr << "Cannot determine cpu model: could not open /proc/cpuinfo for reading." << endl; + abort(); + } + ostringstream content; + content << proc_cpu.rdbuf(); + proc_cpu.close(); + + string content_str = content.str(); + std::size_t model_name_pos = content_str.find("model name"); + if (model_name_pos != string::npos) { + std::size_t start = content_str.find(':', model_name_pos + strlen("model name")) + 2; + std::size_t end = content_str.find('\n', model_name_pos + strlen("model name")); + + return content_str.substr(start, end - start); + } + return "unknown"; +} + +string get_utc_time_string () { + time_t current_time; + struct tm* timeinfo; + time(¤t_time); + timeinfo = gmtime(¤t_time); + char time_buf[80]; + std::size_t time_len = strftime(&time_buf[0], 80, "%a %b %d %T %Y", timeinfo); + return time_buf; +} + double run_forward_dynamics_ABA_benchmark (Model *model, int sample_count) { SampleData sample_data; sample_data.fillRandom(model->dof_count, sample_count); TimerInfo tinfo; - timer_start (&tinfo); for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); ForwardDynamics (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], sample_data.qddot[i]); + sample_data.durations[i] = timer_stop (&tinfo); } - double duration = timer_stop (&tinfo); - - cout << "#DOF: " << setw(3) << model->dof_count - << " #samples: " << sample_count - << " duration = " << setw(10) << duration << "(s)" - << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + report_run(*model, sample_data, "ForwardDynamics"); - return duration; + return sample_data.durations.sum(); } double run_forward_dynamics_lagrangian_benchmark (Model *model, int sample_count) { @@ -82,12 +172,12 @@ double run_forward_dynamics_lagrangian_benchmark (Model *model, int sample_count sample_data.fillRandom(model->dof_count, sample_count); TimerInfo tinfo; - timer_start (&tinfo); MatrixNd H (MatrixNd::Zero(model->dof_count, model->dof_count)); VectorNd C (VectorNd::Zero(model->dof_count)); for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); ForwardDynamicsLagrangian (*model, sample_data.q[i], sample_data.qdot[i], @@ -98,16 +188,12 @@ double run_forward_dynamics_lagrangian_benchmark (Model *model, int sample_count &H, &C ); + sample_data.durations[i] = timer_stop (&tinfo); } - double duration = timer_stop (&tinfo); - - cout << "#DOF: " << setw(3) << model->dof_count - << " #samples: " << sample_count - << " duration = " << setw(10) << duration << "(s)" - << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + report_run(*model, sample_data, "ForwardDynamicsLagrangian_PivLU"); - return duration; + return sample_data.durations.sum(); } double run_inverse_dynamics_RNEA_benchmark (Model *model, int sample_count) { @@ -118,22 +204,19 @@ double run_inverse_dynamics_RNEA_benchmark (Model *model, int sample_count) { timer_start (&tinfo); for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); InverseDynamics (*model, sample_data.q[i], sample_data.qdot[i], sample_data.qddot[i], sample_data.tau[i] ); + sample_data.durations[i] = timer_stop (&tinfo); } - double duration = timer_stop (&tinfo); - - cout << "#DOF: " << setw(3) << model->dof_count - << " #samples: " << sample_count - << " duration = " << setw(10) << duration << "(s)" - << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + report_run(*model, sample_data, "InverseDynamics"); - return duration; + return sample_data.durations.sum(); } double run_CRBA_benchmark (Model *model, int sample_count) { @@ -145,20 +228,16 @@ double run_CRBA_benchmark (Model *model, int sample_count) { Math::MatrixNd Hinv = Math::MatrixNd::Zero(model->dof_count, model->dof_count); TimerInfo tinfo; - timer_start (&tinfo); for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); CompositeRigidBodyAlgorithm (*model, sample_data.q[i], H, true); + sample_data.durations[i] = timer_stop (&tinfo); } - double duration = timer_stop (&tinfo); - - cout << "#DOF: " << setw(3) << model->dof_count - << " #samples: " << sample_count - << " duration = " << setw(10) << duration << "(s)" - << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + report_run(*model, sample_data, "CompositeRigidBodyAlgorithm"); - return duration; + return sample_data.durations.sum(); } double run_nle_benchmark (Model *model, int sample_count) { @@ -166,24 +245,20 @@ double run_nle_benchmark (Model *model, int sample_count) { sample_data.fillRandom(model->dof_count, sample_count); TimerInfo tinfo; - timer_start (&tinfo); for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); NonlinearEffects (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i] ); + sample_data.durations[i] = timer_stop (&tinfo); } - double duration = timer_stop (&tinfo); - - cout << "#DOF: " << setw(3) << model->dof_count - << " #samples: " << sample_count - << " duration = " << setw(10) << duration << "(s)" - << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + report_run(*model, sample_data, "NonlinearEffects"); - return duration; + return sample_data.durations.sum(); } double run_calc_minv_times_tau_benchmark (Model *model, int sample_count) { @@ -193,18 +268,32 @@ double run_calc_minv_times_tau_benchmark (Model *model, int sample_count) { CalcMInvTimesTau (*model, sample_data.q[0], sample_data.tau[0], sample_data.qddot[0]); TimerInfo tinfo; - timer_start (&tinfo); for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); CalcMInvTimesTau (*model, sample_data.q[i], sample_data.tau[i], sample_data.qddot[i]); + sample_data.durations[i] = timer_stop (&tinfo); } - double duration = timer_stop (&tinfo); + report_run(*model, sample_data, "NonlinearEffects"); - cout << "#DOF: " << setw(3) << model->dof_count - << " #samples: " << sample_count - << " duration = " << setw(10) << duration << "(s)" - << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + return sample_data.durations.sum(); +} + +double run_inverse_dynamics_constraints_benchmark (Model *model, ConstraintSet *constraint_set, std::vector &dofActuated, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + VectorNd qddot = VectorNd::Zero(model->dof_count); + TimerInfo tinfo; + timer_start (&tinfo); + + constraint_set->SetActuationMap(*model, dofActuated); + + for (int i = 0; i < sample_count; i++) { + InverseDynamicsConstraintsRelaxed (*model, sample_data.q[i], sample_data.qdot[i], sample_data.qddot[i], *constraint_set, qddot, sample_data.tau[i]); + } + + double duration = timer_stop (&tinfo); return duration; } @@ -214,15 +303,16 @@ double run_contacts_lagrangian_benchmark (Model *model, ConstraintSet *constrain sample_data.fillRandom(model->dof_count, sample_count); TimerInfo tinfo; - timer_start (&tinfo); for (int i = 0; i < sample_count; i++) { - ForwardDynamicsConstraintsDirect (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + timer_start (&tinfo); + ForwardDynamicsConstraintsDirect (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + sample_data.durations[i] = timer_stop (&tinfo); } - double duration = timer_stop (&tinfo); + report_constraints_run(*model, sample_data, "ForwardDynamicsConstraintsDirect"); - return duration; + return sample_data.durations.sum(); } double run_contacts_lagrangian_sparse_benchmark (Model *model, ConstraintSet *constraint_set, int sample_count) { @@ -230,15 +320,16 @@ double run_contacts_lagrangian_sparse_benchmark (Model *model, ConstraintSet *co sample_data.fillRandom(model->dof_count, sample_count); TimerInfo tinfo; - timer_start (&tinfo); for (int i = 0; i < sample_count; i++) { - ForwardDynamicsConstraintsRangeSpaceSparse (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + timer_start (&tinfo); + ForwardDynamicsConstraintsRangeSpaceSparse (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + sample_data.durations[i] = timer_stop (&tinfo); } - double duration = timer_stop (&tinfo); + report_constraints_run(*model, sample_data, "ForwardDynamicsConstraintsRangeSpaceSparse"); - return duration; + return sample_data.durations.sum(); } double run_contacts_null_space (Model *model, ConstraintSet *constraint_set, int sample_count) { @@ -246,15 +337,16 @@ double run_contacts_null_space (Model *model, ConstraintSet *constraint_set, int sample_data.fillRandom(model->dof_count, sample_count); TimerInfo tinfo; - timer_start (&tinfo); for (int i = 0; i < sample_count; i++) { - ForwardDynamicsConstraintsNullSpace (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + timer_start (&tinfo); + ForwardDynamicsConstraintsNullSpace (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + sample_data.durations[i] = timer_stop (&tinfo); } - double duration = timer_stop (&tinfo); + report_constraints_run(*model, sample_data, "ForwardDynamicsConstraintsNullSpace"); - return duration; + return sample_data.durations.sum(); } double run_contacts_kokkevis_benchmark (Model *model, ConstraintSet *constraint_set, int sample_count) { @@ -262,18 +354,19 @@ double run_contacts_kokkevis_benchmark (Model *model, ConstraintSet *constraint_ sample_data.fillRandom(model->dof_count, sample_count); TimerInfo tinfo; - timer_start (&tinfo); for (int i = 0; i < sample_count; i++) { - ForwardDynamicsContactsKokkevis(*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + timer_start (&tinfo); + ForwardDynamicsContactsKokkevis(*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + sample_data.durations[i] = timer_stop (&tinfo); } - double duration = timer_stop (&tinfo); + report_constraints_run(*model, sample_data, "ForwardDynamicsContactsKokkevis"); - return duration; + return sample_data.durations.sum(); } -double contacts_benchmark (int sample_count, ContactsMethod contacts_method) { +void contacts_benchmark (int sample_count, ContactsMethod contacts_method) { // initialize the human model Model *model = new Model(); generate_human36model(model); @@ -284,6 +377,17 @@ double contacts_benchmark (int sample_count, ContactsMethod contacts_method) { unsigned int hand_r = model->GetBodyId ("hand_r"); unsigned int hand_l = model->GetBodyId ("hand_l"); + std::vector< bool > actuatedDof; + actuatedDof.resize(model->qdot_size); + + for(unsigned int i=0; idof_count << endl; - cout << "= #samples: " << sample_count << endl; - cout << "= No constraints (Articulated Body Algorithm):" << endl; - run_forward_dynamics_ABA_benchmark (model, sample_count); - cout << "= Constraints:" << endl; - double duration; + model_name = "Human36"; + if (!json_output) { + cout << "= #DOF: " << setw(3) << model->dof_count << endl; + cout << "= #samples: " << sample_count << endl; + cout << "= No constraints (Articulated Body Algorithm):" << endl; + run_forward_dynamics_ABA_benchmark(model, sample_count); + } // one body one - if (contacts_method == ContactsMethodLagrangian) { - duration = run_contacts_lagrangian_benchmark (model, &one_body_one_constraint, sample_count); - } else if (contacts_method == ContactsMethodRangeSpaceSparse) { - duration = run_contacts_lagrangian_sparse_benchmark (model, &one_body_one_constraint, sample_count); - } else if (contacts_method == ContactsMethodNullSpace) { - duration = run_contacts_null_space (model, &one_body_one_constraint, sample_count); + model_name = "Human36_1Bodies1Constraints"; + if (contacts_method == ConstraintsMethodDirect) { + run_contacts_lagrangian_benchmark (model, &one_body_one_constraint, sample_count); + } else if (contacts_method == ConstraintsMethodRangeSpaceSparse) { + run_contacts_lagrangian_sparse_benchmark (model, &one_body_one_constraint, sample_count); + } else if (contacts_method == ConstraintsMethodNullSpace) { + run_contacts_null_space (model, &one_body_one_constraint, sample_count); } else { - duration = run_contacts_kokkevis_benchmark (model, &one_body_one_constraint, sample_count); + run_contacts_kokkevis_benchmark (model, &one_body_one_constraint, sample_count); } - cout << "ConstraintSet: 1 Body 1 Constraint : " - << " duration = " << setw(10) << duration << "(s)" - << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; - // two_bodies_one - if (contacts_method == ContactsMethodLagrangian) { - duration = run_contacts_lagrangian_benchmark (model, &two_bodies_one_constraint, sample_count); - } else if (contacts_method == ContactsMethodRangeSpaceSparse) { - duration = run_contacts_lagrangian_sparse_benchmark (model, &two_bodies_one_constraint, sample_count); - } else if (contacts_method == ContactsMethodNullSpace) { - duration = run_contacts_null_space (model, &two_bodies_one_constraint, sample_count); + model_name = "Human36_2Bodies1Constraints"; + if (contacts_method == ConstraintsMethodDirect) { + run_contacts_lagrangian_benchmark (model, &two_bodies_one_constraint, sample_count); + } else if (contacts_method == ConstraintsMethodRangeSpaceSparse) { + run_contacts_lagrangian_sparse_benchmark (model, &two_bodies_one_constraint, sample_count); + } else if (contacts_method == ConstraintsMethodNullSpace) { + run_contacts_null_space (model, &two_bodies_one_constraint, sample_count); } else { - duration = run_contacts_kokkevis_benchmark (model, &two_bodies_one_constraint, sample_count); + run_contacts_kokkevis_benchmark (model, &two_bodies_one_constraint, sample_count); } - cout << "ConstraintSet: 2 Bodies 1 Constraint : " - << " duration = " << setw(10) << duration << "(s)" - << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; - - // four_bodies_one - if (contacts_method == ContactsMethodLagrangian) { - duration = run_contacts_lagrangian_benchmark (model, &four_bodies_one_constraint, sample_count); - } else if (contacts_method == ContactsMethodRangeSpaceSparse) { - duration = run_contacts_lagrangian_sparse_benchmark (model, &four_bodies_one_constraint, sample_count); - } else if (contacts_method == ContactsMethodNullSpace) { - duration = run_contacts_null_space (model, &four_bodies_one_constraint, sample_count); + model_name = "Human36_4Bodies1Constraints"; + if (contacts_method == ConstraintsMethodDirect) { + run_contacts_lagrangian_benchmark (model, &four_bodies_one_constraint, sample_count); + } else if (contacts_method == ConstraintsMethodRangeSpaceSparse) { + run_contacts_lagrangian_sparse_benchmark (model, &four_bodies_one_constraint, sample_count); + } else if (contacts_method == ConstraintsMethodNullSpace) { + run_contacts_null_space (model, &four_bodies_one_constraint, sample_count); } else { - duration = run_contacts_kokkevis_benchmark (model, &four_bodies_one_constraint, sample_count); + run_contacts_kokkevis_benchmark (model, &four_bodies_one_constraint, sample_count); } - cout << "ConstraintSet: 4 Bodies 1 Constraint : " - << " duration = " << setw(10) << duration << "(s)" - << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; - // one_body_four - if (contacts_method == ContactsMethodLagrangian) { - duration = run_contacts_lagrangian_benchmark (model, &one_body_four_constraints, sample_count); - } else if (contacts_method == ContactsMethodRangeSpaceSparse) { - duration = run_contacts_lagrangian_sparse_benchmark (model, &one_body_four_constraints, sample_count); - } else if (contacts_method == ContactsMethodNullSpace) { - duration = run_contacts_null_space (model, &one_body_four_constraints, sample_count); + model_name = "Human36_1Bodies4Constraints"; + if (contacts_method == ConstraintsMethodDirect) { + run_contacts_lagrangian_benchmark (model, &one_body_four_constraints, sample_count); + } else if (contacts_method == ConstraintsMethodRangeSpaceSparse) { + run_contacts_lagrangian_sparse_benchmark (model, &one_body_four_constraints, sample_count); + } else if (contacts_method == ConstraintsMethodNullSpace) { + run_contacts_null_space (model, &one_body_four_constraints, sample_count); } else { - duration = run_contacts_kokkevis_benchmark (model, &one_body_four_constraints, sample_count); + run_contacts_kokkevis_benchmark (model, &one_body_four_constraints, sample_count); } - cout << "ConstraintSet: 1 Body 4 Constraints : " - << " duration = " << setw(10) << duration << "(s)" - << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; - // two_bodies_four - if (contacts_method == ContactsMethodLagrangian) { - duration = run_contacts_lagrangian_benchmark (model, &two_bodies_four_constraints, sample_count); - } else if (contacts_method == ContactsMethodRangeSpaceSparse) { - duration = run_contacts_lagrangian_sparse_benchmark (model, &two_bodies_four_constraints, sample_count); - } else if (contacts_method == ContactsMethodNullSpace) { - duration = run_contacts_null_space (model, &two_bodies_four_constraints, sample_count); + model_name = "Human36_2Bodies4Constraints"; + if (contacts_method == ConstraintsMethodDirect) { + run_contacts_lagrangian_benchmark (model, &two_bodies_four_constraints, sample_count); + } else if (contacts_method == ConstraintsMethodRangeSpaceSparse) { + run_contacts_lagrangian_sparse_benchmark (model, &two_bodies_four_constraints, sample_count); + } else if (contacts_method == ConstraintsMethodNullSpace) { + run_contacts_null_space (model, &two_bodies_four_constraints, sample_count); } else { - duration = run_contacts_kokkevis_benchmark (model, &two_bodies_four_constraints, sample_count); + run_contacts_kokkevis_benchmark (model, &two_bodies_four_constraints, sample_count); } - cout << "ConstraintSet: 2 Bodies 4 Constraints: " - << " duration = " << setw(10) << duration << "(s)" - << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; - - // four_bodies_four - if (contacts_method == ContactsMethodLagrangian) { - duration = run_contacts_lagrangian_benchmark (model, &four_bodies_four_constraints, sample_count); - } else if (contacts_method == ContactsMethodRangeSpaceSparse) { - duration = run_contacts_lagrangian_sparse_benchmark (model, &four_bodies_four_constraints, sample_count); - } else if (contacts_method == ContactsMethodNullSpace) { - duration = run_contacts_null_space (model, &four_bodies_four_constraints, sample_count); + model_name = "Human36_4Bodies4Constraints"; + if (contacts_method == ConstraintsMethodDirect) { + run_contacts_lagrangian_benchmark (model, &four_bodies_four_constraints, sample_count); + } else if (contacts_method == ConstraintsMethodRangeSpaceSparse) { + run_contacts_lagrangian_sparse_benchmark (model, &four_bodies_four_constraints, sample_count); + } else if (contacts_method == ConstraintsMethodNullSpace) { + run_contacts_null_space (model, &four_bodies_four_constraints, sample_count); } else { - duration = run_contacts_kokkevis_benchmark (model, &four_bodies_four_constraints, sample_count); + run_contacts_kokkevis_benchmark (model, &four_bodies_four_constraints, sample_count); } - cout << "ConstraintSet: 4 Bodies 4 Constraints: " - << " duration = " << setw(10) << duration << "(s)" - << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; - delete model; +} +double run_single_inverse_kinematics_benchmark(Model *model, std::vector &CS, int sample_count){ + TimerInfo tinfo; + timer_start (&tinfo); + VectorNd qinit = VectorNd::Zero(model->dof_count); + VectorNd qres = VectorNd::Zero(model->dof_count); + VectorNd failures = VectorNd::Zero(model->dof_count); + + for (int i = 0; i < sample_count; i++) { + if (!InverseKinematics(*model, qinit, CS[i], qres)){ + failures[i] = 1; + } + } + double duration = timer_stop (&tinfo); + std::cout << "Success Rate: " << (1-failures.mean())*100 << "% for: "; + return duration; + +} + +double run_all_inverse_kinematics_benchmark (int sample_count){ + + //initialize the human model + Model *model = new Model(); + generate_human36model(model); + + unsigned int foot_r = model->GetBodyId ("foot_r"); + unsigned int foot_l = model->GetBodyId ("foot_l"); + unsigned int hand_r = model->GetBodyId ("hand_r"); + unsigned int hand_l = model->GetBodyId ("hand_l"); + unsigned int head = model->GetBodyId ("head"); + + Vector3d foot_r_point (1., 0., 0.); + Vector3d foot_l_point (-1., 0., 0.); + Vector3d hand_r_point (0., 1., 0.); + Vector3d hand_l_point (1., 0., 1.); + Vector3d head_point (0.,0.,-1.); + + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + + //create constraint sets + std::vector cs_one_point; + std::vector cs_two_point_one_orientation; + std::vector cs_two_full_one_point; + std::vector cs_two_full_two_point_one_orientation; + std::vector cs_five_full; + + for (unsigned int i = 0; i < sample_count; i++){ + Vector3d foot_r_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], foot_r, foot_r_point); + Vector3d foot_l_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], foot_l, foot_l_point); + Vector3d hand_r_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], hand_r, hand_r_point); + Vector3d hand_l_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], hand_l, hand_l_point); + Vector3d head_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], head , head_point); + + Matrix3d foot_r_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], foot_r, false); + Matrix3d foot_l_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], foot_l, false); + Matrix3d hand_r_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], hand_r, false); + Matrix3d hand_l_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], hand_l, false); + Matrix3d head_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], head , false); + + //single point + InverseKinematicsConstraintSet one_point; + one_point.AddPointConstraint(foot_r, foot_r_point, foot_r_position); + one_point.step_tol = 1e-12; + cs_one_point.push_back(one_point); + + //two point and one orientation + InverseKinematicsConstraintSet two_point_one_orientation; + two_point_one_orientation.AddPointConstraint(foot_l,foot_l_point, foot_l_position); + two_point_one_orientation.AddPointConstraint(foot_r, foot_r_point, foot_r_position); + two_point_one_orientation.AddOrientationConstraint(head, head_orientation); + two_point_one_orientation.step_tol = 1e-12; + cs_two_point_one_orientation.push_back(two_point_one_orientation); + + //two full and one point + InverseKinematicsConstraintSet two_full_one_point; + two_full_one_point.AddFullConstraint(hand_r, hand_r_point, hand_r_position, hand_r_orientation); + two_full_one_point.AddFullConstraint(hand_l, hand_l_point, hand_l_position, hand_l_orientation); + two_full_one_point.AddPointConstraint(head, head_point, head_position); + two_full_one_point.step_tol = 1e-12; + cs_two_full_one_point.push_back(two_full_one_point); + + //two full, two points and one orienation + InverseKinematicsConstraintSet two_full_two_point_one_orientation; + two_full_two_point_one_orientation.AddPointConstraint(foot_r, foot_r_point, foot_r_position); + two_full_two_point_one_orientation.AddPointConstraint(foot_l, foot_l_point, foot_l_position); + two_full_two_point_one_orientation.AddFullConstraint(hand_r, hand_r_point, hand_r_position, hand_r_orientation); + two_full_two_point_one_orientation.AddFullConstraint(hand_l, hand_l_point, hand_l_position, hand_l_orientation); + two_full_two_point_one_orientation.AddOrientationConstraint(head, head_orientation); + two_full_two_point_one_orientation.step_tol = 1e-12; + cs_two_full_two_point_one_orientation.push_back(two_full_two_point_one_orientation); + + //five points 5 orientations + InverseKinematicsConstraintSet five_full; + five_full.AddFullConstraint(foot_r, foot_r_point, foot_r_position, foot_r_orientation); + five_full.AddFullConstraint(foot_l, foot_l_point, foot_l_position, foot_l_orientation); + five_full.AddFullConstraint(hand_r, hand_r_point, hand_r_position, hand_r_orientation); + five_full.AddFullConstraint(hand_l, hand_l_point, hand_l_position, hand_l_orientation); + five_full.AddFullConstraint(head, head_point, head_position, head_orientation); + five_full.step_tol = 1e-12; + cs_five_full.push_back(five_full); + } + + cout << "= #DOF: " << setw(3) << model->dof_count << endl; + cout << "= #samples: " << sample_count << endl; + double duration; + + duration = run_single_inverse_kinematics_benchmark(model, cs_one_point, sample_count); + cout << "Constraints: 1 Body: 1 Point : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + duration = run_single_inverse_kinematics_benchmark(model, cs_two_point_one_orientation, sample_count); + cout << "Constraints: 3 Bodies: 2 Points 1 Orienation : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + duration = run_single_inverse_kinematics_benchmark(model, cs_two_full_one_point, sample_count); + cout << "Constraints: 3 Bodies: 2 Full 1 Point : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + duration = run_single_inverse_kinematics_benchmark(model, cs_two_full_two_point_one_orientation, sample_count); + cout << "Constraints: 5 Bodies: 2 Full 2 Points 1 Orienation : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + duration = run_single_inverse_kinematics_benchmark(model, cs_five_full, sample_count); + cout << "Constraints: 5 Bodies: 5 Full : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; return duration; } @@ -478,6 +695,7 @@ void print_usage () { #if defined RBDL_BUILD_ADDON_URDFREADER cout << " --floating-base | -f : the specified URDF model is a floating base model." << endl; #endif + cout << " --json : prints output in json format." << endl; cout << " --no-fd : disables benchmarking of forward dynamics." << endl; cout << " --no-fd-aba : disables benchmark for forwards dynamics using" << endl; cout << " the Articulated Body Algorithm" << endl; @@ -542,6 +760,8 @@ void parse_args (int argc, char* argv[]) { } else if (arg == "--floating-base" || arg == "-f") { urdf_floating_base = true; #endif + } else if (arg == "--json") { + json_output = true; } else if (arg == "--no-fd" ) { benchmark_run_fd_aba = false; benchmark_run_fd_lagrangian = false; @@ -560,9 +780,12 @@ void parse_args (int argc, char* argv[]) { } else if (arg == "--only-contacts" || arg == "-C") { disable_all_benchmarks(); benchmark_run_contacts = true; + } else if (arg == "--only-ik") { + disable_all_benchmarks(); + benchmark_run_ik = true; #if defined (RBDL_BUILD_ADDON_LUAMODEL) || defined (RBDL_BUILD_ADDON_URDFREADER) - } else if (model_file == "") { - model_file = arg; + } else if (model_name == "") { + model_name = arg; #endif } else { print_usage(); @@ -580,18 +803,18 @@ int main (int argc, char *argv[]) { model = new Model(); - if (model_file != "") { - if (model_file.substr (model_file.size() - 4, 4) == ".lua") { + if (model_name != "") { + if (model_name.substr (model_name.size() - 4, 4) == ".lua") { #ifdef RBDL_BUILD_ADDON_LUAMODEL - RigidBodyDynamics::Addons::LuaModelReadFromFile (model_file.c_str(), model); + RigidBodyDynamics::Addons::LuaModelReadFromFile (model_name.c_str(), model); #else cerr << "Could not load Lua model: LuaModel addon not enabled!" << endl; abort(); #endif } - if (model_file.substr (model_file.size() - 5, 5) == ".urdf") { + if (model_name.substr (model_name.size() - 5, 5) == ".urdf") { #ifdef RBDL_BUILD_ADDON_URDFREADER - RigidBodyDynamics::Addons::URDFReadFromFile(model_file.c_str(), model, urdf_floating_base); + RigidBodyDynamics::Addons::URDFReadFromFile(model_name.c_str(), model, urdf_floating_base); #else cerr << "Could not load URDF model: urdfreader addon not enabled!" << endl; abort(); @@ -599,27 +822,27 @@ int main (int argc, char *argv[]) { } if (benchmark_run_fd_aba) { - cout << "= Forward Dynamics: ABA =" << endl; + report_section("Forward Dynamics: ABA"); run_forward_dynamics_ABA_benchmark (model, benchmark_sample_count); } if (benchmark_run_fd_lagrangian) { - cout << "= Forward Dynamics: Lagrangian (Piv. LU decomposition) =" << endl; + report_section("Forward Dynamics: Lagrangian (Piv. LU decomposition)"); run_forward_dynamics_lagrangian_benchmark (model, benchmark_sample_count); } if (benchmark_run_id_rnea) { - cout << "= Inverse Dynamics: RNEA =" << endl; + report_section("Inverse Dynamics: RNEA"); run_inverse_dynamics_RNEA_benchmark (model, benchmark_sample_count); } if (benchmark_run_crba) { - cout << "= Joint Space Inertia Matrix: CRBA =" << endl; + report_section("Joint Space Inertia Matrix: CRBA"); run_CRBA_benchmark (model, benchmark_sample_count); } if (benchmark_run_nle) { - cout << "= Nonlinear effects =" << endl; + report_section("Nonlinear Effects"); run_nle_benchmark (model, benchmark_sample_count); } @@ -628,12 +851,18 @@ int main (int argc, char *argv[]) { return 0; } - rbdl_print_version(); - cout << endl; + if (!json_output) { + rbdl_print_version(); + cout << endl; + } if (benchmark_run_fd_aba) { - cout << "= Forward Dynamics: ABA =" << endl; + report_section("Forward Dynamics: ABA"); for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { + ostringstream model_name_stream; + model_name_stream << "planar_model_depth_" << depth; + model_name = model_name_stream.str(); + model = new Model(); model->gravity = Vector3d (0., -9.81, 0.); @@ -643,11 +872,10 @@ int main (int argc, char *argv[]) { delete model; } - cout << endl; } if (benchmark_run_fd_lagrangian) { - cout << "= Forward Dynamics: Lagrangian (Piv. LU decomposition) =" << endl; + report_section("Forward Dynamics: Lagrangian (Piv. LU decomposition)"); for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { model = new Model(); model->gravity = Vector3d (0., -9.81, 0.); @@ -658,11 +886,10 @@ int main (int argc, char *argv[]) { delete model; } - cout << endl; } if (benchmark_run_id_rnea) { - cout << "= Inverse Dynamics: RNEA =" << endl; + report_section("Inverse Dynamics: RNEA"); for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { model = new Model(); model->gravity = Vector3d (0., -9.81, 0.); @@ -673,11 +900,10 @@ int main (int argc, char *argv[]) { delete model; } - cout << endl; } if (benchmark_run_crba) { - cout << "= Joint Space Inertia Matrix: CRBA =" << endl; + report_section("Joint Space Inertia Matrix: CRBA"); for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { model = new Model(); model->gravity = Vector3d (0., -9.81, 0.); @@ -688,11 +914,10 @@ int main (int argc, char *argv[]) { delete model; } - cout << endl; } if (benchmark_run_nle) { - cout << "= Nonlinear Effects =" << endl; + report_section("Nonlinear Effects"); for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { model = new Model(); model->gravity = Vector3d (0., -9.81, 0.); @@ -703,11 +928,10 @@ int main (int argc, char *argv[]) { delete model; } - cout << endl; } if (benchmark_run_calc_minv_times_tau) { - cout << "= CalcMInvTimesTau =" << endl; + report_section("CalcMInvTimesTau"); for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { model = new Model(); model->gravity = Vector3d (0., -9.81, 0.); @@ -718,21 +942,85 @@ int main (int argc, char *argv[]) { delete model; } - cout << endl; } if (benchmark_run_contacts) { - cout << "= Contacts: ForwardDynamicsConstraintsLagrangian" << endl; - contacts_benchmark (benchmark_sample_count, ContactsMethodLagrangian); + report_section("Contacts: ForwardDynamicsConstraintsDirect"); + contacts_benchmark (benchmark_sample_count, ConstraintsMethodDirect); - cout << "= Contacts: ForwardDynamicsConstraintsRangeSpaceSparse" << endl; - contacts_benchmark (benchmark_sample_count, ContactsMethodRangeSpaceSparse); + report_section("Contacts: ForwardDynamicsConstraintsRangeSpaceSparse"); + contacts_benchmark (benchmark_sample_count, ConstraintsMethodRangeSpaceSparse); + + report_section("Contacts: ForwardDynamicsConstraintsNullSpace"); + contacts_benchmark (benchmark_sample_count, ConstraintsMethodNullSpace); + + report_section("Contacts: ForwardDynamicsContactsKokkevis"); + contacts_benchmark (benchmark_sample_count, ConstraintsMethodKokkevis); + } + + if (benchmark_run_ik) { + report_section("Inverse Kinematics"); + run_all_inverse_kinematics_benchmark(benchmark_sample_count); + } + + if (json_output) { + cout.precision(15); + cout << "{" << endl; + + cout << " \"rbdl_info\" : {" << endl; + int compile_version = rbdl_get_api_version(); + int compile_major = (compile_version & 0xff0000) >> 16; + int compile_minor = (compile_version & 0x00ff00) >> 8; + int compile_patch = (compile_version & 0x0000ff); + + std::ostringstream compile_version_string(""); + compile_version_string << compile_major << "." << compile_minor << "." << compile_patch; + + cout << " \"version_str\" : \"" << compile_version_string.str() << "\"," << endl; + cout << " \"major\" : " << compile_major << "," << endl; + cout << " \"minor\" : " << compile_minor << "," << endl; + cout << " \"patch\" : " << compile_patch << "," << endl; + cout << " \"build_type\" : \"" << RBDL_BUILD_TYPE << "\"," << endl; + cout << " \"commit\" : \"" << RBDL_BUILD_COMMIT << "\"," << endl; + cout << " \"branch\" : \"" << RBDL_BUILD_BRANCH << "\"," << endl; + cout << " \"compiler_id\" : \"" << RBDL_BUILD_COMPILER_ID << "\"," << endl; + cout << " \"compiler_version\" : \"" << RBDL_BUILD_COMPILER_VERSION << "\"" << endl; + + cout << " }," << endl; + + cout << " \"host_info\" : {" << endl; + cout << " \"cpu_model_name\" : \"" << get_cpu_model_name() << "\"," << endl; + cout << " \"time_utc\" : " << "\"" << get_utc_time_string() << "\"" << endl; + cout << " }," << endl; + + cout << " \"runs\" : "; + cout << "[" << endl; + + for (int i; i < benchmark_runs.size(); i++) { + const BenchmarkRun& run = benchmark_runs[i]; + + const char* indent = " "; + + cout << " " << "{" << endl; + cout << indent << "\"model\" : \"" << run.model_name << "\"," << endl; + cout << indent << "\"dof\" : " << run.model_dof << "," << endl; + cout << indent << "\"benchmark\" : \"" << run.benchmark << "\"," << endl; + cout << indent << "\"duration\" : " << run.duration << "," << endl; + cout << indent << "\"sample_count\" : " << run.sample_count << "," << endl; + cout << indent << "\"avg\" : " << run.avg << "," << endl; + cout << indent << "\"min\" : " << run.min << "," << endl; + cout << indent << "\"max\" : " << run.max << endl; + cout << " " << "}"; + + if (i != benchmark_runs.size() - 1) { + cout << ","; + } + cout << endl; + } - cout << "= Contacts: ForwardDynamicsConstraintsNullSpace" << endl; - contacts_benchmark (benchmark_sample_count, ContactsMethodNullSpace); + cout << " ]" << endl; - cout << "= Contacts: ForwardDynamicsContactsKokkevis" << endl; - contacts_benchmark (benchmark_sample_count, ContactsMethodKokkevis); + cout << "}" << endl; } return 0; diff --git a/addons/geometry/Function.h b/addons/geometry/Function.h index 0daaf02..4449cdd 100644 --- a/addons/geometry/Function.h +++ b/addons/geometry/Function.h @@ -209,7 +209,7 @@ class Function_::Linear : public Function_ { assert(x.size() == coefficients.size()-1); assert(derivComponents.size() > 0); if (derivComponents.size() == 1) - return coefficients(derivComponents[0]); + return coefficients[derivComponents[0]]; return static_cast(0); } virtual int getArgumentSize() const { diff --git a/addons/geometry/LICENSE b/addons/geometry/LICENSE index 6e03346..92bb10c 100644 --- a/addons/geometry/LICENSE +++ b/addons/geometry/LICENSE @@ -1,5 +1,5 @@ Rigid Body Dynamics Library Geometry Addon - -Copyright (c) 2016 Matthew Millard +Copyright (c) 2016 Matthew Millard (zlib license) diff --git a/addons/geometry/README.md b/addons/geometry/README.md index be2861b..561dd87 100644 --- a/addons/geometry/README.md +++ b/addons/geometry/README.md @@ -4,7 +4,7 @@ @author Matthew Millard -\copyright 2016 Matthew Millard +\copyright 2016 Matthew Millard \b Requirements This addon is standalone as of its first release diff --git a/addons/geometry/SegmentedQuinticBezierToolkit.cc b/addons/geometry/SegmentedQuinticBezierToolkit.cc index 526efcd..f5bb7fb 100644 --- a/addons/geometry/SegmentedQuinticBezierToolkit.cc +++ b/addons/geometry/SegmentedQuinticBezierToolkit.cc @@ -24,10 +24,10 @@ Update: This is a port of the original code so that it will work with the multibody code RBDL written by Martin Felis. - + Author: Matthew Millard - + Date: Nov 2015 @@ -42,6 +42,7 @@ #include #include #include +#include //#include using namespace RigidBodyDynamics::Addons::Geometry; @@ -58,7 +59,7 @@ static double UTOL_INITIALSOLN = std::numeric_limits::epsilon()*1e11; static int MAXITER_INITIALSOLN = 12; static int NUM_SAMPLE_PTS = 100; //The number of knot points to use to sample - //each Bezier corner section +//each Bezier corner section double SegmentedQuinticBezierToolkit::scaleCurviness(double curviness) { @@ -74,32 +75,33 @@ This function will print cvs file of the column vector col0 and the matrix data @params filename: The name of the file to print */ void SegmentedQuinticBezierToolkit:: - printMatrixToFile( const VectorNd& col0, - const MatrixNd& data, - std::string& filename) +printMatrixToFile( const VectorNd& col0, + const MatrixNd& data, + std::string& filename) { - + ofstream datafile; datafile.open(filename.c_str()); - for(int i = 0; i < data.rows(); i++){ - datafile << col0[i] << ","; - for(int j = 0; j < data.cols(); j++){ - if(j& curveFit, - MatrixNd& ctrlPts, - VectorNd& xVal, - VectorNd& yVal, - std::string& filename) +printBezierSplineFitCurves( const Function_& curveFit, + MatrixNd& ctrlPts, + VectorNd& xVal, + VectorNd& yVal, + std::string& filename) { std::string caller = "printBezierSplineFitCurves"; int nbezier = int(ctrlPts.cols()/2.0); @@ -124,36 +126,34 @@ void SegmentedQuinticBezierToolkit:: double u = 0; int oidx = 0; int offset = 0; - for(int j=0; j < nbezier ; j++) - { - if(j > 0){ - offset = 1; + for(int j=0; j < nbezier ; j++) { + if(j > 0) { + offset = 1; } - for(int i=0; i=0 && curviness <= 1) , - "SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints", + SimTK_ERRCHK_ALWAYS( (curviness>=0 && curviness <= 1) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints", "Error: double argument curviness must be between 0.0 and 1.0."); */ - if( !(curviness>=0 && curviness <= 1) ){ - - cerr << "SegmentedQuinticBezierToolkit::" - <<"calcQuinticBezierCornerControlPoints" - <<"Error: double argument curviness must be between 0.0 and 1.0." - <<"curviness : " << curviness << " " - << endl; - assert (0); - abort(); + if( !(curviness>=0 && curviness <= 1) ) { + ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::" + << "calcQuinticBezierCornerControlPoints" + << "Error: double argument curviness must be between 0.0 and 1.0." + << "curviness : " << curviness << " " + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } @@ -201,20 +200,20 @@ MatrixNd SegmentedQuinticBezierToolkit:: double yC = 0.; double epsilon = std::numeric_limits::epsilon(); double rootEPS = sqrt(epsilon); - if(abs(dydx0-dydx1) > rootEPS){ - xC = (y1-y0-x1*dydx1+x0*dydx0)/(dydx0-dydx1); - }else{ + if(abs(dydx0-dydx1) > rootEPS) { + xC = (y1-y0-x1*dydx1+x0*dydx0)/(dydx0-dydx1); + } else { xC = (x1+x0)/2.0; } yC = (xC-x1)*dydx1 + y1; //Check to make sure that the inputs are consistent with a corner, and will //not produce an 's' shaped section. To check this we compute the sides of - //a triangle that is formed by the two points that the user entered, and + //a triangle that is formed by the two points that the user entered, and //also the intersection of the 2 lines the user entered. If the distance //between the two points the user entered is larger than the distance from //either point to the intersection loctaion, this function will generate a - //'C' shaped curve. If this is not true, an 'S' shaped curve will result, + //'C' shaped curve. If this is not true, an 'S' shaped curve will result, //and this function should not be used. double xCx0 = (xC-x0); @@ -232,34 +231,31 @@ MatrixNd SegmentedQuinticBezierToolkit:: //This error message needs to be better. /* - SimTK_ERRCHK_ALWAYS( ((c > a) && (c > b)), - "SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints", + SimTK_ERRCHK_ALWAYS( ((c > a) && (c > b)), + "SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints", "The intersection point for the two lines defined by the input" "parameters must be consistent with a C shaped corner."); */ - - - - if( !((c > a) && (c > b)) ){ - cerr << "SegmentedQuinticBezierToolkit" - << "::calcQuinticBezierCornerControlPoints:" - << "The line segments at the end of the curve sections " - << "do not intersect within the domain " - << "("<< x0 << "," << x1 << ") of the curve. " - << "and so there is a chance that curve will not" - << " be monotonic. There are 2 ways to fix this problem: " - << endl - << "1. Add an intermediate point," - << endl - << " 2. Space the domain points more widely " - << endl - << "Details: " - << endl << " a: " << a - << endl << " b: " << b - << endl << " c: " << c << endl; - assert (0); - abort(); + if( !((c > a) && (c > b)) ) { + ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit" + << "::calcQuinticBezierCornerControlPoints:" + << "The line segments at the end of the curve sections " + << "do not intersect within the domain " + << "("<< x0 << "," << x1 << ") of the curve. " + << "and so there is a chance that curve will not" + << " be monotonic. There are 2 ways to fix this problem: " + << endl + << "1. Add an intermediate point," + << endl + << " 2. Space the domain points more widely " + << endl + << "Details: " + << endl << " a: " << a + << endl << " b: " << b + << endl << " c: " << c << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } /* @@ -330,39 +326,39 @@ MatrixNd SegmentedQuinticBezierToolkit:: // BASIC QUINTIC BEZIER EVALUATION FUNCTIONS //============================================================================= -/* +/* Multiplications Additions Assignments 21 20 13 */ double SegmentedQuinticBezierToolkit:: - calcQuinticBezierCurveVal(double u1, const VectorNd& pts) +calcQuinticBezierCurveVal(double u1, const VectorNd& pts) { double val = -1; /* - SimTK_ERRCHK1_ALWAYS( (u>=0 && u <= 1) , - "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal", + SimTK_ERRCHK1_ALWAYS( (u>=0 && u <= 1) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal", "Error: double argument u must be between 0.0 and 1.0" "but %f was entered.",u); */ - if(!(u1 >= 0 && u1 <= 1)){ - cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal" - << "Error: double argument u must be between 0.0 and 1.0" - << "but " << u1 <<" was entered."; - assert (0); - abort(); + if(!(u1 >= 0 && u1 <= 1)) { + ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal" + << "Error: double argument u must be between 0.0 and 1.0" + << "but " << u1 <<" was entered."; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - + /* - SimTK_ERRCHK_ALWAYS( (pts.size() == 6) , - "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal", + SimTK_ERRCHK_ALWAYS( (pts.size() == 6) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal", "Error: vector argument pts must have a length of 6."); */ - if(!(pts.size() == 6) ){ - cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal:" - << "Error: vector argument pts must have a length of 6."; - assert (0); - abort(); + if(!(pts.size() == 6) ) { + ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal:" + << "Error: vector argument pts must have a length of 6."; + throw RigidBodyDynamics::Errors::RBDLSizeMismatchError(errormsg.str()); } double u2 = u1*u1; @@ -370,19 +366,19 @@ double SegmentedQuinticBezierToolkit:: double u4 = u3*u1; double u5 = u4*u1; - //v0 = 1; + //v0 = 1; double v1 = (1-u1); double v2 = v1*v1; double v3 = v2*v1; double v4 = v3*v1; double v5 = v4*v1; - val = pts[0] *v5*1.0 - + pts[1]*u1*v4*5.0 - + pts[2]*u2*v3*10.0 - + pts[3]*u3*v2*10.0 - + pts[4]*u4*v1*5.0 - + pts[5]*u5 *1.0; + val = pts[0] *v5*1.0 + + pts[1]*u1*v4*5.0 + + pts[2]*u2*v3*10.0 + + pts[3]*u3*v2*10.0 + + pts[4]*u4*v1*5.0 + + pts[5]*u5 *1.0; return val; @@ -393,7 +389,7 @@ Detailed Computational Costs dy/dx Divisions Multiplications Additions Assignments dy/du 20 19 11 dx/du 20 19 11 - dy/dx 1 + dy/dx 1 total 1 40 38 22 d2y/dx2 Divisions Multiplications Additions Assignments @@ -401,7 +397,7 @@ Detailed Computational Costs dx/du 20 19 11 d2y/du2 17 17 9 d2x/du2 17 17 9 - d2y/dx2 2 4 1 3 + d2y/dx2 2 4 1 3 total 2 78 73 23 d3y/dx3 Divisions Multiplications Additions Assignments @@ -438,10 +434,10 @@ Detailed Computational Costs d4y/du4 11 11 3 d4x/du4 11 11 3 d5y/du5 6 6 1 - d5x/du5 6 6 1 + d5x/du5 6 6 1 d5y/dx5 7 100 36 28 - total 7 236 170 88 + total 7 236 170 88 d6y/dx6 dy/du 20 19 11 @@ -453,105 +449,101 @@ Detailed Computational Costs d4y/du4 11 11 3 d4x/du4 11 11 3 d5y/du5 6 6 1 - d5x/du5 6 6 1 + d5x/du5 6 6 1 d6y/dx6 9 198 75 46 total 9 334 209 106 */ double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivDYDX( - double u, - const VectorNd& xpts, - const VectorNd& ypts, - int order) + double u, + const VectorNd& xpts, + const VectorNd& ypts, + int order) { double val = NAN;//SimTK::NaN; - + //Bounds checking on the input /* - SimTK_ERRCHK_ALWAYS( (u>=0 && u <= 1) , - "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + SimTK_ERRCHK_ALWAYS( (u>=0 && u <= 1) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", "Error: double argument u must be between 0.0 and 1.0."); - SimTK_ERRCHK_ALWAYS( (xpts.size()==6) , - "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + SimTK_ERRCHK_ALWAYS( (xpts.size()==6) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", "Error: vector argument xpts \nmust have a length of 6."); - SimTK_ERRCHK_ALWAYS( (ypts.size()==6) , - "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + SimTK_ERRCHK_ALWAYS( (ypts.size()==6) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", "Error: vector argument ypts \nmust have a length of 6."); SimTK_ERRCHK_ALWAYS( (order >= 1), - "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", "Error: order must be greater than."); SimTK_ERRCHK_ALWAYS( (order <= 6), - "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", "Error: order must be less than, or equal to 6."); */ - if( !(u>=0 && u <= 1) ){ - cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" - << "Error: double argument u must be between 0.0 and 1.0." - << endl; - assert(0); - abort(); - + if( !(u>=0 && u <= 1) ) { + ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: double argument u must be between 0.0 and 1.0." + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( !(xpts.size()==6) ){ - cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" - << "Error: vector argument xpts must have a length of 6." - << endl; - assert(0); - abort(); - + if( !(xpts.size()==6) ) { + ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: vector argument xpts must have a length of 6." + << endl; + throw RigidBodyDynamics::Errors::RBDLSizeMismatchError(errormsg.str()); } - if( !(ypts.size()==6) ){ - cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" - << "Error: vector argument ypts must have a length of 6." - << endl; - assert(0); - abort(); + if( !(ypts.size()==6) ) { + ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: vector argument ypts must have a length of 6." + << endl; + throw RigidBodyDynamics::Errors::RBDLSizeMismatchError(errormsg.str()); } - if( !(order >= 1) ){ - cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" - << "Error: order must be greater than." - << endl; - assert(0); - abort(); + if( !(order >= 1) ) { + ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: order must be greater than." + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( !(order <= 6) ){ - cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" - << "Error: order must be less than, or equal to 6." - << endl; - assert(0); - abort(); + if( !(order <= 6) ) { + ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: order must be less than, or equal to 6." + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } //std::string localCaller = caller; //localCaller.append(".calcQuinticBezierCurveDerivDYDX"); //Compute the derivative d^n y/ dx^n - switch(order){ - case 1: //Calculate dy/dx - { + switch(order) { + case 1: { //Calculate dy/dx double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); double dydx = dydu/dxdu; val = dydx; - //Question: + //Question: //how is a divide by zero treated? Is SimTK::INF returned? - } - break; - case 2: //Calculate d^2y/dx^2 - { - double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); - double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); - double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); - double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); - - //Optimized code from Maple - + } + break; + case 2: { //Calculate d^2y/dx^2 + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + + //Optimized code from Maple - //see MuscleCurveCodeOpt_20120210 for details double t1 = 0.1e1 / dxdu; double t3 = dxdu*dxdu;//dxdu ^ 2; @@ -559,235 +551,231 @@ double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivDYDX( val = d2ydx2; - } - break; - case 3: //Calculate d^3y/dx^3 - { - double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); - double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); - double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); - double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); - double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); - double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); - - double t1 = 1 / dxdu; - double t3 = dxdu*dxdu;//(dxdu ^ 2); - double t4 = 1 / t3; - double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); - double t14 = (dydu * t4); - double d3ydx3 = ((d3ydu3 * t1 - 2 * d2ydu2 * t4 * d2xdu2 - + 2 * dydu / t3 / dxdu * t11 - t14 * d3xdu3) * t1 - - (d2ydu2 * t1 - t14 * d2xdu2) * t4 * d2xdu2) * t1; + } + break; + case 3: { //Calculate d^3y/dx^3 + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); + double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); + + double t1 = 1 / dxdu; + double t3 = dxdu*dxdu;//(dxdu ^ 2); + double t4 = 1 / t3; + double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); + double t14 = (dydu * t4); + double d3ydx3 = ((d3ydu3 * t1 - 2 * d2ydu2 * t4 * d2xdu2 + + 2 * dydu / t3 / dxdu * t11 - t14 * d3xdu3) * t1 + - (d2ydu2 * t1 - t14 * d2xdu2) * t4 * d2xdu2) * t1; val = d3ydx3; - } - break; - case 4: //Calculate d^4y/dx^4 - { - double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); - double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); - double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); - double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); - double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); - double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); - double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4); - double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4); - - double t1 = 1 / dxdu; - double t3 = dxdu*dxdu;//dxdu ^ 2; - double t4 = 1 / t3; - double t9 = (0.1e1 / t3 / dxdu); - double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); - double t14 = (d2ydu2 * t4); - double t17 = t3*t3;//(t3 ^ 2); - double t23 = (dydu * t9); - double t27 = (dydu * t4); - double t37 = d3ydu3 * t1 - 2 * t14 * d2xdu2 + 2 * t23 * t11 - - t27 * d3xdu3; - double t43 = d2ydu2 * t1 - t27 * d2xdu2; - double t47 = t43 * t4; - double d4ydx4 = (((d4ydu4 * t1 - 3 * d3ydu3 * t4 * d2xdu2 - + 6 * d2ydu2 * t9 * t11 - 3 * t14 * d3xdu3 - - 6 * dydu / t17 * t11 * d2xdu2 - + 6 * t23 * d2xdu2 * d3xdu3 - - t27 * d4xdu4) * t1 - 2 * t37 * t4 * d2xdu2 - + 2 * t43 * t9 * t11 - t47 * d3xdu3) * t1 - - (t37 * t1 - t47 * d2xdu2) * t4 * d2xdu2) * t1; - - + } + break; + case 4: { //Calculate d^4y/dx^4 + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); + double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); + double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4); + double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4); + + double t1 = 1 / dxdu; + double t3 = dxdu*dxdu;//dxdu ^ 2; + double t4 = 1 / t3; + double t9 = (0.1e1 / t3 / dxdu); + double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); + double t14 = (d2ydu2 * t4); + double t17 = t3*t3;//(t3 ^ 2); + double t23 = (dydu * t9); + double t27 = (dydu * t4); + double t37 = d3ydu3 * t1 - 2 * t14 * d2xdu2 + 2 * t23 * t11 + - t27 * d3xdu3; + double t43 = d2ydu2 * t1 - t27 * d2xdu2; + double t47 = t43 * t4; + double d4ydx4 = (((d4ydu4 * t1 - 3 * d3ydu3 * t4 * d2xdu2 + + 6 * d2ydu2 * t9 * t11 - 3 * t14 * d3xdu3 + - 6 * dydu / t17 * t11 * d2xdu2 + + 6 * t23 * d2xdu2 * d3xdu3 + - t27 * d4xdu4) * t1 - 2 * t37 * t4 * d2xdu2 + + 2 * t43 * t9 * t11 - t47 * d3xdu3) * t1 + - (t37 * t1 - t47 * d2xdu2) * t4 * d2xdu2) * t1; + + val = d4ydx4; - } - break; - case 5: - { - double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); - double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); - double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); - double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); - double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); - double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); - double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4); - double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4); - double d5xdu5=calcQuinticBezierCurveDerivU(u,xpts,5); - double d5ydu5=calcQuinticBezierCurveDerivU(u,ypts,5); - - double t1 = 1 / dxdu; - double t3 = dxdu*dxdu;//dxdu ^ 2; - double t4 = 1 / t3; - double t9 = (0.1e1 / t3 / dxdu); - double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); - double t14 = (d3ydu3 * t4); - double t17 = t3*t3;//(t3 ^ 2); - double t18 = 1 / t17; - double t20 = (t11 * d2xdu2); - double t23 = (d2ydu2 * t9); - double t24 = (d2xdu2 * d3xdu3); - double t27 = (d2ydu2 * t4); - double t33 = t11*t11;//(t11 ^ 2); - double t36 = (dydu * t18); - double t40 = (dydu * t9); - double t41 = d3xdu3*d3xdu3;//(d3xdu3 ^ 2); - double t47 = (dydu * t4); - double t49 = d5ydu5 * t1 - 4 * d4ydu4 * t4 * d2xdu2 - + 12 * d3ydu3 * t9 * t11 - 6 * t14 * d3xdu3 - - 24 * d2ydu2 * t18 * t20 + 24 * t23 * t24 - - 4 * t27 * d4xdu4 + 24 * dydu / t17 / dxdu * t33 - - 36 * t36 * t11 * d3xdu3 + 6 * t40 * t41 - + 8 * t40 * d2xdu2 * d4xdu4 - t47 * d5xdu5; - double t63 = d4ydu4 * t1 - 3 * t14 * d2xdu2 + 6 * t23 * t11 - - 3 * t27 * d3xdu3 - 6 * t36 * t20 - + 6 * t40 * t24 - t47 * d4xdu4; - double t73 = d3ydu3 * t1 - 2 * t27 * d2xdu2 + 2 * t40 * t11 - - t47 * d3xdu3; - double t77 = t73 * t4; - double t82 = d2ydu2 * t1 - t47 * d2xdu2; - double t86 = t82 * t9; - double t89 = t82 * t4; - double t99 = t63 * t1 - 2 * t77 * d2xdu2 + 2 * t86 * t11 - - t89 * d3xdu3; - double t105 = t73 * t1 - t89 * d2xdu2; - double t109 = t105 * t4; - double d5ydx5 = (((t49 * t1 - 3 * t63 * t4 * d2xdu2 - + 6 * t73 * t9 * t11 - 3 * t77 * d3xdu3 - - 6 * t82 * t18 * t20 - + 6 * t86 * t24 - t89 * d4xdu4) * t1 - - 2 * t99 * t4 * d2xdu2 - + 2 * t105 * t9 * t11 - t109 * d3xdu3) * t1 - - (t99 * t1 - t109 * d2xdu2) * t4 * d2xdu2) * t1; + } + break; + case 5: { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); + double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); + double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4); + double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4); + double d5xdu5=calcQuinticBezierCurveDerivU(u,xpts,5); + double d5ydu5=calcQuinticBezierCurveDerivU(u,ypts,5); + + double t1 = 1 / dxdu; + double t3 = dxdu*dxdu;//dxdu ^ 2; + double t4 = 1 / t3; + double t9 = (0.1e1 / t3 / dxdu); + double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); + double t14 = (d3ydu3 * t4); + double t17 = t3*t3;//(t3 ^ 2); + double t18 = 1 / t17; + double t20 = (t11 * d2xdu2); + double t23 = (d2ydu2 * t9); + double t24 = (d2xdu2 * d3xdu3); + double t27 = (d2ydu2 * t4); + double t33 = t11*t11;//(t11 ^ 2); + double t36 = (dydu * t18); + double t40 = (dydu * t9); + double t41 = d3xdu3*d3xdu3;//(d3xdu3 ^ 2); + double t47 = (dydu * t4); + double t49 = d5ydu5 * t1 - 4 * d4ydu4 * t4 * d2xdu2 + + 12 * d3ydu3 * t9 * t11 - 6 * t14 * d3xdu3 + - 24 * d2ydu2 * t18 * t20 + 24 * t23 * t24 + - 4 * t27 * d4xdu4 + 24 * dydu / t17 / dxdu * t33 + - 36 * t36 * t11 * d3xdu3 + 6 * t40 * t41 + + 8 * t40 * d2xdu2 * d4xdu4 - t47 * d5xdu5; + double t63 = d4ydu4 * t1 - 3 * t14 * d2xdu2 + 6 * t23 * t11 + - 3 * t27 * d3xdu3 - 6 * t36 * t20 + + 6 * t40 * t24 - t47 * d4xdu4; + double t73 = d3ydu3 * t1 - 2 * t27 * d2xdu2 + 2 * t40 * t11 + - t47 * d3xdu3; + double t77 = t73 * t4; + double t82 = d2ydu2 * t1 - t47 * d2xdu2; + double t86 = t82 * t9; + double t89 = t82 * t4; + double t99 = t63 * t1 - 2 * t77 * d2xdu2 + 2 * t86 * t11 + - t89 * d3xdu3; + double t105 = t73 * t1 - t89 * d2xdu2; + double t109 = t105 * t4; + double d5ydx5 = (((t49 * t1 - 3 * t63 * t4 * d2xdu2 + + 6 * t73 * t9 * t11 - 3 * t77 * d3xdu3 + - 6 * t82 * t18 * t20 + + 6 * t86 * t24 - t89 * d4xdu4) * t1 + - 2 * t99 * t4 * d2xdu2 + + 2 * t105 * t9 * t11 - t109 * d3xdu3) * t1 + - (t99 * t1 - t109 * d2xdu2) * t4 * d2xdu2) * t1; val = d5ydx5; - } - break; - case 6: - { - double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); - double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); - double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); - double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); - double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); - double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); - double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4); - double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4); - double d5xdu5=calcQuinticBezierCurveDerivU(u,xpts,5); - double d5ydu5=calcQuinticBezierCurveDerivU(u,ypts,5); - double d6xdu6=calcQuinticBezierCurveDerivU(u,xpts,6); - double d6ydu6=calcQuinticBezierCurveDerivU(u,ypts,6); - - double t1 = dxdu*dxdu;//(dxdu ^ 2); - double t3 = (0.1e1 / t1 / dxdu); - double t5 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); - double t8 = t1*t1;//(t1 ^ 2); - double t9 = 1 / t8; - double t11 = (t5 * d2xdu2); - double t14 = (d3ydu3 * t3); - double t15 = (d2xdu2 * d3xdu3); - double t19 = (0.1e1 / t8 / dxdu); - double t21 = t5*t5;//(t5 ^ 2); - double t24 = (d2ydu2 * t9); - double t25 = (t5 * d3xdu3); - double t28 = (d2ydu2 * t3); - double t29 = d3xdu3*d3xdu3;//(d3xdu3 ^ 2); - double t32 = (d2xdu2 * d4xdu4); - double t41 = (dydu * t19); - double t45 = (dydu * t9); - double t49 = (dydu * t3); - double t56 = 1 / dxdu; - double t61 = 1 / t1; - double t62 = (dydu * t61); - double t67 = (d4ydu4 * t61); - double t70 = (d2ydu2 * t61); - double t73 = (d3ydu3 * t61); - double t76 = 20 * d4ydu4 * t3 * t5 - 60 * d3ydu3 * t9 * t11 - + 60 * t14 * t15 + 120 * d2ydu2 * t19 * t21 - - 180 * t24 * t25 - + 30 * t28 * t29 + 40 * t28 * t32 - - 120 * dydu / t8 / t1 * t21 * d2xdu2 - + 240 * t41 *t11*d3xdu3 - - 60 * t45 * t5 * d4xdu4 + 20 * t49 * d3xdu3 * d4xdu4 - + 10 * t49 * d2xdu2 * d5xdu5 + d6ydu6 * t56 - - 90 * t45 * d2xdu2 * t29 - t62 * d6xdu6 - - 5 * d5ydu5 * t61 * d2xdu2 - 10 * t67 * d3xdu3 - - 5 * t70 * d5xdu5 - 10 * t73 * d4xdu4; - - double t100 = d5ydu5 * t56 - 4 * t67 * d2xdu2 + 12 * t14 * t5 - - 6 * t73 * d3xdu3 - 24 * t24 * t11 + 24 * t28 * t15 - - 4 * t70 * d4xdu4 + 24 * t41 * t21 - 36 * t45 * t25 - + 6 * t49 * t29 + 8 * t49 * t32 - t62 * d5xdu5; - - double t116 = d4ydu4 * t56 - 3 * t73 * d2xdu2 + 6 * t28 * t5 - - 3 * t70 * d3xdu3 - 6 * t45 * t11 + 6 * t49 * t15 - - t62 * d4xdu4; - - double t120 = t116 * t61; - double t129 = d3ydu3 * t56 - 2 * t70 * d2xdu2 + 2 * t49 * t5 - - t62 * d3xdu3; - double t133 = t129 * t3; - double t136 = t129 * t61; - double t141 = d2ydu2 * t56 - t62 * d2xdu2; - double t145 = t141 * t9; - double t148 = t141 * t3; - double t153 = t141 * t61; - double t155 = t76 * t56 - 4 * t100 * t61 * d2xdu2 - + 12 * t116 * t3 * t5 - 6 * t120 * d3xdu3 - - 24 * t129 * t9 * t11 + 24 * t133 * t15 - - 4 * t136 * d4xdu4 - + 24 * t141 * t19 * t21 - 36 * t145 * t25 + 6 * t148 * t29 - + 8 * t148 * t32 - t153 * d5xdu5; - - double t169 = t100 * t56 - 3 * t120 * d2xdu2 + 6 * t133 * t5 - - 3 * t136 * d3xdu3 - 6 * t145 * t11 + 6 * t148 * t15 - - t153 * d4xdu4; - - double t179 = t116 * t56 - 2 * t136 * d2xdu2 + 2 * t148 * t5 - - t153 * d3xdu3; - - double t183 = t179 * t61; - double t188 = t129 * t56 - t153 * d2xdu2; - double t192 = t188 * t3; - double t195 = t188 * t61; - double t205 = t169 * t56 - 2 * t183 * d2xdu2 + 2 * t192 * t5 - - t195 * d3xdu3; - double t211 = t179 * t56 - t195 * d2xdu2; - double t215 = t211 * t61; - double d6ydx6 = (((t155 * t56 - 3 * t169 * t61 * d2xdu2 - + 6 * t179 * t3 * t5 - 3 * t183 * d3xdu3 - - 6 * t188 * t9 *t11 - + 6 * t192 * t15 - t195 * d4xdu4) * t56 - - 2 * t205 * t61 * d2xdu2 - + 2 * t211*t3*t5-t215*d3xdu3)*t56 - - (t205 * t56 - t215 * d2xdu2) * t61 * d2xdu2) * t56; + } + break; + case 6: { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); + double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); + double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4); + double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4); + double d5xdu5=calcQuinticBezierCurveDerivU(u,xpts,5); + double d5ydu5=calcQuinticBezierCurveDerivU(u,ypts,5); + double d6xdu6=calcQuinticBezierCurveDerivU(u,xpts,6); + double d6ydu6=calcQuinticBezierCurveDerivU(u,ypts,6); + + double t1 = dxdu*dxdu;//(dxdu ^ 2); + double t3 = (0.1e1 / t1 / dxdu); + double t5 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); + double t8 = t1*t1;//(t1 ^ 2); + double t9 = 1 / t8; + double t11 = (t5 * d2xdu2); + double t14 = (d3ydu3 * t3); + double t15 = (d2xdu2 * d3xdu3); + double t19 = (0.1e1 / t8 / dxdu); + double t21 = t5*t5;//(t5 ^ 2); + double t24 = (d2ydu2 * t9); + double t25 = (t5 * d3xdu3); + double t28 = (d2ydu2 * t3); + double t29 = d3xdu3*d3xdu3;//(d3xdu3 ^ 2); + double t32 = (d2xdu2 * d4xdu4); + double t41 = (dydu * t19); + double t45 = (dydu * t9); + double t49 = (dydu * t3); + double t56 = 1 / dxdu; + double t61 = 1 / t1; + double t62 = (dydu * t61); + double t67 = (d4ydu4 * t61); + double t70 = (d2ydu2 * t61); + double t73 = (d3ydu3 * t61); + double t76 = 20 * d4ydu4 * t3 * t5 - 60 * d3ydu3 * t9 * t11 + + 60 * t14 * t15 + 120 * d2ydu2 * t19 * t21 + - 180 * t24 * t25 + + 30 * t28 * t29 + 40 * t28 * t32 + - 120 * dydu / t8 / t1 * t21 * d2xdu2 + + 240 * t41 *t11*d3xdu3 + - 60 * t45 * t5 * d4xdu4 + 20 * t49 * d3xdu3 * d4xdu4 + + 10 * t49 * d2xdu2 * d5xdu5 + d6ydu6 * t56 + - 90 * t45 * d2xdu2 * t29 - t62 * d6xdu6 + - 5 * d5ydu5 * t61 * d2xdu2 - 10 * t67 * d3xdu3 + - 5 * t70 * d5xdu5 - 10 * t73 * d4xdu4; + + double t100 = d5ydu5 * t56 - 4 * t67 * d2xdu2 + 12 * t14 * t5 + - 6 * t73 * d3xdu3 - 24 * t24 * t11 + 24 * t28 * t15 + - 4 * t70 * d4xdu4 + 24 * t41 * t21 - 36 * t45 * t25 + + 6 * t49 * t29 + 8 * t49 * t32 - t62 * d5xdu5; + + double t116 = d4ydu4 * t56 - 3 * t73 * d2xdu2 + 6 * t28 * t5 + - 3 * t70 * d3xdu3 - 6 * t45 * t11 + 6 * t49 * t15 + - t62 * d4xdu4; + + double t120 = t116 * t61; + double t129 = d3ydu3 * t56 - 2 * t70 * d2xdu2 + 2 * t49 * t5 + - t62 * d3xdu3; + double t133 = t129 * t3; + double t136 = t129 * t61; + double t141 = d2ydu2 * t56 - t62 * d2xdu2; + double t145 = t141 * t9; + double t148 = t141 * t3; + double t153 = t141 * t61; + double t155 = t76 * t56 - 4 * t100 * t61 * d2xdu2 + + 12 * t116 * t3 * t5 - 6 * t120 * d3xdu3 + - 24 * t129 * t9 * t11 + 24 * t133 * t15 + - 4 * t136 * d4xdu4 + + 24 * t141 * t19 * t21 - 36 * t145 * t25 + 6 * t148 * t29 + + 8 * t148 * t32 - t153 * d5xdu5; + + double t169 = t100 * t56 - 3 * t120 * d2xdu2 + 6 * t133 * t5 + - 3 * t136 * d3xdu3 - 6 * t145 * t11 + 6 * t148 * t15 + - t153 * d4xdu4; + + double t179 = t116 * t56 - 2 * t136 * d2xdu2 + 2 * t148 * t5 + - t153 * d3xdu3; + + double t183 = t179 * t61; + double t188 = t129 * t56 - t153 * d2xdu2; + double t192 = t188 * t3; + double t195 = t188 * t61; + double t205 = t169 * t56 - 2 * t183 * d2xdu2 + 2 * t192 * t5 + - t195 * d3xdu3; + double t211 = t179 * t56 - t195 * d2xdu2; + double t215 = t211 * t61; + double d6ydx6 = (((t155 * t56 - 3 * t169 * t61 * d2xdu2 + + 6 * t179 * t3 * t5 - 3 * t183 * d3xdu3 + - 6 * t188 * t9 *t11 + + 6 * t192 * t15 - t195 * d4xdu4) * t56 + - 2 * t205 * t61 * d2xdu2 + + 2 * t211*t3*t5-t215*d3xdu3)*t56 + - (t205 * t56 - t215 * d2xdu2) * t61 * d2xdu2) * t56; val = d6ydx6; - } - break; + } + break; default: val = NAN; //SimTK::NaN; - } - return val; + } + return val; } /* Computational Cost Details @@ -798,45 +786,45 @@ d3y/du3 14 14 6 */ double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU( - double u, - const VectorNd& pts, - int order) + double u, + const VectorNd& pts, + int order) { double val = -1; /* - SimTK_ERRCHK_ALWAYS( (u>=0 && u <= 1) , - "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + SimTK_ERRCHK_ALWAYS( (u>=0 && u <= 1) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", "Error: double argument u must be between 0.0 and 1.0."); - SimTK_ERRCHK_ALWAYS( (pts.size()==6) , - "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + SimTK_ERRCHK_ALWAYS( (pts.size()==6) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", "Error: vector argument pts \nmust have a length of 6."); SimTK_ERRCHK_ALWAYS( (order >= 1), - "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", "Error: order must be greater than, or equal to 1"); */ - if( !(u>=0 && u <= 1) ){ - cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" - << "Error: double argument u must be between 0.0 and 1.0." - << endl; - assert(0); - abort(); + if( !(u>=0 && u <= 1) ) { + ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: double argument u must be between 0.0 and 1.0." + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( !(pts.size()==6) ){ - cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" - << "Error: vector argument pts must have a length of 6." - << endl; - assert(0); - abort(); + if( !(pts.size()==6) ) { + std::ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: vector argument pts must have a length of 6." + << endl; + throw RigidBodyDynamics::Errors::RBDLSizeMismatchError(errormsg.str()); } - if( !(order >= 1) ){ - cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" - << "Error: order must be greater than, or equal to 1" - << endl; - assert(0); - abort(); + if( !(order >= 1) ) { + std::ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: order must be greater than, or equal to 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } //Compute the Bezier point @@ -847,9 +835,8 @@ double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU( double p4 = pts[4]; double p5 = pts[5]; - switch(order){ - case 1: - { + switch(order) { + case 1: { double t1 = u*u;//u ^ 2; double t2 = t1*t1;//t1 ^ 2; double t4 = t1 * u; @@ -860,17 +847,16 @@ double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU( double t11 = t4 * 0.80e2; double t12 = t1 * 0.90e2; double t16 = t2 * 0.50e2; - val = p0 * (t2 * (-0.5e1) + t5 - t6 + t7 - 0.5e1) - + p1 * (t10 - t11 + t12 + u * (-0.40e2) + 0.5e1) - + p2 * (-t16 + t4 * 0.120e3 - t12 + t7) - + p3 * (t16 - t11 + t6) - + p4 * (-t10 + t5) - + p5 * t2 * 0.5e1; + val = p0 * (t2 * (-0.5e1) + t5 - t6 + t7 - 0.5e1) + + p1 * (t10 - t11 + t12 + u * (-0.40e2) + 0.5e1) + + p2 * (-t16 + t4 * 0.120e3 - t12 + t7) + + p3 * (t16 - t11 + t6) + + p4 * (-t10 + t5) + + p5 * t2 * 0.5e1; - } - break; - case 2: - { + } + break; + case 2: { double t1 = u*u;//u ^ 2; double t2 = t1 * u; double t4 = t1 * 0.60e2; @@ -879,53 +865,50 @@ double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU( double t9 = t1 * 0.240e3; double t10 = u * 0.180e3; double t13 = t2 * 0.200e3; - val = p0 * (t2 * (-0.20e2) + t4 - t5 + 0.20e2) - + p1 * (t8 - t9 + t10 - 0.40e2) - + p2 * (-t13 + t1 * 0.360e3 - t10 + 0.20e2) - + p3 * (t13 - t9 + t5) - + p4 * (-t8 + t4) - + p5 * t2 * 0.20e2; + val = p0 * (t2 * (-0.20e2) + t4 - t5 + 0.20e2) + + p1 * (t8 - t9 + t10 - 0.40e2) + + p2 * (-t13 + t1 * 0.360e3 - t10 + 0.20e2) + + p3 * (t13 - t9 + t5) + + p4 * (-t8 + t4) + + p5 * t2 * 0.20e2; - } - break; - case 3: - { + } + break; + case 3: { double t1 = u*u;//u ^ 2; double t3 = u * 0.120e3; double t6 = t1 * 0.300e3; double t7 = u * 0.480e3; double t10 = t1 * 0.600e3; - val = p0 * (t1 * (-0.60e2) + t3 - 0.60e2) - + p1 * (t6 - t7 + 0.180e3) - + p2 * (-t10 + u * 0.720e3 - 0.180e3) - + p3 * (t10 - t7 + 0.60e2) - + p4 * (-t6 + t3) - + p5 * t1 * 0.60e2; + val = p0 * (t1 * (-0.60e2) + t3 - 0.60e2) + + p1 * (t6 - t7 + 0.180e3) + + p2 * (-t10 + u * 0.720e3 - 0.180e3) + + p3 * (t10 - t7 + 0.60e2) + + p4 * (-t6 + t3) + + p5 * t1 * 0.60e2; - } - break; - case 4: - { + } + break; + case 4: { double t4 = u * 0.600e3; double t7 = u * 0.1200e4; - val = p0 * (u * (-0.120e3) + 0.120e3) - + p1 * (t4 - 0.480e3) - + p2 * (-t7 + 0.720e3) - + p3 * (t7 - 0.480e3) - + p4 * (-t4 + 0.120e3) - + p5 * u * 0.120e3; - } - break; - case 5: - { - val = p0 * (-0.120e3) - + p1 * 0.600e3 - + p2 * (-0.1200e4) - + p3 * 0.1200e4 - + p4 * (-0.600e3) - + p5 * 0.120e3; - } - break; + val = p0 * (u * (-0.120e3) + 0.120e3) + + p1 * (t4 - 0.480e3) + + p2 * (-t7 + 0.720e3) + + p3 * (t7 - 0.480e3) + + p4 * (-t4 + 0.120e3) + + p5 * u * 0.120e3; + } + break; + case 5: { + val = p0 * (-0.120e3) + + p1 * 0.600e3 + + p2 * (-0.1200e4) + + p3 * 0.1200e4 + + p4 * (-0.600e3) + + p5 * 0.120e3; + } + break; default: val=0; @@ -936,13 +919,14 @@ double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU( } -double SegmentedQuinticBezierToolkit::clampU(double u){ +double SegmentedQuinticBezierToolkit::clampU(double u) +{ double uC = u; - if(u<0.0){ - uC=0; + if(u<0.0) { + uC=0; } - if(u>1.0){ - uC=1; + if(u>1.0) { + uC=1; } return uC; } @@ -952,47 +936,49 @@ double SegmentedQuinticBezierToolkit::clampU(double u){ Geuss calculation 1 1 1 Newton Iter - f 21 20 13 + f 21 20 13 df 20 19 11 - update 4 1 3 6 + update 4 1 3 6 total 4 1 41 42 30 \endverbatim - To evaluate u to SimTK::Eps*100 this typically involves 2 Newton + To evaluate u to SimTK::Eps*100 this typically involves 2 Newton iterations, yielding a total cost of \verbatim Comparisons Div Mult Additions Assignments eval U 7+8=15 2 82 42 60 */ -double SegmentedQuinticBezierToolkit::calcU(double ax, - const VectorNd& bezierPtsX, - double tol, - int maxIter) +double SegmentedQuinticBezierToolkit::calcU(double ax, + const VectorNd& bezierPtsX, + double tol, + int maxIter) { //Check to make sure that ax is in the curve domain double minX = std::numeric_limits::max(); double maxX = -minX; - for(int i=0; i maxX) - maxX = bezierPtsX[i]; - if(bezierPtsX[i] < minX) - minX = bezierPtsX[i]; + for(int i=0; i maxX) { + maxX = bezierPtsX[i]; + } + if(bezierPtsX[i] < minX) { + minX = bezierPtsX[i]; + } } /* - SimTK_ERRCHK_ALWAYS( ax >= minX && ax <= maxX, - "SegmentedQuinticBezierToolkit::calcU", + SimTK_ERRCHK_ALWAYS( ax >= minX && ax <= maxX, + "SegmentedQuinticBezierToolkit::calcU", "Error: input ax was not in the domain of the Bezier curve specified \n" "by the control points in bezierPtsX."); */ - if( !(ax >= minX && ax <= maxX) ){ - cerr << "SegmentedQuinticBezierToolkit::calcU:" - << "Error: input ax was not in the domain of the " - << "Bezier curve specified by the control points in bezierPtsX." - << endl; - assert(0); - abort(); + if( !(ax >= minX && ax <= maxX) ) { + std::ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::calcU:" + << "Error: input ax was not in the domain of the " + << "Bezier curve specified by the control points in bezierPtsX." + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } double u = ax/(maxX-minX); @@ -1006,7 +992,7 @@ double SegmentedQuinticBezierToolkit::calcU(double ax, //as these curves, though C2, can be so nonlinear //that the Newton method oscillates unless it is //close to the initial solution. - if(abs(f) > tol){ + if(abs(f) > tol) { double uL = 0; double uR = 1; @@ -1016,24 +1002,24 @@ double SegmentedQuinticBezierToolkit::calcU(double ax, int iterBisection = 0; while(iterBisection < MAXITER_INITIALSOLN - && min(abs(fL),abs(fR)) > UTOL_INITIALSOLN ){ + && min(abs(fL),abs(fR)) > UTOL_INITIALSOLN ) { u = 0.5*(uL+uR); f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax; - if(signbit(f) == signbit(fL)){ + if(signbit(f) == signbit(fL)) { fL = f; uL = u; - }else{ + } else { fR = f; uR = u; } iterBisection++; } - if(abs(fL) < abs(fR)){ + if(abs(fL) < abs(fR)) { u = uL; f = fL; - }else{ + } else { u = uR; f = fR; } @@ -1050,18 +1036,18 @@ double SegmentedQuinticBezierToolkit::calcU(double ax, double perturbation01 = 0.0; //Take Newton steps to the desired tolerance while((abs(f) > min(tol,UTOL_DESIRED)) - && (iter < maxIter) - && (pathologic == false) ){ + && (iter < maxIter) + && (pathologic == false) ) { //Take a Newton step df = calcQuinticBezierCurveDerivU(u,bezierPtsX,1); - - if(abs(df) > 0){ + + if(abs(df) > 0) { du = -f/df; u = u + stepLength*du; u = clampU(u); fprev = f; f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax; - }else{ + } else { //This should never happen. If we are unluky enough to get here //purturb the current solution and continue until we run out of //iterations. @@ -1070,24 +1056,22 @@ double SegmentedQuinticBezierToolkit::calcU(double ax, u = clampU(u); } - iter++; + iter++; } - //Check for convergence - if( abs(f) > tol ){ + //Check for convergence + if( abs(f) > tol ) { std::stringstream errMsg; errMsg.precision(17); errMsg << "SegmentedQuinticBezierToolkit::calcU:" << endl - << "Error: desired tolerance of " << tol << endl - << " on U not met by the Newton iteration." << endl - << " A tolerance of " << f << " was reached." << endl - << " Curve range x(u): " << minX << "-" << maxX << endl - << " Desired x: " << ax << " closest u " << u << endl - << " Bezier points " << endl << bezierPtsX << endl; - cerr << errMsg.str(); - assert(0); - abort(); - } + << "Error: desired tolerance of " << tol << endl + << " on U not met by the Newton iteration." << endl + << " A tolerance of " << f << " was reached." << endl + << " Curve range x(u): " << minX << "-" << maxX << endl + << " Desired x: " << ax << " closest u " << u << endl + << " Bezier points " << endl << bezierPtsX << endl; + throw RigidBodyDynamics::Errors::RBDLError(errMsg.str()); + } return u; } @@ -1096,76 +1080,76 @@ double SegmentedQuinticBezierToolkit::calcU(double ax, Cost: n comparisons, for a quintic Bezier curve with n-spline sections Comp Div Mult Add Assignments -Cost 3*n+2 1*n 3 - +Cost 3*n+2 1*n 3 + */ -int SegmentedQuinticBezierToolkit::calcIndex(double x, - const MatrixNd& bezierPtsX) +int SegmentedQuinticBezierToolkit::calcIndex(double x, + const MatrixNd& bezierPtsX) { int idx = 0; bool flag_found = false; - for(int i=0; i= bezierPtsX(0,i) && x < bezierPtsX(5,i) ){ - idx = i; - i = bezierPtsX.cols(); - flag_found = true; - } + for(int i=0; i= bezierPtsX(0,i) && x < bezierPtsX(5,i) ) { + idx = i; + i = bezierPtsX.cols(); + flag_found = true; + } } //Check if the value x is identically the last point - if(flag_found == false && x == bezierPtsX(5,bezierPtsX.cols()-1)){ - idx = bezierPtsX.cols()-1; - flag_found = true; + if(flag_found == false && x == bezierPtsX(5,bezierPtsX.cols()-1)) { + idx = bezierPtsX.cols()-1; + flag_found = true; } /* - SimTK_ERRCHK_ALWAYS( (flag_found == true), - "SegmentedQuinticBezierToolkit::calcIndex", + SimTK_ERRCHK_ALWAYS( (flag_found == true), + "SegmentedQuinticBezierToolkit::calcIndex", "Error: A value of x was used that is not within the Bezier curve set."); */ - if( !(flag_found == true)){ - cerr << "SegmentedQuinticBezierToolkit::calcIndex" - << "Error: A value of x was used that is not" - << " within the Bezier curve set." << endl; - assert(0); - abort(); - } - + if( !(flag_found == true)) { + ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::calcIndex" + << "Error: A value of x was used that is not" + << " within the Bezier curve set." << endl; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); + } + return idx; } -int SegmentedQuinticBezierToolkit::calcIndex(double x, - const std::vector& bezierPtsX) +int SegmentedQuinticBezierToolkit::calcIndex(double x, + const std::vector& bezierPtsX) { int idx = 0; bool flag_found = false; - int n = bezierPtsX.size(); - for(int i=0; i= bezierPtsX[i][0] && x < bezierPtsX[i][5] ){ + int n = bezierPtsX.size(); + for(int i=0; i= bezierPtsX[i][0] && x < bezierPtsX[i][5] ) { idx = i; flag_found = true; - break; + break; } } //Check if the value x is identically the last point - if(!flag_found && x == bezierPtsX[n-1][5]){ - idx = n-1; - flag_found = true; + if(!flag_found && x == bezierPtsX[n-1][5]) { + idx = n-1; + flag_found = true; } - if(!(flag_found == true)){ - cerr << "SegmentedQuinticBezierToolkit::calcIndex " - << "Error: A value of x was used that is not " - << "within the Bezier curve set." - << endl; - assert(0); - abort(); + if(!(flag_found == true)) { + ostringstream errormsg; + errormsg << "SegmentedQuinticBezierToolkit::calcIndex " + << "Error: A value of x was used that is not " + << "within the Bezier curve set." + << endl; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); } return idx; @@ -1192,22 +1176,22 @@ RK45 on 1pt 6*(26 2 103 65 76) Total 156 12 618 390 456 \endverbatim -Typically the integral is evaluated 100 times per section in order to +Typically the integral is evaluated 100 times per section in order to build an accurate spline-fit of the integrated function. Once again, ignoring the overhead of the integrator, the function evaluations alone for the current example would be \verbatim RK45 on 100pts per section, over 3 sections - Comp Div Mult Additions Assignments + Comp Div Mult Additions Assignments 3*100*(156 12 618 390 456 Total 46,800 3600 185,400 117,000 136,000 */ /* MatrixNd SegmentedQuinticBezierToolkit::calcNumIntBezierYfcnX( - const VectorNd& vX, - double ic0, double intAcc, + const VectorNd& vX, + double ic0, double intAcc, double uTol, int uMaxIter, const MatrixNd& mX, const MatrixNd& mY, const SimTK::Array_& aSplineUX, @@ -1229,7 +1213,7 @@ MatrixNd SegmentedQuinticBezierToolkit::calcNumIntBezierYfcnX( //so that I don't make a mistake double startTime = vX(0); double endTime = vX(vX.size()-1); - + if(flag_intLeftToRight){ bdata._startValue = startTime; }else{ @@ -1247,11 +1231,11 @@ MatrixNd SegmentedQuinticBezierToolkit::calcNumIntBezierYfcnX( integ.setReturnEveryInternalStep(false); integ.initialize(initState); - int idx = 0; + int idx = 0; double nextTimeInterval = 0; Integrator::SuccessfulStepStatus status; - while (idx < vX.nelt()) { + while (idx < vX.nelt()) { if(idx < vX.nelt()){ if(flag_intLeftToRight){ nextTimeInterval = vX(idx); @@ -1266,12 +1250,12 @@ MatrixNd SegmentedQuinticBezierToolkit::calcNumIntBezierYfcnX( if (status == Integrator::EndOfSimulation) break; - + const State& state = integ.getState(); if(flag_intLeftToRight){ intXY(idx,0) = nextTimeInterval; - intXY(idx,1) = (double)state.getZ()[0]; + intXY(idx,1) = (double)state.getZ()[0]; }else{ intXY(vX.size()-idx-1,0) = vX(vX.size()-idx-1); intXY(vX.size()-idx-1,1) = (double)state.getZ()[0]; diff --git a/addons/geometry/SmoothSegmentedFunction.cc b/addons/geometry/SmoothSegmentedFunction.cc index 69094d9..b28faff 100644 --- a/addons/geometry/SmoothSegmentedFunction.cc +++ b/addons/geometry/SmoothSegmentedFunction.cc @@ -20,25 +20,26 @@ * See the License for the specific language governing permissions and * * limitations under the License. * * -------------------------------------------------------------------------- */ - /* - Update: - This is a port of the original code so that it will work with - the multibody code RBDL written by Martin Felis. - - Author: - Matthew Millard - - Date: - Nov 2015 +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 */ - + //============================================================================= // INCLUDES //============================================================================= #include "SmoothSegmentedFunction.h" #include #include +#include //============================================================================= // STATICS @@ -62,10 +63,10 @@ static int NUM_SAMPLE_PTS = 100; ========================================================================= WITHOUT INTEGRAL _________________________________________________________________________ - Function Comp Div Mult Add Assignments + Function Comp Div Mult Add Assignments _________________________________________________________________________ member assign M:2, 9 - curve gen: m,m*100 m m*100 m m*100*(4) + curve gen: m,m*100 m m*100 m m*100*(4) +m*100(3) +m*100*(3) Function detail @@ -77,26 +78,26 @@ static int NUM_SAMPLE_PTS = 100; m*100 SegmentedQuinticBezierToolkit:: calcQuinticBezierCurveVal Cost: Mult Add Assignments - 21 20 13 + 21 20 13 Total ~typically > 2100*m multiplications, additions, > 1000*m assignments > 100*m divisions - _________________________________________________________________________ + _________________________________________________________________________ Comp Div Mult Add Assignments Total: m+m*100(3) m*100 m*100*21 m*100*20 m*100*13 - +m+m*100*3 +m*100*4+9+M:2 + +m+m*100*3 +m*100*4+9+M:2 + m*Cost(SimTK::SplineFitter ...) ========================================================================= ADDITIONAL COST OF COMPUTING THE INTEGRAL CURVE - - Comp Div Mult Add Assign + + Comp Div Mult Add Assign RK45 Fn.Eval m*100*(156 12 618 390 456) RK45 Overhead m*100*(? ? ? ? ? ) Spline cost m*100*(? ? ? ? ? ) Total: ~typically > 100,000's mult, additions, assignments - > 40,000 comparisions + > 40,000 comparisions > 3000 divisions ========================================================================= @@ -106,59 +107,61 @@ static int NUM_SAMPLE_PTS = 100; N.B. These costs are dependent on SegmentedQuinticBezierToolkit */ SmoothSegmentedFunction:: - SmoothSegmentedFunction( - const RigidBodyDynamics::Math::MatrixNd& mX, - const RigidBodyDynamics::Math::MatrixNd& mY, - double x0, double x1, - double y0, double y1, - double dydx0, double dydx1, - const std::string& name): - _x0(x0),_x1(x1),_y0(y0),_y1(y1),_dydx0(dydx0),_dydx1(dydx1), - _name(name) +SmoothSegmentedFunction( + const RigidBodyDynamics::Math::MatrixNd& mX, + const RigidBodyDynamics::Math::MatrixNd& mY, + double x0, double x1, + double y0, double y1, + double dydx0, double dydx1, + const std::string& name): + _x0(x0),_x1(x1),_y0(y0),_y1(y1),_dydx0(dydx0),_dydx1(dydx1), + _name(name) { - + _numBezierSections = mX.cols(); _mXVec.resize(_numBezierSections); _mYVec.resize(_numBezierSections); - for(int s=0; s < _numBezierSections; s++){ + for(int s=0; s < _numBezierSections; s++) { _mXVec[s] = mX.col(s); _mYVec[s] = mY.col(s); } } //============================================================================== - SmoothSegmentedFunction::SmoothSegmentedFunction(): - _x0(NAN),_x1(NAN), - _y0(NAN),_y1(NAN), - _dydx0(NAN),_dydx1(NAN),_name("NOT_YET_SET") - { - //_arraySplineUX.resize(0); - _mXVec.resize(0); - _mYVec.resize(0); - //_splineYintX = SimTK::Spline(); - _numBezierSections = (int)NAN; - } +SmoothSegmentedFunction::SmoothSegmentedFunction(): + _x0(NAN),_x1(NAN), + _y0(NAN),_y1(NAN), + _dydx0(NAN),_dydx1(NAN),_name("NOT_YET_SET") +{ + //_arraySplineUX.resize(0); + _mXVec.resize(0); + _mYVec.resize(0); + //_splineYintX = SimTK::Spline(); + _numBezierSections = (int)NAN; +} //============================================================================== void SmoothSegmentedFunction:: - updSmoothSegmentedFunction( - const RigidBodyDynamics::Math::MatrixNd& mX, - const RigidBodyDynamics::Math::MatrixNd& mY, - double x0, double x1, - double y0, double y1, - double dydx0, double dydx1, - const std::string& name) +updSmoothSegmentedFunction( + const RigidBodyDynamics::Math::MatrixNd& mX, + const RigidBodyDynamics::Math::MatrixNd& mY, + double x0, double x1, + double y0, double y1, + double dydx0, double dydx1, + const std::string& name) { - if(mX.rows() != 6 || mY.rows() != 6 || mX.cols() != mY.cols() ){ - cerr<<"SmoothSegmentedFunction::updSmoothSegmentedFunction " - << _name.c_str() - <<": matrices mX and mY must have 6 rows, and the same" - <<" number of columns." - << endl; - assert(0); - abort(); + if(mX.rows() != 6 || mY.rows() != 6 || mX.cols() != mY.cols() ) { + ostringstream errormsg; + //! [Size Mismatch] + errormsg << "SmoothSegmentedFunction::updSmoothSegmentedFunction " + << _name.c_str() + << ": matrices mX and mY must have 6 rows, and the same" + << " number of columns." + << endl; + throw RigidBodyDynamics::Errors::RBDLSizeMismatchError(errormsg.str()); + //! [Size Mismatch] } _x0 =x0; @@ -167,14 +170,14 @@ void SmoothSegmentedFunction:: _y1 =y1; _dydx0=dydx0; _dydx1=dydx1; - - if(mX.cols() != _mXVec.size()){ + + if(mX.cols() != _mXVec.size()) { _mXVec.resize(mX.cols()); _mYVec.resize(mY.cols()); } _numBezierSections = mX.cols(); - for(int s=0; s < mX.cols(); s++){ + for(int s=0; s < mX.cols(); s++) { _mXVec[s] = mX.col(s); _mYVec[s] = mY.col(s); } @@ -189,8 +192,8 @@ void SmoothSegmentedFunction::shift(double xShift, double yShift) _y0 += yShift; _y1 += yShift; - for(int i=0; i<_mXVec.size();++i){ - for(int j=0; j<_mXVec.at(i).rows();++j){ + for(int i=0; i<_mXVec.size(); ++i) { + for(int j=0; j<_mXVec.at(i).rows(); ++j) { _mXVec.at(i)[j] += xShift; _mYVec.at(i)[j] += yShift; } @@ -201,15 +204,17 @@ void SmoothSegmentedFunction::shift(double xShift, double yShift) void SmoothSegmentedFunction::scale(double xScale, double yScale) { - if( abs( xScale ) <= SQRTEPS){ - cerr<<"SmoothSegmentedFunction::scale " - << _name.c_str() - <<": xScale must be greater than sqrt(eps). Setting xScale to such" - <<" a small value will cause the slope of the curve to approach " - <<" infinity, or become undefined." - << endl; - assert(0); - abort(); + if( abs( xScale ) <= SQRTEPS) { + ostringstream errormsg; + //! [Invalid Parameter] + errormsg << "SmoothSegmentedFunction::scale " + << _name.c_str() + << ": xScale must be greater than sqrt(eps). Setting xScale to such" + << " a small value will cause the slope of the curve to approach " + << " infinity, or become undefined." + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + //! [Invalid Parameter] } _x0 *= xScale; @@ -219,77 +224,132 @@ void SmoothSegmentedFunction::scale(double xScale, double yScale) _dydx0 *= yScale/xScale; _dydx1 *= yScale/xScale; - for(int i=0; i<_mXVec.size();++i){ - for(int j=0; j<_mXVec.at(i).rows();++j){ + for(int i=0; i<_mXVec.size(); ++i) { + for(int j=0; j<_mXVec.at(i).rows(); ++j) { _mXVec.at(i)[j] *= xScale; _mYVec.at(i)[j] *= yScale; } } - + } //============================================================================== - /*Detailed Computational Costs - ________________________________________________________________________ - If x is in the Bezier Curve - Name Comp. Div. Mult. Add. Assign. +/*Detailed Computational Costs +________________________________________________________________________ + If x is in the Bezier Curve + Name Comp. Div. Mult. Add. Assign. _______________________________________________________________________ - SegmentedQuinticBezierToolkit:: - calcIndex 3*m+2 1*m 3 - *calcU 15 2 82 42 60 - calcQuinticBezierCurveVal 21 20 13 - total 15+3*m+2 2 103 62+1*m 76 + SegmentedQuinticBezierToolkit:: + calcIndex 3*m+2 1*m 3 + *calcU 15 2 82 42 60 + calcQuinticBezierCurveVal 21 20 13 + total 15+3*m+2 2 103 62+1*m 76 - *Approximate. Uses iteration + *Approximate. Uses iteration ________________________________________________________________________ If x is in the linear region - Name Comp. Div. Mult. Add. Assign. - 1 1 2 1 + Name Comp. Div. Mult. Add. Assign. + 1 1 2 1 ________________________________________________________________________ - - */ + +*/ double SmoothSegmentedFunction::calcValue(double x) const { double yVal = 0; - if(x >= _x0 && x <= _x1 ) - { + if(x >= _x0 && x <= _x1 ) { int idx = SegmentedQuinticBezierToolkit::calcIndex(x,_mXVec); double u = SegmentedQuinticBezierToolkit:: - calcU(x,_mXVec[idx], UTOL, MAXITER); + calcU(x,_mXVec[idx], UTOL, MAXITER); yVal = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCurveVal(u,_mYVec[idx]); - }else{ - if(x < _x0){ - yVal = _y0 + _dydx0*(x-_x0); - }else{ - yVal = _y1 + _dydx1*(x-_x1); - } + calcQuinticBezierCurveVal(u,_mYVec[idx]); + } else { + if(x < _x0) { + yVal = _y0 + _dydx0*(x-_x0); + } else { + yVal = _y1 + _dydx1*(x-_x1); + } } return yVal; } + +double SmoothSegmentedFunction::calcInverseValue(double y, + double xGuess) const +{ + + double xVal = 0; + + int idx = -1; + double yLeft = 0.; + double yRight = 0; + double xLeft = 0.; + double xRight = 0; + double xDist = 0; + double xDistBest = numeric_limits::infinity(); + + for(unsigned int i=0; i < _numBezierSections; ++i) { + + yLeft = y - _mYVec[i][0]; + yRight= _mYVec[i][5] - y; + + xLeft = xGuess - _mXVec[i][0]; + xRight= _mXVec[i][5] - xGuess; + xDist = fabs(xLeft)+fabs(xRight); + + //If the y value is in the spline interval and the + //x interval is closer to the guess, update the interval + if(yLeft*yRight >= 0 && xDist < xDistBest) { + idx = i; + xDistBest = xDist; + } + + } + + //y value is in the linear region + if(idx == -1) { + if( (y-_y1)*_dydx1 >= 0 + && fabs(_dydx1) > numeric_limits< double >::epsilon() ) { + xVal = (y-_y1)/_dydx1 + _x1; + } else if( (_y0-y)*_dydx0 >= 0 + && fabs(_dydx0) > numeric_limits< double >::epsilon() ) { + xVal = (y-_y0)/_dydx0 + _x0; + } else { + xVal = numeric_limits::signaling_NaN(); + } + + } else { + //y is in an interval + double u = SegmentedQuinticBezierToolkit:: + calcU(y,_mYVec[idx], UTOL, MAXITER); + xVal = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveVal(u,_mXVec[idx]); + } + + return xVal; + +} + double SmoothSegmentedFunction::calcValue( - const RigidBodyDynamics::Math::VectorNd& ax) const + const RigidBodyDynamics::Math::VectorNd& ax) const { - - if( !(ax.size() == 1) ){ - cerr<<"SmoothSegmentedFunction::calcValue " << _name.c_str() - <<": Argument x must have only 1 element, as this function is " - << "designed only for 1D functions, but a function with " - << ax.size() << " elements was entered" - << endl; - assert(0); - abort(); - - } - - return calcValue(ax[0]); + + if( !(ax.size() == 1) ) { + ostringstream errormsg; + errormsg << "SmoothSegmentedFunction::calcValue " << _name.c_str() + << ": Argument x must have only 1 element, as this function is " + << "designed only for 1D functions, but a function with " + << ax.size() << " elements was entered" + << endl; + throw RigidBodyDynamics::Errors::RBDLDofMismatchError(errormsg.str()); + } + + return calcValue(ax[0]); } /*Detailed Computational Costs @@ -299,8 +359,8 @@ If x is in the Bezier Curve, and dy/dx is being evaluated _______________________________________________________________________ Overhead: SegmentedQuinticBezierToolkit:: - calcIndex 3*m+2 1*m 3 - *calcU 15 2 82 42 60 + calcIndex 3*m+2 1*m 3 + *calcU 15 2 82 42 60 Derivative Evaluation: **calcQuinticBezierCurveDYDX 21 20 13 dy/du 20 19 11 @@ -322,35 +382,37 @@ ________________________________________________________________________ double SmoothSegmentedFunction::calcDerivative(double x, int order) const { //return calcDerivative( SimTK::Array_(order,0), - // RigidBodyDynamics::Math::VectorNd(1,x)); + // RigidBodyDynamics::Math::VectorNd(1,x)); double yVal = 0; //QUINTIC SPLINE - - if(order==0){ - yVal = calcValue(x); - }else{ - if(x >= _x0 && x <= _x1){ - int idx = SegmentedQuinticBezierToolkit::calcIndex(x,_mXVec); - double u = SegmentedQuinticBezierToolkit:: - calcU(x,_mXVec[idx],UTOL,MAXITER); - yVal = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCurveDerivDYDX(u, _mXVec[idx], - _mYVec[idx], order); -/* - std::cout << _mX(3, idx) << std::endl; - std::cout << _mX(idx) << std::endl;*/ - }else{ - if(order == 1){ - if(x < _x0){ - yVal = _dydx0; - }else{ - yVal = _dydx1;} - }else{ - yVal = 0;} + + if(order==0) { + yVal = calcValue(x); + } else { + if(x >= _x0 && x <= _x1) { + int idx = SegmentedQuinticBezierToolkit::calcIndex(x,_mXVec); + double u = SegmentedQuinticBezierToolkit:: + calcU(x,_mXVec[idx],UTOL,MAXITER); + yVal = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(u, _mXVec[idx], + _mYVec[idx], order); + /* + std::cout << _mX(3, idx) << std::endl; + std::cout << _mX(idx) << std::endl;*/ + } else { + if(order == 1) { + if(x < _x0) { + yVal = _dydx0; + } else { + yVal = _dydx1; } + } else { + yVal = 0; + } } + } return yVal; } @@ -358,8 +420,8 @@ double SmoothSegmentedFunction::calcDerivative(double x, int order) const double SmoothSegmentedFunction:: - calcDerivative( const std::vector& derivComponents, - const RigidBodyDynamics::Math::VectorNd& ax) const +calcDerivative( const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& ax) const { /* for(int i=0; i < (signed)derivComponents.size(); i++){ @@ -383,39 +445,39 @@ double SmoothSegmentedFunction:: _name.c_str(), ax.size()); */ - for(int i=0; i < (signed)derivComponents.size(); i++){ - if( !(derivComponents[i] == 0)){ - cerr << "SmoothSegmentedFunction::calcDerivative " - << _name.c_str() - <<": derivComponents can only be populated with 0's because " - << "SmoothSegmentedFunction is only valid for a 1D function," - << " but derivComponents had a value of " - << derivComponents[i] << " in it" - << endl; - assert(0); - abort(); - + for(int i=0; i < (signed)derivComponents.size(); i++) { + if( !(derivComponents[i] == 0)) { + ostringstream errormsg; + errormsg << "SmoothSegmentedFunction::calcDerivative " + << _name.c_str() + << ": derivComponents can only be populated with 0's because " + << "SmoothSegmentedFunction is only valid for a 1D function," + << " but derivComponents had a value of " + << derivComponents[i] << " in it" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } } - - if( !(derivComponents.size() <= 6)){ - cerr << "SmoothSegmentedFunction::calcDerivative " << _name.c_str() - << ": calcDerivative is only valid up to a 6th order derivative" - << " but derivComponents had a size of " - << derivComponents.size() - << endl; - assert(0); - abort(); + //! [Dof Mismatch] + if( !(derivComponents.size() <= 6)) { + ostringstream errormsg; + errormsg << "SmoothSegmentedFunction::calcDerivative " << _name.c_str() + << ": calcDerivative is only valid up to a 6th order derivative" + << " but derivComponents had a size of " + << derivComponents.size() + << endl; + throw RigidBodyDynamics::Errors::RBDLDofMismatchError(errormsg.str()); } - - if( !(ax.size() == 1) ){ - cerr << "SmoothSegmentedFunction::calcValue " << _name.c_str() - << ": Argument x must have only 1 element, as this function is " - << "designed only for 1D functions, but ax had a size of " - << ax.size() - << endl; - assert(0); - abort(); + //! [Dof Mismatch] + + if( !(ax.size() == 1) ) { + ostringstream errormsg; + errormsg << "SmoothSegmentedFunction::calcValue " << _name.c_str() + << ": Argument x must have only 1 element, as this function is " + << "designed only for 1D functions, but ax had a size of " + << ax.size() + << endl; + throw RigidBodyDynamics::Errors::RBDLDofMismatchError(errormsg.str()); } @@ -435,7 +497,7 @@ ________________________________________________________________________ If x is in the linear region Name Comp. Div. Mult. Add. Assign. - *spline.calcValue 1 2 3 1 + *spline.calcValue 1 2 3 1 integral eval 2 4 5 1 total 3 6 8 2 @@ -451,40 +513,40 @@ double SmoothSegmentedFunction::calcIntegral(double x) const "%s: This curve was not constructed with its integral because" "computeIntegral was false",_name.c_str()); - double yVal = 0; + double yVal = 0; if(x >= _x0 && x <= _x1){ yVal = _splineYintX.calcValue(RigidBodyDynamics::Math::VectorNd(1,x)); }else{ - //LINEAR EXTRAPOLATION + //LINEAR EXTRAPOLATION if(x < _x0){ RigidBodyDynamics::Math::VectorNd tmp(1); tmp(0) = _x0; double ic = _splineYintX.calcValue(tmp); if(_intx0x1){//Integrating left to right - yVal = _y0*(x-_x0) - + _dydx0*(x-_x0)*(x-_x0)*0.5 + yVal = _y0*(x-_x0) + + _dydx0*(x-_x0)*(x-_x0)*0.5 + ic; }else{//Integrating right to left - yVal = -_y0*(x-_x0) - - _dydx0*(x-_x0)*(x-_x0)*0.5 + yVal = -_y0*(x-_x0) + - _dydx0*(x-_x0)*(x-_x0)*0.5 + ic; - } + } }else{ RigidBodyDynamics::Math::VectorNd tmp(1); tmp(0) = _x1; double ic = _splineYintX.calcValue(tmp); if(_intx0x1){ - yVal = _y1*(x-_x1) - + _dydx1*(x-_x1)*(x-_x1)*0.5 + yVal = _y1*(x-_x1) + + _dydx1*(x-_x1)*(x-_x1)*0.5 + ic; }else{ - yVal = -_y1*(x-_x1) - - _dydx1*(x-_x1)*(x-_x1)*0.5 + yVal = -_y1*(x-_x1) + - _dydx1*(x-_x1)*(x-_x1)*0.5 + ic; } } - } - + } + return yVal; } */ @@ -517,23 +579,23 @@ std::string SmoothSegmentedFunction::getName() const return _name; } -void SmoothSegmentedFunction::setName(const std::string &name) +void SmoothSegmentedFunction::setName(const std::string &name) { _name = name; } -RigidBodyDynamics::Math::VectorNd - SmoothSegmentedFunction::getCurveDomain() const +RigidBodyDynamics::Math::VectorNd +SmoothSegmentedFunction::getCurveDomain() const { - RigidBodyDynamics::Math::VectorNd xrange(2); - - xrange[0] = 0; - xrange[1] = 0; + RigidBodyDynamics::Math::VectorNd domain(2); + + domain[0] = 0; + domain[1] = 0; if (!_mXVec.empty()) { - xrange[0] = _mXVec[0][0]; - xrange[1] = _mXVec[_mXVec.size()-1][_mXVec[0].size()-1]; + domain[0] = _mXVec[0][0]; + domain[1] = _mXVec[_mXVec.size()-1][_mXVec[0].size()-1]; } - return xrange; + return domain; } /////////////////////////////////////////////////////////////////////////////// @@ -550,74 +612,75 @@ _______________________________________________________________________ 119+21*m 14 574 294+7m 441 calcValue 21 20 13 - calcDerivative: dy/dx 1 40 38 22 + calcDerivative: dy/dx 1 40 38 22 : d2y/dx2 2 78 73 23 : d3y/dx3 4 118 105 58 : d4y/dx4 5 168 137 71 : d5y/dx5 7 236 170 88 : d6y/dx6 9 334 209 106 - **calcIntegral 7 2 3 1 + **calcIntegral 7 2 3 1 total per point 126+21*m 42 1571 1049 823 total per elbow 126k+21k*m 42k 1571k 1049k 823k - + *Approximate. Overhead associated with finding the correct Bezier - spline section, and evaluating u(x). + spline section, and evaluating u(x). Assumes 2 Newton iterations in calcU **Approximate. Includes estimated cost of evaluating a cubic spline with 100 knots */ -RigidBodyDynamics::Math::MatrixNd - SmoothSegmentedFunction::calcSampledCurve(int maxOrder, - double domainMin, - double domainMax) const{ +RigidBodyDynamics::Math::MatrixNd +SmoothSegmentedFunction::calcSampledCurve(int maxOrder, + double domainMin, + double domainMax) const +{ int pts = 1; //Number of points between each of the spline points used - //to fit u(x), and also the integral spline - if( !(maxOrder <= getMaxDerivativeOrder()) ){ - cerr << "SmoothSegmentedFunction::calcSampledCurve " - << "Derivative order past the maximum computed order requested" - << endl; - assert(0); - abort(); + //to fit u(x), and also the integral spline + if( !(maxOrder <= getMaxDerivativeOrder()) ) { + ostringstream errormsg; + errormsg << "SmoothSegmentedFunction::calcSampledCurve " + << "Derivative order past the maximum computed order requested" + << endl; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); } double x0,x1,delta; //y,dy,d1y,d2y,d3y,d4y,d5y,d6y,iy - RigidBodyDynamics::Math::VectorNd - midX(NUM_SAMPLE_PTS*_numBezierSections-(_numBezierSections-1)); - RigidBodyDynamics::Math::VectorNd x(NUM_SAMPLE_PTS); - - //Generate a sample of X values inside of the curve that is denser where - //the curve is more curvy. - double u; - int idx = 0; - for(int s=0; s < _numBezierSections; s++){ + RigidBodyDynamics::Math::VectorNd + midX(NUM_SAMPLE_PTS*_numBezierSections-(_numBezierSections-1)); + RigidBodyDynamics::Math::VectorNd x(NUM_SAMPLE_PTS); + + //Generate a sample of X values inside of the curve that is denser where + //the curve is more curvy. + double u; + int idx = 0; + for(int s=0; s < _numBezierSections; s++) { //Sample the local set for u and x - for(int i=0;i 1){ - //Skip the last point of a set that has another set of points - //after it. Why? The last point and the starting point of the - //next set are identical in value. - if(i<(NUM_SAMPLE_PTS-1) || s == (_numBezierSections-1)){ - midX[idx] = x[i]; - idx++; - } - }else{ - midX[idx] = x[i]; + for(int i=0; i 1) { + //Skip the last point of a set that has another set of points + //after it. Why? The last point and the starting point of the + //next set are identical in value. + if(i<(NUM_SAMPLE_PTS-1) || s == (_numBezierSections-1)) { + midX[idx] = x[i]; idx++; } + } else { + midX[idx] = x[i]; + idx++; } - } + } + } + - RigidBodyDynamics::Math::VectorNd xsmpl(pts*(midX.size()-1)+2*10*pts); - + RigidBodyDynamics::Math::MatrixNd results; /* @@ -634,80 +697,92 @@ RigidBodyDynamics::Math::MatrixNd d1y[0]=0; d2y[0] = 0; d2y[1] = 0; - for(int i=0;i<3;i++) + for(int i=0; i<3; i++) { d3y[i]=0; - for(int i=0;i<4;i++) + } + for(int i=0; i<4; i++) { d4y[i]=0; - for(int i=0;i<5;i++) + } + for(int i=0; i<5; i++) { d5y[i]=0; - for(int i=0;i<6;i++) + } + for(int i=0; i<6; i++) { d6y[i]=0; + } //generate some sample points in the extrapolated region idx = 0; x0 = _x0 - 0.1*(_x1-_x0); - if(domainMin < x0) + if(domainMin < x0) { x0 = domainMin; + } x1 = _x0; delta = (0.1)*(x1-x0)/(pts); - for(int j=0; j x1) + if(domainMax > x1) { x1 = domainMax; + } delta = (1.0/9.0)*(x1-x0)/(pts); - for(int j=0;j=1) - results(i,2) = calcDerivative(d1y,ax); - - if(maxOrder>=2) - results(i,3) = calcDerivative(d2y,ax); - - if(maxOrder>=3) - results(i,4) = calcDerivative(d3y,ax); - - if(maxOrder>=4) - results(i,5) = calcDerivative(d4y,ax); - - if(maxOrder>=5) - results(i,6) = calcDerivative(d5y,ax); - - if(maxOrder>=6) - results(i,7) = calcDerivative(d6y,ax); + if(maxOrder>=1) { + results(i,2) = calcDerivative(d1y,ax); + } + + if(maxOrder>=2) { + results(i,3) = calcDerivative(d2y,ax); + } + + if(maxOrder>=3) { + results(i,4) = calcDerivative(d3y,ax); + } + + if(maxOrder>=4) { + results(i,5) = calcDerivative(d4y,ax); + } + + if(maxOrder>=5) { + results(i,6) = calcDerivative(d5y,ax); + } + + if(maxOrder>=6) { + results(i,7) = calcDerivative(d6y,ax); + } /* if(_computeIntegral){ @@ -715,31 +790,31 @@ RigidBodyDynamics::Math::MatrixNd } */ } - return results; + return results; } void SmoothSegmentedFunction::getXControlPoints( - RigidBodyDynamics::Math::MatrixNd& mat) const + RigidBodyDynamics::Math::MatrixNd& mat) const { mat.resize(_mXVec.size(), _mXVec.at(0).size()); - for(int i=0; i<_mXVec.size();++i){ - for(int j=0; j<_mXVec.size();++j){ + for(int i=0; i<_mXVec.size(); ++i) { + for(int j=0; j<_mXVec.size(); ++j) { mat(i,j) = _mXVec.at(i)[j]; } } } void SmoothSegmentedFunction::getYControlPoints( - RigidBodyDynamics::Math::MatrixNd& mat) const + RigidBodyDynamics::Math::MatrixNd& mat) const { -mat.resize(_mYVec.size(), _mYVec.at(0).size()); + mat.resize(_mYVec.size(), _mYVec.at(0).size()); -for(int i=0; i<_mYVec.size();++i){ - for(int j=0; j<_mYVec.size();++j){ - mat(i,j) = _mYVec.at(i)[j]; + for(int i=0; i<_mYVec.size(); ++i) { + for(int j=0; j<_mYVec.size(); ++j) { + mat(i,j) = _mYVec.at(i)[j]; + } } -} } @@ -754,29 +829,29 @@ _______________________________________________________________________ 51+9m 6 246 126+3m 189 calcValue 21 20 13 - calcDerivative : dy/dx 1 40 38 22 + calcDerivative : dy/dx 1 40 38 22 : d2y/dx2 2 78 73 23 - **calcIntegral 7 2 3 1 + **calcIntegral 7 2 3 1 total per point 58+9m 9 387 260+3m 248 total per elbow 5.8k+900m 900 38.7k 26k+300m 24.8k - + *Approximate. Overhead associated with finding the correct Bezier - spline section, and evaluating u(x). + spline section, and evaluating u(x). Assumes 2 Newton iterations in calcU **Approximate. Includes estimated cost of evaluating a cubic spline with 100 knots */ void SmoothSegmentedFunction::printCurveToCSVFile( - const std::string& path, - const std::string& fileNameWithoutExtension, - double domainMin, - double domainMax) const + const std::string& path, + const std::string& fileNameWithoutExtension, + double domainMin, + double domainMax) const { //Only compute up to the 2nd derivative - RigidBodyDynamics::Math::MatrixNd results = + RigidBodyDynamics::Math::MatrixNd results = calcSampledCurve(2,domainMin,domainMax); std::vector colNames(results.cols()); colNames[0] = "x"; @@ -796,49 +871,52 @@ void SmoothSegmentedFunction::printCurveToCSVFile( This function will print cvs file of the column vector col0 and the matrix data */ void SmoothSegmentedFunction:: - printMatrixToFile( RigidBodyDynamics::Math::MatrixNd& data, - std::vector& colNames, - const std::string& path, - const std::string& filename) const +printMatrixToFile( RigidBodyDynamics::Math::MatrixNd& data, + std::vector& colNames, + const std::string& path, + const std::string& filename) const { - + ofstream datafile; std::string fullpath = path; - - if(fullpath.length() > 0) + + if(fullpath.length() > 0) { fullpath.append("/"); - + } + fullpath.append(filename); datafile.open(fullpath.c_str(),std::ios::out); - if(!datafile){ + if(!datafile) { datafile.close(); - cerr << "SmoothSegmentedFunction::printMatrixToFile " - << _name.c_str() << ": Failed to open the file path: " - << fullpath.c_str() - << endl; - assert(0); - abort(); + ostringstream errormsg; + errormsg << "SmoothSegmentedFunction::printMatrixToFile " + << _name.c_str() << ": Failed to open the file path: " + << fullpath.c_str() + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidFileError(errormsg.str()); } - for(int i = 0; i < (signed)colNames.size(); i++){ - if(i < (signed)colNames.size()-1) + for(int i = 0; i < (signed)colNames.size(); i++) { + if(i < (signed)colNames.size()-1) { datafile << colNames[i] << ","; - else + } else { datafile << colNames[i] << "\n"; + } } - for(int i = 0; i < data.rows(); i++){ - for(int j = 0; j < data.cols(); j++){ - if(j + * Copyright (c) 2016 Matthew Millard * * Licensed under the zlib license. See LICENSE for more details. */ diff --git a/addons/geometry/tests/CMakeLists.txt b/addons/geometry/tests/CMakeLists.txt index a8e8df7..5f7749b 100644 --- a/addons/geometry/tests/CMakeLists.txt +++ b/addons/geometry/tests/CMakeLists.txt @@ -11,12 +11,8 @@ SET ( RBDL_ADDON_GEOMETRY_TESTS_VERSION PROJECT (RBDL_GEOMETRY_TESTS VERSION ${RBDL_ADDON_GEOMETRY_TESTS_VERSION}) -# Needed for UnitTest++ -LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../CMake ) - -# Look for unittest++ -FIND_PACKAGE (UnitTest++ REQUIRED) -INCLUDE_DIRECTORIES (${UNITTEST++_INCLUDE_DIR}) +# Look for catch2 +FIND_PACKAGE(Catch2 REQUIRED) SET ( GEOMETRY_TESTS_SRCS testSmoothSegmentedFunction.cc @@ -30,6 +26,13 @@ SET ( GEOMETRY_TESTS_SRCS ) INCLUDE_DIRECTORIES ( ../ ) +INCLUDE_DIRECTORIES (${CMAKE_SOURCE_DIR}/tests) +# on unix systems add /usr/local/include to include dirs +# this was added since macos homebrew installs many things there but macos +# does not automatically search their, and on linux systems this does not hurt +IF (NOT WIN32) + INCLUDE_DIRECTORIES(/usr/local/include) +ENDIF (NOT WIN32) SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES @@ -40,7 +43,7 @@ ADD_EXECUTABLE ( rbdl_geometry_tests ${GEOMETRY_TESTS_SRCS} ) SET_TARGET_PROPERTIES ( rbdl_geometry_tests PROPERTIES LINKER_LANGUAGE CXX - OUTPUT_NAME runGeometryTests + OUTPUT_NAME geometry_tests ) SET (RBDL_LIBRARY rbdl) @@ -58,7 +61,7 @@ OPTION (RUN_AUTOMATIC_TESTS "Perform automatic tests after compilation?" OFF) IF (RUN_AUTOMATIC_TESTS) ADD_CUSTOM_COMMAND (TARGET rbdl_geometry_tests POST_BUILD - COMMAND ./runGeometryTests + COMMAND ./geometry_tests COMMENT "Running automated addon geometry tests..." ) ENDIF (RUN_AUTOMATIC_TESTS) diff --git a/addons/geometry/tests/numericalTestFunctions.cc b/addons/geometry/tests/numericalTestFunctions.cc index 6afe038..7f4024b 100644 --- a/addons/geometry/tests/numericalTestFunctions.cc +++ b/addons/geometry/tests/numericalTestFunctions.cc @@ -1,534 +1,533 @@ -/* -------------------------------------------------------------------------- * - * OpenSim: testSmoothSegmentedFunctionFactory.cpp * - * -------------------------------------------------------------------------- * - * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * - * See http://opensim.stanford.edu and the NOTICE file for more information. * - * OpenSim is developed at Stanford University and supported by the US * - * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * - * through the Warrior Web program. * - * * - * Copyright (c) 2005-2012 Stanford University and the Authors * - * Author(s): Matthew Millard * - * * - * Licensed under the Apache License, Version 2.0 (the "License"); you may * - * not use this file except in compliance with the License. You may obtain a * - * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * - * * - * Unless required by applicable law or agreed to in writing, software * - * distributed under the License is distributed on an "AS IS" BASIS, * - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * - * See the License for the specific language governing permissions and * - * limitations under the License. * - * -------------------------------------------------------------------------- */ -/* - Update: - This is a port of the original code so that it will work with - the multibody code RBDL written by Martin Felis. - - This also includes additional curves (the Anderson2007 curves) - which are not presently in OpenSim. - - Author: - Matthew Millard - - Date: - Nov 2015 - -*/ -/* -Below is a basic bench mark simulation for the SmoothSegmentedFunctionFactory -class, a class that enables the easy generation of C2 continuous curves -that define the various characteristic curves required in a muscle model - */ - -// Author: Matthew Millard - -//============================================================================== -// INCLUDES -//============================================================================== - - -#include "numericalTestFunctions.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace RigidBodyDynamics::Addons::Geometry; -using namespace std; - - -void printMatrixToFile( - const RigidBodyDynamics::Math::VectorNd& col0, - const RigidBodyDynamics::Math::MatrixNd& data, - string& filename) -{ - - ofstream datafile; - datafile.open(filename.c_str()); - - for(int i = 0; i < data.rows(); i++){ - datafile << col0[i] << ","; - for(int j = 0; j < data.cols(); j++){ - if(j y_C3max){ - c3 = 1; - y = y_C3max; - } - - h = pow( ( (EPSILON*y*2.0)/(c3) ) , 1.0/3.0); - - //Now check that h to the left and right are at least similar - //If not, take the smallest one. - xL = x[i]-h/2; - xR = x[i]+h/2; - - fL = mcf.calcDerivative(xL, order-1); - fR = mcf.calcDerivative(xR, order-1); - - //Just for convenience checking ... - dyNUM = (fR-fL)/h; - dy = mcf.calcDerivative(x[i],order); - err = abs(dy-dyNUM); - - /*if(err > tol && abs(dy) > rootEPS && order <= 2){ - err = err/abs(dy); - if(err > tol) - cout << "rel tol exceeded" << endl; - }*/ - - dyV[i] = dyNUM; - - } - - - return dyV; -} - -bool isFunctionContinuous( RigidBodyDynamics::Math::VectorNd& xV, - SmoothSegmentedFunction& yV, - int order, - double minValueSecondDerivative, - double taylorErrorMult) -{ - bool flag_continuous = true; - - double xL = 0; // left shoulder point - double xR = 0; // right shoulder point - double yL = 0; // left shoulder point function value - double yR = 0; // right shoulder point function value - double dydxL = 0; // left shoulder point derivative value - double dydxR = 0; // right shoulder point derivative value - - double xVal = 0; //x value to test - double yVal = 0; //Y(x) value to test - - double yValEL = 0; //Extrapolation to yVal from the left - double yValER = 0; //Extrapolation to yVal from the right - - double errL = 0; - double errR = 0; - - double errLMX = 0; - double errRMX = 0; - - - for(int i =1; i < xV.size()-1; i++){ - xVal = xV[i]; - yVal = yV.calcDerivative(xVal, order); - - xL = 0.5*(xV[i]+xV[i-1]); - xR = 0.5*(xV[i]+xV[i+1]); - - yL = yV.calcDerivative(xL,order); - yR = yV.calcDerivative(xR,order); - - dydxL = yV.calcDerivative(xL,order+1); - dydxR = yV.calcDerivative(xR,order+1); - - - yValEL = yL + dydxL*(xVal-xL); - yValER = yR - dydxR*(xR-xVal); - - errL = abs(yValEL-yVal); - errR = abs(yValER-yVal); - - errLMX = abs(yV.calcDerivative(xL,order+2)*0.5*(xVal-xL)*(xVal-xL)); - errRMX = abs(yV.calcDerivative(xR,order+2)*0.5*(xR-xVal)*(xR-xVal)); - - errLMX*=taylorErrorMult; - errRMX*=taylorErrorMult; - - if(errLMX < minValueSecondDerivative) - errLMX = minValueSecondDerivative; - - if(errRMX < minValueSecondDerivative) - errRMX = minValueSecondDerivative; // to accomodate numerical - //error in errL - - if(errL > errLMX || errR > errRMX){ - flag_continuous = false; - } - } - - return flag_continuous; -} - -bool isVectorMonotonic( RigidBodyDynamics::Math::VectorNd& y, - int multEPS) -{ - double dir = y[y.size()-1]-y[0]; - bool isMonotonic = true; - - if(dir < 0){ - for(int i =1; i y[i-1]+EPSILON*multEPS){ - isMonotonic = false; - //printf("Monotonicity broken at idx %i, since %fe-16 > %fe-16\n", - // i,y(i)*1e16,y(i-1)*1e16); - printf("Monotonicity broken at idx %i, since " - "y(i)-y(i-1) < tol, (%f*EPSILON < EPSILON*%i) \n", - i,((y[i]-y[i-1])/EPSILON), multEPS); - } - } - } - if(dir > 0){ - for(int i =1; i = 0; i=i-1){ - width = abs(x[i]-x[i+1]); - inty[i] = inty[i+1] + width*(0.5)*(y[i]+y[i+1]); - } - } - - - return inty; -} - - -double calcMaximumVectorError(RigidBodyDynamics::Math::VectorNd& a, - RigidBodyDynamics::Math::VectorNd& b) -{ - double error = 0; - double cerror=0; - for(int i = 0; i< a.size(); i++) - { - cerror = abs(a[i]-b[i]); - if(cerror > error){ - error = cerror; - } - } - return error; -} - - - -bool isCurveC2Continuous(SmoothSegmentedFunction& mcf, - RigidBodyDynamics::Math::MatrixNd& mcfSample, - double continuityTol) -{ - //cout << " TEST: C2 Continuity " << endl; - - int multC0 = 5; - int multC1 = 50; - int multC2 = 200; - - RigidBodyDynamics::Math::VectorNd fcnSample = - RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); - - for(int i=0; i < mcfSample.rows(); i++){ - fcnSample[i] = mcfSample(i,0); - } - - - bool c0 = isFunctionContinuous(fcnSample, mcf, 0, continuityTol, multC0); - bool c1 = isFunctionContinuous(fcnSample, mcf, 1, continuityTol, multC1); - bool c2 = isFunctionContinuous(fcnSample, mcf, 2, continuityTol, multC2); - - - - return (c0 && c1 && c2); - //printf( " passed: C2 continuity established to a multiple\n" - // " of the next Taylor series error term.\n " - // " C0,C1, and C2 multiples: %i,%i and %i\n", - // multC0,multC1,multC2); - //cout << endl; -} - - -bool isCurveMontonic(RigidBodyDynamics::Math::MatrixNd mcfSample) -{ - //cout << " TEST: Monotonicity " << endl; - int multEps = 10; - - RigidBodyDynamics::Math::VectorNd fcnSample = - RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); - - for(int i=0; i < mcfSample.rows(); i++){ - fcnSample[i] = mcfSample(i,1); - } - - bool monotonic = isVectorMonotonic(fcnSample,10); - return monotonic; - //printf(" passed: curve is monotonic to %i*EPSILON",multEps); - //cout << endl; -} - - -bool areCurveDerivativesCloseToNumericDerivatives( - SmoothSegmentedFunction& mcf, - RigidBodyDynamics::Math::MatrixNd& mcfSample, - double tol) -{ - //cout << " TEST: Derivative correctness " << endl; - int maxDer = 4;//mcf.getMaxDerivativeOrder() - 2; - - RigidBodyDynamics::Math::MatrixNd numSample(mcfSample.rows(),maxDer); - RigidBodyDynamics::Math::MatrixNd relError(mcfSample.rows(),maxDer); - - - RigidBodyDynamics::Math::VectorNd domainX = - RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); - - for(int j=0; j tol){ - relError(j,i) = relError(j,i)/mcfSample(j,i+2); - } - } - - } - - RigidBodyDynamics::Math::VectorNd errRelMax = - RigidBodyDynamics::Math::VectorNd::Zero(6); - RigidBodyDynamics::Math::VectorNd errAbsMax = - RigidBodyDynamics::Math::VectorNd::Zero(6); - - double absTol = 5*tol; - - bool flagError12=false; - RigidBodyDynamics::Math::VectorNd tolExceeded12V = - RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); - - int tolExceeded12 = 0; - int tolExceeded34 = 0; - for(int j=0;j tol && mcfSample(i,j+2) > tol){ - if(j <= 1){ - tolExceeded12++; - tolExceeded12V[i]=1; - flagError12=true; - } - if(j>=2) - tolExceeded34++; - } - if(mcfSample(i,j+2) > tol) - if(errRelMax[j] < abs(relError(i,j))) - errRelMax[j] = abs(relError(i,j)); - - //This is a harder test: here we're comparing absolute error - //so the tolerance margin is a little higher - if(relError(i,j) > absTol && mcfSample(i,j+2) <= tol){ - if(j <= 1){ - tolExceeded12++; - tolExceeded12V[i]=1; - flagError12=true; - } - if(j>=2) - tolExceeded34++; - } - - if(mcfSample(i,j+2) < tol) - if(errAbsMax[j] < abs(relError(i,j))) - errAbsMax[j] = abs(relError(i,j)); - - - } - - /* - if(flagError12 == true){ - printf("Derivative %i Rel Error Exceeded:\n",j); - printf("x dx_relErr dx_calcVal dx_sample" - " dx2_relErr dx2_calcVal dx2_sample\n"); - for(int i=0; i +#include +#include +#include +#include +#include +#include + +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace std; + + +void printMatrixToFile( + const RigidBodyDynamics::Math::VectorNd& col0, + const RigidBodyDynamics::Math::MatrixNd& data, + string& filename) +{ + + ofstream datafile; + datafile.open(filename.c_str()); + + for(int i = 0; i < data.rows(); i++){ + datafile << col0[i] << ","; + for(int j = 0; j < data.cols(); j++){ + if(j y_C3max){ + c3 = 1; + y = y_C3max; + } + + h = pow( ( (EPSILON*y*2.0)/(c3) ) , 1.0/3.0); + + //Now check that h to the left and right are at least similar + //If not, take the smallest one. + xL = x[i]-h/2; + xR = x[i]+h/2; + + fL = mcf.calcDerivative(xL, order-1); + fR = mcf.calcDerivative(xR, order-1); + + //Just for convenience checking ... + dyNUM = (fR-fL)/h; + dy = mcf.calcDerivative(x[i],order); + err = abs(dy-dyNUM); + + /*if(err > tol && abs(dy) > rootEPS && order <= 2){ + err = err/abs(dy); + if(err > tol) + cout << "rel tol exceeded" << endl; + }*/ + + dyV[i] = dyNUM; + + } + + + return dyV; +} + +bool isFunctionContinuous( RigidBodyDynamics::Math::VectorNd& xV, + SmoothSegmentedFunction& yV, + int order, + double minValueSecondDerivative, + double taylorErrorMult) +{ + bool flag_continuous = true; + + double xL = 0; // left shoulder point + double xR = 0; // right shoulder point + double yL = 0; // left shoulder point function value + double yR = 0; // right shoulder point function value + double dydxL = 0; // left shoulder point derivative value + double dydxR = 0; // right shoulder point derivative value + + double xVal = 0; //x value to test + double yVal = 0; //Y(x) value to test + + double yValEL = 0; //Extrapolation to yVal from the left + double yValER = 0; //Extrapolation to yVal from the right + + double errL = 0; + double errR = 0; + + double errLMX = 0; + double errRMX = 0; + + + for(int i =1; i < xV.size()-1; i++){ + xVal = xV[i]; + yVal = yV.calcDerivative(xVal, order); + + xL = 0.5*(xV[i]+xV[i-1]); + xR = 0.5*(xV[i]+xV[i+1]); + + yL = yV.calcDerivative(xL,order); + yR = yV.calcDerivative(xR,order); + + dydxL = yV.calcDerivative(xL,order+1); + dydxR = yV.calcDerivative(xR,order+1); + + + yValEL = yL + dydxL*(xVal-xL); + yValER = yR - dydxR*(xR-xVal); + + errL = abs(yValEL-yVal); + errR = abs(yValER-yVal); + + errLMX = abs(yV.calcDerivative(xL,order+2)*0.5*(xVal-xL)*(xVal-xL)); + errRMX = abs(yV.calcDerivative(xR,order+2)*0.5*(xR-xVal)*(xR-xVal)); + + errLMX*=taylorErrorMult; + errRMX*=taylorErrorMult; + + if(errLMX < minValueSecondDerivative) + errLMX = minValueSecondDerivative; + + if(errRMX < minValueSecondDerivative) + errRMX = minValueSecondDerivative; // to accomodate numerical + //error in errL + + if(errL > errLMX || errR > errRMX){ + flag_continuous = false; + } + } + + return flag_continuous; +} + +bool isVectorMonotonic( RigidBodyDynamics::Math::VectorNd& y, + int multEPS) +{ + double dir = y[y.size()-1]-y[0]; + bool isMonotonic = true; + + if(dir < 0){ + for(int i =1; i y[i-1]+EPSILON*multEPS){ + isMonotonic = false; + //printf("Monotonicity broken at idx %i, since %fe-16 > %fe-16\n", + // i,y(i)*1e16,y(i-1)*1e16); + printf("Monotonicity broken at idx %i, since " + "y(i)-y(i-1) < tol, (%f*EPSILON < EPSILON*%i) \n", + i,((y[i]-y[i-1])/EPSILON), multEPS); + } + } + } + if(dir > 0){ + for(int i =1; i = 0; i=i-1){ + width = abs(x[i]-x[i+1]); + inty[i] = inty[i+1] + width*(0.5)*(y[i]+y[i+1]); + } + } + + + return inty; +} + + +double calcMaximumVectorError(RigidBodyDynamics::Math::VectorNd& a, + RigidBodyDynamics::Math::VectorNd& b) +{ + double error = 0; + double cerror=0; + for(int i = 0; i< a.size(); i++) + { + cerror = abs(a[i]-b[i]); + if(cerror > error){ + error = cerror; + } + } + return error; +} + + + +bool isCurveC2Continuous(SmoothSegmentedFunction& mcf, + RigidBodyDynamics::Math::MatrixNd& mcfSample, + double continuityTol) +{ + //cout << " TEST: C2 Continuity " << endl; + + int multC0 = 5; + int multC1 = 50; + int multC2 = 200; + + RigidBodyDynamics::Math::VectorNd fcnSample = + RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); + + for(int i=0; i < mcfSample.rows(); i++){ + fcnSample[i] = mcfSample(i,0); + } + + + bool c0 = isFunctionContinuous(fcnSample, mcf, 0, continuityTol, multC0); + bool c1 = isFunctionContinuous(fcnSample, mcf, 1, continuityTol, multC1); + bool c2 = isFunctionContinuous(fcnSample, mcf, 2, continuityTol, multC2); + + + + return (c0 && c1 && c2); + //printf( " passed: C2 continuity established to a multiple\n" + // " of the next Taylor series error term.\n " + // " C0,C1, and C2 multiples: %i,%i and %i\n", + // multC0,multC1,multC2); + //cout << endl; +} + + +bool isCurveMontonic(RigidBodyDynamics::Math::MatrixNd mcfSample) +{ + //cout << " TEST: Monotonicity " << endl; + int multEps = 10; + + RigidBodyDynamics::Math::VectorNd fcnSample = + RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); + + for(int i=0; i < mcfSample.rows(); i++){ + fcnSample[i] = mcfSample(i,1); + } + + bool monotonic = isVectorMonotonic(fcnSample,10); + return monotonic; + //printf(" passed: curve is monotonic to %i*EPSILON",multEps); + //cout << endl; +} + + +bool areCurveDerivativesCloseToNumericDerivatives( + SmoothSegmentedFunction& mcf, + RigidBodyDynamics::Math::MatrixNd& mcfSample, + double tol) +{ + //cout << " TEST: Derivative correctness " << endl; + int maxDer = 4;//mcf.getMaxDerivativeOrder() - 2; + + RigidBodyDynamics::Math::MatrixNd numSample(mcfSample.rows(),maxDer); + RigidBodyDynamics::Math::MatrixNd relError(mcfSample.rows(),maxDer); + + + RigidBodyDynamics::Math::VectorNd domainX = + RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); + + for(int j=0; j tol){ + relError(j,i) = relError(j,i)/mcfSample(j,i+2); + } + } + + } + + RigidBodyDynamics::Math::VectorNd errRelMax = + RigidBodyDynamics::Math::VectorNd::Zero(6); + RigidBodyDynamics::Math::VectorNd errAbsMax = + RigidBodyDynamics::Math::VectorNd::Zero(6); + + double absTol = 5*tol; + + bool flagError12=false; + RigidBodyDynamics::Math::VectorNd tolExceeded12V = + RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); + + int tolExceeded12 = 0; + int tolExceeded34 = 0; + for(int j=0;j tol && mcfSample(i,j+2) > tol){ + if(j <= 1){ + tolExceeded12++; + tolExceeded12V[i]=1; + flagError12=true; + } + if(j>=2) + tolExceeded34++; + } + if(mcfSample(i,j+2) > tol) + if(errRelMax[j] < abs(relError(i,j))) + errRelMax[j] = abs(relError(i,j)); + + //This is a harder test: here we're comparing absolute error + //so the tolerance margin is a little higher + if(relError(i,j) > absTol && mcfSample(i,j+2) <= tol){ + if(j <= 1){ + tolExceeded12++; + tolExceeded12V[i]=1; + flagError12=true; + } + if(j>=2) + tolExceeded34++; + } + + if(mcfSample(i,j+2) < tol) + if(errAbsMax[j] < abs(relError(i,j))) + errAbsMax[j] = abs(relError(i,j)); + + + } + + /* + if(flagError12 == true){ + printf("Derivative %i Rel Error Exceeded:\n",j); + printf("x dx_relErr dx_calcVal dx_sample" + " dx2_relErr dx2_calcVal dx2_sample\n"); + for(int i=0; i -#include -#include -#include -#include -#include -#include - -using namespace RigidBodyDynamics::Addons::Geometry; - -using namespace std; - -static double EPSILON = numeric_limits::epsilon(); -static double SQRTEPSILON = sqrt(EPSILON); -static double TOL = std::numeric_limits::epsilon()*1e4; - -static bool FLAG_PLOT_CURVES = false; -static string FILE_PATH = ""; -static double TOL_DX = 5e-3; -static double TOL_DX_BIG = 1e-2; -static double TOL_BIG = 1e-6; -static double TOL_SMALL = 1e-12; - -/** -This function will print cvs file of the column vector col0 and the matrix - data - -@params col0: A vector that must have the same number of rows as the data matrix - This column vector is printed as the first column -@params data: A matrix of data -@params filename: The name of the file to print -*/ -void printMatrixToFile( - const RigidBodyDynamics::Math::VectorNd& col0, - const RigidBodyDynamics::Math::MatrixNd& data, - string& filename); - - -/** -This function will print cvs file of the matrix - data - -@params data: A matrix of data -@params filename: The name of the file to print -*/ -void printMatrixToFile( - const RigidBodyDynamics::Math::MatrixNd& data, - string& filename); - - -/** - This function computes a standard central difference dy/dx. If - extrap_endpoints is set to 1, then the derivative at the end points is - estimated by linearly extrapolating the dy/dx values beside the end points - - @param x domain vector - @param y range vector - @param extrap_endpoints: (false) Endpoints of the returned vector will be - zero, because a central difference - is undefined at these endpoints - (true) Endpoints are computed by linearly - extrapolating using a first difference from - the neighboring 2 points - @returns dy/dx computed using central differences -*/ -RigidBodyDynamics::Math::VectorNd - calcCentralDifference( RigidBodyDynamics::Math::VectorNd& x, - RigidBodyDynamics::Math::VectorNd& y, - bool extrap_endpoints); - -/** - This function computes a standard central difference dy/dx at each point in - a vector x, for a SmoothSegmentedFunction mcf, to a desired tolerance. This - function will take the best step size at each point to minimize the - error caused by taking a numerical derivative, and the error caused by - numerical rounding error: - - For a step size of h/2 to the left and to the right of the point of - interest the error is - error = 1/4*h^2*c3 + r*f(x)/h, (1) - - Where c3 is the coefficient of the 3rd order Taylor series expansion - about point x. Thus c3 can be computed if the order + 2 derivative is - known - - c3 = (d^3f(x)/dx^3)/(6) (2) - - And r*f(x)/h is the rounding error that occurs due to the central - difference. - - Taking a first derivative of 1 and solving for h yields - - h = (r*f(x)*2/c3)^(1/3) - - Where r is EPSILON - - @param x domain vector - @param mcf the SmoothSegmentedFunction of interest - @param order the order of the numerical derivative - @param tolerance desired tolerance on the returned result - @returns dy/dx computed using central differences -*/ -RigidBodyDynamics::Math::VectorNd -calcCentralDifference( RigidBodyDynamics::Math::VectorNd& x, - SmoothSegmentedFunction& mcf, - double tol, int order); - -/** - This function tests numerically for continuity of a curve. The test is - performed by taking a point on the curve, and then two points (called the - shoulder points) to the left and right of the point in question. The - shoulder points are located half way between the sample points in xV. - - The value of the function's derivative is evaluated at each of the shoulder - points and used to linearly extrapolate from the shoulder points back to the - original point. If the original point and the linear extrapolations of each - of the shoulder points agree within a tolerance, then the curve is assumed - to be continuous. This tolerance is evaluated to be the smaller of the 2nd - derivative times a multiplier (taylorErrorMult) or minValueSecondDerivative - - - @param x Values to test for continuity - - @param yx The SmoothSegmentedFunction to test - - @param order The order of the curve of SmoothSegmentedFunction to test - for continuity - - @param minValueSecondDerivative the minimum value allowed for the 2nd - term in the Taylor series. Here we need to define a minimum because - this 2nd term is used to define a point specific tolerance for the - continuity test. If the 2nd derivative happens to go to zero, we - still cannot let the minimum error tolerance to go to zero - else - the test will fail on otherwise good curves. - - @param taylorErrorMult This scales the error tolerance. The default error - tolerance is the the 2nd order Taylor series term. This term is - dependent on the size of the higher-order-terms and the sample - spacing used for xV (since the shoulder points are picked half- - way between the sampled points) -*/ -bool isFunctionContinuous( RigidBodyDynamics::Math::VectorNd& xV, - SmoothSegmentedFunction& yV, - int order, - double minValueSecondDerivative, - double taylorErrorMult); - - -/** -This function will scan through a vector and determine if it is monotonic or -not - -@param y the vector of interest -@param multEPS The tolerance on the monotoncity check, expressed as a scaling of - EPSILON -@return true if the vector is monotonic, false if it is not -*/ -bool isVectorMonotonic( RigidBodyDynamics::Math::VectorNd& y, - int multEPS); - - -/** -This function will compute the numerical integral of y(x) using the trapezoidal -method - -@param x the domain vector -@param y the range vector, of y(x), evaluated at x -@param flag_TrueIntForward_FalseIntBackward - When this flag is set to true, the integral of y(x) will be evaluated from - left to right, starting with int(y(0)) = 0. When this flag is false, then - y(x) will be evaluated from right to left with int(y(n)) = 0, where n is - the maximum number of elements. -@return the integral of y(x) -*/ -RigidBodyDynamics::Math::VectorNd calcTrapzIntegral( - RigidBodyDynamics::Math::VectorNd& x, - RigidBodyDynamics::Math::VectorNd& y, - bool flag_TrueIntForward_FalseIntBackward); - - -/** - @param a The first vector - @param b The second vector - @return Returns the maximum absolute difference between vectors a and b -*/ -double calcMaximumVectorError(RigidBodyDynamics::Math::VectorNd& a, - RigidBodyDynamics::Math::VectorNd& b); - - -/* -This function tests the SmoothSegmentedFunction to see if it is C2 continuous. -This function works by using the applying the function isFunctionContinuous -multiple times. For details of the method used please refer to the doxygen for -isFunctionContinuous. - -@param mcf - a SmoothSegmentedFunction -@param mcfSample: - A n-by-m matrix of values where the first column is the domain (x) of interest - and the remaining columns are the curve value (y), and its derivatives (dy/dx, - d2y/dx2, d3y/dx3, etc). This matrix is returned by the function - calcSampledCurve in SmoothSegmented Function. -@param continuityTol -@return bool: true if the curve is C2 continuous to the desired tolernace - -*/ -bool isCurveC2Continuous(SmoothSegmentedFunction& mcf, - RigidBodyDynamics::Math::MatrixNd& mcfSample, - double continuityTol); - -/* - 4. The MuscleCurveFunctions which are supposed to be monotonic will be - tested for monotonicity. -*/ -bool isCurveMontonic(RigidBodyDynamics::Math::MatrixNd mcfSample); - -/** -This function compares the i^th derivative return by a SmoothSegmented function -against a numerical derivative computed using a central difference applied to -the (i-1)^th derivative of the function. This function first checks the -1st derivative and continues checking derivatives until the 4th derivative. - -@param mcf - a SmoothSegmentedFunction -@param mcfSample: - A n-by-m matrix of values where the first column is the domain (x) of interest - and the remaining columns are the curve value (y), and its derivatives (dy/dx, - d2y/dx2, d3y/dx3, etc). This matrix is returned by the function - calcSampledCurve in SmoothSegmented Function. - -@param tol : the tolerance used to assess the relative error between the - numerically computed derivatives and the derivatives returned by - the SmoothSegmentedFunction -@return bool: true if all of the derivatives up to the 4th (hard coded) are - within a relative tolerance of tol w.r.t. to the numerically - computed derivatives -*/ -bool areCurveDerivativesCloseToNumericDerivatives( - SmoothSegmentedFunction& mcf, - RigidBodyDynamics::Math::MatrixNd& mcfSample, - double tol); +/* -------------------------------------------------------------------------- * + * OpenSim: testSmoothSegmentedFunctionFactory.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + This also includes additional curves (the Anderson2007 curves) + which are not presently in OpenSim. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ +/* +Below is a basic bench mark simulation for the SmoothSegmentedFunctionFactory +class, a class that enables the easy generation of C2 continuous curves +that define the various characteristic curves required in a muscle model + */ + +// Author: Matthew Millard + +//============================================================================== +// INCLUDES +//============================================================================== + +#include "../geometry.h" +#include +#include +#include +#include +#include +#include + +using namespace RigidBodyDynamics::Addons::Geometry; + +using namespace std; + +static double EPSILON = numeric_limits::epsilon(); +static double SQRTEPSILON = sqrt(EPSILON); +static double TOL = std::numeric_limits::epsilon()*1e4; + +static bool FLAG_PLOT_CURVES = false; +static string FILE_PATH = ""; +static double TOL_DX = 5e-3; +static double TOL_DX_BIG = 1e-2; +static double TOL_BIG = 1e-6; +static double TOL_SMALL = 1e-12; + +/** +This function will print cvs file of the column vector col0 and the matrix + data + +@params col0: A vector that must have the same number of rows as the data matrix + This column vector is printed as the first column +@params data: A matrix of data +@params filename: The name of the file to print +*/ +void printMatrixToFile( + const RigidBodyDynamics::Math::VectorNd& col0, + const RigidBodyDynamics::Math::MatrixNd& data, + string& filename); + + +/** +This function will print cvs file of the matrix + data + +@params data: A matrix of data +@params filename: The name of the file to print +*/ +void printMatrixToFile( + const RigidBodyDynamics::Math::MatrixNd& data, + string& filename); + + +/** + This function computes a standard central difference dy/dx. If + extrap_endpoints is set to 1, then the derivative at the end points is + estimated by linearly extrapolating the dy/dx values beside the end points + + @param x domain vector + @param y range vector + @param extrap_endpoints: (false) Endpoints of the returned vector will be + zero, because a central difference + is undefined at these endpoints + (true) Endpoints are computed by linearly + extrapolating using a first difference from + the neighboring 2 points + @returns dy/dx computed using central differences +*/ +RigidBodyDynamics::Math::VectorNd + calcCentralDifference( RigidBodyDynamics::Math::VectorNd& x, + RigidBodyDynamics::Math::VectorNd& y, + bool extrap_endpoints); + +/** + This function computes a standard central difference dy/dx at each point in + a vector x, for a SmoothSegmentedFunction mcf, to a desired tolerance. This + function will take the best step size at each point to minimize the + error caused by taking a numerical derivative, and the error caused by + numerical rounding error: + + For a step size of h/2 to the left and to the right of the point of + interest the error is + error = 1/4*h^2*c3 + r*f(x)/h, (1) + + Where c3 is the coefficient of the 3rd order Taylor series expansion + about point x. Thus c3 can be computed if the order + 2 derivative is + known + + c3 = (d^3f(x)/dx^3)/(6) (2) + + And r*f(x)/h is the rounding error that occurs due to the central + difference. + + Taking a first derivative of 1 and solving for h yields + + h = (r*f(x)*2/c3)^(1/3) + + Where r is EPSILON + + @param x domain vector + @param mcf the SmoothSegmentedFunction of interest + @param order the order of the numerical derivative + @param tolerance desired tolerance on the returned result + @returns dy/dx computed using central differences +*/ +RigidBodyDynamics::Math::VectorNd +calcCentralDifference( RigidBodyDynamics::Math::VectorNd& x, + SmoothSegmentedFunction& mcf, + double tol, int order); + +/** + This function tests numerically for continuity of a curve. The test is + performed by taking a point on the curve, and then two points (called the + shoulder points) to the left and right of the point in question. The + shoulder points are located half way between the sample points in xV. + + The value of the function's derivative is evaluated at each of the shoulder + points and used to linearly extrapolate from the shoulder points back to the + original point. If the original point and the linear extrapolations of each + of the shoulder points agree within a tolerance, then the curve is assumed + to be continuous. This tolerance is evaluated to be the smaller of the 2nd + derivative times a multiplier (taylorErrorMult) or minValueSecondDerivative + + + @param x Values to test for continuity + + @param yx The SmoothSegmentedFunction to test + + @param order The order of the curve of SmoothSegmentedFunction to test + for continuity + + @param minValueSecondDerivative the minimum value allowed for the 2nd + term in the Taylor series. Here we need to define a minimum because + this 2nd term is used to define a point specific tolerance for the + continuity test. If the 2nd derivative happens to go to zero, we + still cannot let the minimum error tolerance to go to zero - else + the test will fail on otherwise good curves. + + @param taylorErrorMult This scales the error tolerance. The default error + tolerance is the the 2nd order Taylor series term. This term is + dependent on the size of the higher-order-terms and the sample + spacing used for xV (since the shoulder points are picked half- + way between the sampled points) +*/ +bool isFunctionContinuous( RigidBodyDynamics::Math::VectorNd& xV, + SmoothSegmentedFunction& yV, + int order, + double minValueSecondDerivative, + double taylorErrorMult); + + +/** +This function will scan through a vector and determine if it is monotonic or +not + +@param y the vector of interest +@param multEPS The tolerance on the monotoncity check, expressed as a scaling of + EPSILON +@return true if the vector is monotonic, false if it is not +*/ +bool isVectorMonotonic( RigidBodyDynamics::Math::VectorNd& y, + int multEPS); + + +/** +This function will compute the numerical integral of y(x) using the trapezoidal +method + +@param x the domain vector +@param y the range vector, of y(x), evaluated at x +@param flag_TrueIntForward_FalseIntBackward + When this flag is set to true, the integral of y(x) will be evaluated from + left to right, starting with int(y(0)) = 0. When this flag is false, then + y(x) will be evaluated from right to left with int(y(n)) = 0, where n is + the maximum number of elements. +@return the integral of y(x) +*/ +RigidBodyDynamics::Math::VectorNd calcTrapzIntegral( + RigidBodyDynamics::Math::VectorNd& x, + RigidBodyDynamics::Math::VectorNd& y, + bool flag_TrueIntForward_FalseIntBackward); + + +/** + @param a The first vector + @param b The second vector + @return Returns the maximum absolute difference between vectors a and b +*/ +double calcMaximumVectorError(RigidBodyDynamics::Math::VectorNd& a, + RigidBodyDynamics::Math::VectorNd& b); + + +/* +This function tests the SmoothSegmentedFunction to see if it is C2 continuous. +This function works by using the applying the function isFunctionContinuous +multiple times. For details of the method used please refer to the doxygen for +isFunctionContinuous. + +@param mcf - a SmoothSegmentedFunction +@param mcfSample: + A n-by-m matrix of values where the first column is the domain (x) of interest + and the remaining columns are the curve value (y), and its derivatives (dy/dx, + d2y/dx2, d3y/dx3, etc). This matrix is returned by the function + calcSampledCurve in SmoothSegmented Function. +@param continuityTol +@return bool: true if the curve is C2 continuous to the desired tolernace + +*/ +bool isCurveC2Continuous(SmoothSegmentedFunction& mcf, + RigidBodyDynamics::Math::MatrixNd& mcfSample, + double continuityTol); + +/* + 4. The MuscleCurveFunctions which are supposed to be monotonic will be + tested for monotonicity. +*/ +bool isCurveMontonic(RigidBodyDynamics::Math::MatrixNd mcfSample); + +/** +This function compares the i^th derivative return by a SmoothSegmented function +against a numerical derivative computed using a central difference applied to +the (i-1)^th derivative of the function. This function first checks the +1st derivative and continues checking derivatives until the 4th derivative. + +@param mcf - a SmoothSegmentedFunction +@param mcfSample: + A n-by-m matrix of values where the first column is the domain (x) of interest + and the remaining columns are the curve value (y), and its derivatives (dy/dx, + d2y/dx2, d3y/dx3, etc). This matrix is returned by the function + calcSampledCurve in SmoothSegmented Function. + +@param tol : the tolerance used to assess the relative error between the + numerically computed derivatives and the derivatives returned by + the SmoothSegmentedFunction +@return bool: true if all of the derivatives up to the 4th (hard coded) are + within a relative tolerance of tol w.r.t. to the numerically + computed derivatives +*/ +bool areCurveDerivativesCloseToNumericDerivatives( + SmoothSegmentedFunction& mcf, + RigidBodyDynamics::Math::MatrixNd& mcfSample, + double tol); diff --git a/addons/geometry/tests/testSmoothSegmentedFunction.cc b/addons/geometry/tests/testSmoothSegmentedFunction.cc index e9596e4..c32dcdb 100644 --- a/addons/geometry/tests/testSmoothSegmentedFunction.cc +++ b/addons/geometry/tests/testSmoothSegmentedFunction.cc @@ -1,483 +1,544 @@ -/* -------------------------------------------------------------------------- * - * OpenSim: testSmoothSegmentedFunctionFactory.cpp * - * -------------------------------------------------------------------------- * - * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * - * See http://opensim.stanford.edu and the NOTICE file for more information. * - * OpenSim is developed at Stanford University and supported by the US * - * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * - * through the Warrior Web program. * - * * - * Copyright (c) 2005-2012 Stanford University and the Authors * - * Author(s): Matthew Millard * - * * - * Licensed under the Apache License, Version 2.0 (the "License"); you may * - * not use this file except in compliance with the License. You may obtain a * - * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * - * * - * Unless required by applicable law or agreed to in writing, software * - * distributed under the License is distributed on an "AS IS" BASIS, * - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * - * See the License for the specific language governing permissions and * - * limitations under the License. * - * -------------------------------------------------------------------------- */ -/* - Update: - This is a port of the original code so that it will work with - the multibody code RBDL written by Martin Felis. - - This also includes additional curves (the Anderson2007 curves) - which are not presently in OpenSim. - - Author: - Matthew Millard - - Date: - Nov 2015 - -*/ -/* -Below is a basic bench mark simulation for the SmoothSegmentedFunctionFactory -class, a class that enables the easy generation of C2 continuous curves -that define the various characteristic curves required in a muscle model - */ - -// Author: Matthew Millard - -//============================================================================== -// INCLUDES -//============================================================================== - -#include "geometry.h" -#include "numericalTestFunctions.h" -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace RigidBodyDynamics::Addons::Geometry; - -using namespace std; - - - - -/** - This function will create a quintic Bezier curve y(x) and sample it, its - first derivative w.r.t. U (dx(u)/du and dy(u)/du), and its first derivative - w.r.t. to X and print it to the screen. -*/ -void testSegmentedQuinticBezierDerivatives( - int maximumNumberOfToleranceViolations) -{ - //cout <<"**************************************************"<tol) - //printf("Error > Tol: (%i,%i): %f > %f\n",j,i,errorDer(j,i),tol); - } - } - //errorDer.cwiseAbs(); - //cout << errorDer << endl; - - //printf("...absolute tolerance of %f met\n", tol); - - //cout << " TEST: Bezier Curve Derivative DYDX to d6y/dx6" << endl; - RigidBodyDynamics::Math::MatrixNd numericDerXY(analyticDerXU.rows(), 6); - RigidBodyDynamics::Math::MatrixNd analyticDerXY(analyticDerXU.rows(),6); - - for(int i=0; i< analyticDerXU.rows(); i++) - { - analyticDerXY(i,0) = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,1); - analyticDerXY(i,1) = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,2); - analyticDerXY(i,2) = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,3); - analyticDerXY(i,3) = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,4); - analyticDerXY(i,4) = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,5); - analyticDerXY(i,5) = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,6); - } - - - for(int j=0; jrelTol){ - //printf("Error > Tol: (%i,%i): %f > %f\n",i,j, - // errorDerXY(i,j),relTol); - relTolExceeded++; - } - } - } - //cout << relTolExceeded << endl; - - //The relative tolerance gets exceeded occasionally in locations of - //rapid change in the curve. Provided there are only a few locations - //where the relative tolerance of 5% is broken, the curves should be - //regarded as being good. Ten errors out of a possible 100*6 data points - //seems relatively small. - CHECK(relTolExceeded < maximumNumberOfToleranceViolations); - - - //std::string fname = "analyticDerXY.csv"; - //printMatrixToFile(analyticDerXY,fname); - //fname = "numericDerXY.csv"; - //printMatrixToFile(numericDerXY,fname); - //fname = "errorDerXY.csv"; - //printMatrixToFile(errorDerXY,fname); - //printf(" ...relative tolerance of %f not exceeded more than %i times\n" - // " across all 6 derivatives, with 100 samples each\n", - // relTol, 10); - //cout <<"**************************************************"< +#include +#include +#include +#include +#include +#include + +#include "rbdl_tests.h" + +using namespace RigidBodyDynamics::Addons::Geometry; + +using namespace std; + + +/** + This function will create a quintic Bezier curve y(x) and sample it, its + first derivative w.r.t. U (dx(u)/du and dy(u)/du), and its first derivative + w.r.t. to X and print it to the screen. +*/ +void testSegmentedQuinticBezierDerivatives( + int maximumNumberOfToleranceViolations) +{ + //cout <<"**************************************************"<tol) + //printf("Error > Tol: (%i,%i): %f > %f\n",j,i,errorDer(j,i),tol); + } + } + //errorDer.cwiseAbs(); + //cout << errorDer << endl; + + //printf("...absolute tolerance of %f met\n", tol); + + //cout << " TEST: Bezier Curve Derivative DYDX to d6y/dx6" << endl; + RigidBodyDynamics::Math::MatrixNd numericDerXY(analyticDerXU.rows(), 6); + RigidBodyDynamics::Math::MatrixNd analyticDerXY(analyticDerXU.rows(),6); + + for(int i=0; i< analyticDerXU.rows(); i++) + { + analyticDerXY(i,0) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,1); + analyticDerXY(i,1) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,2); + analyticDerXY(i,2) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,3); + analyticDerXY(i,3) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,4); + analyticDerXY(i,4) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,5); + analyticDerXY(i,5) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,6); + } + + + for(int j=0; jrelTol){ + //printf("Error > Tol: (%i,%i): %f > %f\n",i,j, + // errorDerXY(i,j),relTol); + relTolExceeded++; + } + } + } + //cout << relTolExceeded << endl; + + //The relative tolerance gets exceeded occasionally in locations of + //rapid change in the curve. Provided there are only a few locations + //where the relative tolerance of 5% is broken, the curves should be + //regarded as being good. Ten errors out of a possible 100*6 data points + //seems relatively small. + CHECK(relTolExceeded < maximumNumberOfToleranceViolations); + + + //std::string fname = "analyticDerXY.csv"; + //printMatrixToFile(analyticDerXY,fname); + //fname = "numericDerXY.csv"; + //printMatrixToFile(numericDerXY,fname); + //fname = "errorDerXY.csv"; + //printMatrixToFile(errorDerXY,fname); + //printf(" ...relative tolerance of %f not exceeded more than %i times\n" + // " across all 6 derivatives, with 100 samples each\n", + // relTol, 10); + //cout <<"**************************************************"< +#include +#include +#include +#include +#include + +#include +#include #include #include "luatables.h" +#include "luatypes.h" extern "C" { @@ -17,239 +27,245 @@ using namespace std; using namespace RigidBodyDynamics; using namespace RigidBodyDynamics::Math; -template<> -Vector3d LuaTableNode::getDefault(const Vector3d &default_value) { - Vector3d result = default_value; +#ifdef RBDL_BUILD_ADDON_MUSCLE +#include "../muscle/Millard2016TorqueMuscle.h" +using namespace RigidBodyDynamics::Addons::Muscle; +#endif - if (stackQueryValue()) { - LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); - - if (vector_table.length() != 3) { - cerr << "LuaModel Error: invalid 3d vector!" << endl; - abort(); - } - - result[0] = vector_table[1]; - result[1] = vector_table[2]; - result[2] = vector_table[3]; - } - stackRestore(); +namespace RigidBodyDynamics +{ - return result; -} +namespace Addons +{ -template<> -SpatialVector LuaTableNode::getDefault( - const SpatialVector &default_value -) { - SpatialVector result = default_value; +//============================================================================== +void appendEnumToFileStream( + std::ofstream &updFileStream, + const std::string &enumName, + const std::vector &enumEntries, + unsigned int startingIndex, + const std::string &entryPrefix) +{ - if (stackQueryValue()) { - LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); - - if (vector_table.length() != 6) { - cerr << "LuaModel Error: invalid 6d vector!" << endl; - abort(); + updFileStream << "\n"; + updFileStream << "enum " << enumName << "\n{\n"; + unsigned int maxLineLength=0; + unsigned int numChars; + for(unsigned int i=0; i &enumEntries, + const std::string &entryPrefix, + const std::vector< unsigned int> &indexEntries) +{ -template<> -Matrix3d LuaTableNode::getDefault(const Matrix3d &default_value) { - Matrix3d result = default_value; - - if (stackQueryValue()) { - LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); - - if (vector_table.length() != 3) { - cerr << "LuaModel Error: invalid 3d matrix!" << endl; - abort(); + updFileStream << "\n"; + updFileStream << "struct " << mapName << "Entry {\n"; + updFileStream << " " << enumName << " id;\n"; + updFileStream << " const char* name;\n"; + updFileStream << " unsigned int index;\n"; + updFileStream << "};\n\n"; + updFileStream << "static const " << mapName << "Entry " << mapName << "[] = {\n"; + + unsigned int longLine=0; + unsigned int indexPadding =1; + unsigned int indexPaddingTemp=1; + for(unsigned int i=0;i 9){ + indexPaddingTemp = 1+ + unsigned(int(std::floor( log10(double(indexEntries[i]))))); + if(indexPaddingTemp > indexPadding){ + indexPadding = indexPaddingTemp; + } } - if (vector_table[1].length() != 3 - || vector_table[2].length() != 3 - || vector_table[3].length() != 3) { - cerr << "LuaModel Error: invalid 3d matrix!" << endl; - abort(); + } + longLine += unsigned(entryPrefix.size()+2); + unsigned int numChars=0; + + for(unsigned int i=0; i 9){ + numChars-=unsigned(int(std::floor( log10(double(indexEntries[i]))))); + } + numChars -= 1; + for(unsigned int j=0; j -SpatialTransform LuaTableNode::getDefault( - const SpatialTransform &default_value -) { - SpatialTransform result = default_value; +//============================================================================== - if (stackQueryValue()) { - LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); - - result.r = vector_table["r"].getDefault(Vector3d::Zero(3)); - result.E = vector_table["E"].getDefault(Matrix3d::Identity (3,3)); - } +void appendEnumStructToFileStream( + std::ofstream &updFileStream, + const std::string &mapName, + const std::vector < std::string > &enumTypeAndFieldNames, + const std::vector< std::vector > &enumEntries, + const std::vector< std::string > &entryPrefix, + const std::vector< std::string > &indexTypeAndFieldName, + const std::vector< std::vector< unsigned int > > &indexEntries) +{ - stackRestore(); + updFileStream << "\n"; + updFileStream << "struct " << mapName << "Entry {\n"; + for(unsigned int i=0; i enumNamePadding; + enumNamePadding.resize(enumTypeAndFieldNames.size()); + unsigned int enumNamePaddingTemp =1; + for(unsigned int i=0; i indexPadding; + indexPadding.resize( indexTypeAndFieldName.size() ); + unsigned int indexPaddingTemp=1; + for(unsigned int j=0; j -Joint LuaTableNode::getDefault(const Joint &default_value) { - Joint result = default_value; - - if (stackQueryValue()) { - LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); - - int joint_dofs = vector_table.length(); - - if (joint_dofs == 1) { - string dof_string = vector_table[1].getDefault(""); - if (dof_string == "JointTypeSpherical") { - stackRestore(); - return Joint(JointTypeSpherical); - } else if (dof_string == "JointTypeEulerZYX") { - stackRestore(); - return Joint(JointTypeEulerZYX); - } - if (dof_string == "JointTypeEulerXYZ") { - stackRestore(); - return Joint(JointTypeEulerXYZ); - } - if (dof_string == "JointTypeEulerYXZ") { - stackRestore(); - return Joint(JointTypeEulerYXZ); - } - if (dof_string == "JointTypeTranslationXYZ") { - stackRestore(); - return Joint(JointTypeTranslationXYZ); + for(unsigned int i=0;i 0) { - if (vector_table[1].length() != 6) { - cerr << "LuaModel Error: invalid joint motion subspace description at " - << this->keyStackToString() << endl; - abort(); + if(indexEntries.size()>0){ + for(unsigned int j=0; j< indexEntries[0].size(); ++j){ + if(indexEntries[i][j]>9){ + indexPaddingTemp = 1+ + unsigned(int(std::floor( log10(double(indexEntries[i][j]))))); + if(indexPaddingTemp > indexPadding[j]){ + indexPadding[j] = indexPaddingTemp; + } + } } } - - switch (joint_dofs) { - case 0: - result = Joint(JointTypeFixed); - break; - case 1: - result = Joint (vector_table[1].get()); - break; - case 2: - result = Joint( - vector_table[1].get(), - vector_table[2].get() - ); - break; - case 3: - result = Joint( - vector_table[1].get(), - vector_table[2].get(), - vector_table[3].get() - ); - break; - case 4: - result = Joint( - vector_table[1].get(), - vector_table[2].get(), - vector_table[3].get(), - vector_table[4].get() - ); - break; - case 5: - result = Joint( - vector_table[1].get(), - vector_table[2].get(), - vector_table[3].get(), - vector_table[4].get(), - vector_table[5].get() - ); - break; - case 6: - result = Joint( - vector_table[1].get(), - vector_table[2].get(), - vector_table[3].get(), - vector_table[4].get(), - vector_table[5].get(), - vector_table[6].get() - ); - break; - default: - cerr << "Invalid number of DOFs for joint." << endl; - abort(); - break; - } } - stackRestore(); - - return result; -} + for(unsigned int i=0; i -Body LuaTableNode::getDefault(const Body &default_value) { - Body result = default_value; + unsigned int numChars=0; - if (stackQueryValue()) { - LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + for(unsigned int i=0; i(com); - inertia = vector_table["inertia"].getDefault(inertia); + if(indexEntries.size()>0){ + updFileStream << ","; + for(unsigned int j=0; j 9){ + numChars-=unsigned(int(std::floor( log10(double(indexEntries[i][j]))))); + } + numChars -= 1; + for(unsigned int z=0; z& constraint_sets, const std::vector& constraint_set_names, bool verbose @@ -258,8 +274,10 @@ bool LuaModelReadConstraintsFromTable ( typedef map StringIntMap; StringIntMap body_table_id_map; +//============================================================================== RBDL_DLLAPI -bool LuaModelReadFromLuaState (lua_State* L, Model* model, bool verbose) { +bool LuaModelReadFromLuaState (lua_State* L, Model* model, bool verbose) +{ assert (model); LuaTable model_table = LuaTable::fromLuaState (L); @@ -267,22 +285,58 @@ bool LuaModelReadFromLuaState (lua_State* L, Model* model, bool verbose) { return LuaModelReadFromTable (model_table, model, verbose); } +//============================================================================== +bool LuaModelReadLocalFrames ( + LuaTable &model_table, + const RigidBodyDynamics::Model *model, + std::vector& upd_local_frame_set, + bool verbose); + RBDL_DLLAPI -bool LuaModelReadFromFile (const char* filename, Model* model, bool verbose) { - if(!model) { - std::cerr << "Model not provided." << std::endl; - assert(false); - abort(); - } +bool LuaModelReadLocalFrames ( + const char* filename, + const RigidBodyDynamics::Model *model, + std::vector& upd_local_frame_set, + bool verbose) +{ + LuaTable model_table = LuaTable::fromFile (filename); + return LuaModelReadLocalFrames(model_table,model,upd_local_frame_set,verbose); +} - LuaTable model_table = LuaTable::fromFile (filename); +//============================================================================== +bool LuaModelReadPoints ( + LuaTable &model_table, + const RigidBodyDynamics::Model *model, + std::vector& upd_point_set, + bool verbose); - return LuaModelReadFromTable (model_table, model, verbose); +RBDL_DLLAPI +bool LuaModelReadPoints ( + const char* filename, + const RigidBodyDynamics::Model *model, + std::vector& upd_point_set, + bool verbose) +{ + LuaTable model_table = LuaTable::fromFile (filename); + return LuaModelReadPoints(model_table,model,upd_point_set,verbose); } +//============================================================================== +RBDL_DLLAPI +bool LuaModelReadFromFile (const char* filename, Model* upd_model, bool verbose) +{ + if(!upd_model) { + throw Errors::RBDLError("Model not provided."); + } + + LuaTable model_table = LuaTable::fromFile (filename); + return LuaModelReadFromTable (model_table, upd_model, verbose); +} +//============================================================================== RBDL_DLLAPI -std::vector LuaModelGetConstraintSetNames(const char* filename) { +std::vector LuaModelGetConstraintSetNames(const char* filename) +{ std::vector result; LuaTable model_table = LuaTable::fromFile (filename); @@ -298,9 +352,9 @@ std::vector LuaModelGetConstraintSetNames(const char* filename) { for (size_t ci = 0; ci < constraint_keys.size(); ++ci) { if (constraint_keys[ci].type != LuaKey::String) { - std::cerr << "Invalid constraint found in model.constraint_sets: no constraint set name was specified!" - << std::endl; - abort(); + throw Errors::RBDLFileParseError( + "Invalid constraint found in model.constraint_sets: " + "no constraint set name was specified!"); } result.push_back(constraint_keys[ci].string_value); @@ -309,6 +363,37 @@ std::vector LuaModelGetConstraintSetNames(const char* filename) { return result; } +//============================================================================== +RBDL_DLLAPI +bool LuaModelGetConstraintSetPhases(const char* filename, + const std::vector &constraint_set_names, + std::vector< unsigned int > &constraint_set_phases) +{ + LuaTable luaTable = LuaTable::fromFile (filename); + unsigned int phases = luaTable["constraint_set_phases"].length(); + constraint_set_phases.resize(phases); + bool found = false; + std::string phaseName; + + for(unsigned int i=1; i(); + found = false; + for(unsigned int j=0; j& constraint_sets, const std::vector& constraint_set_names, bool verbose -) { +) +{ if(!model) { - std::cerr << "Model not provided." << std::endl; - assert(false); - abort(); + throw Errors::RBDLError("Model not provided."); } if(constraint_sets.size() != constraint_set_names.size()) { - std::cerr << "Number of constraint sets different from the number of \ - constraint set names." << std::endl; - assert(false); - abort(); + throw Errors::RBDLFileParseError( + "Number of constraint sets different from" + " the number of constraint set names."); } LuaTable model_table = LuaTable::fromFile (filename); bool modelLoaded = LuaModelReadFromTable (model_table, model, verbose); bool constraintsLoaded = LuaModelReadConstraintsFromTable (model_table, model - , constraint_sets, constraint_set_names, verbose); + , constraint_sets, constraint_set_names, verbose); for(size_t i = 0; i < constraint_sets.size(); ++i) { - constraint_sets[i].Bind(*model); + constraint_sets[i].Bind(*model); } return modelLoaded && constraintsLoaded; } - -bool LuaModelReadFromTable (LuaTable &model_table, Model* model, bool verbose) { +//============================================================================== +bool LuaModelReadFromTable (LuaTable &model_table, Model* model, bool verbose) +{ if (model_table["gravity"].exists()) { model->gravity = model_table["gravity"].get(); - if (verbose) + if (verbose) { cout << "gravity = " << model->gravity.transpose() << endl; + } } int frame_count = model_table["frames"].length(); @@ -355,32 +440,31 @@ bool LuaModelReadFromTable (LuaTable &model_table, Model* model, bool verbose) { for (int i = 1; i <= frame_count; i++) { if (!model_table["frames"][i]["parent"].exists()) { - cerr << "Parent not defined for frame " << i << "." << endl; - abort(); + throw Errors::RBDLError("Parent not defined for frame "); } string body_name = model_table["frames"][i]["name"].getDefault(""); string parent_name = model_table["frames"][i]["parent"].get(); unsigned int parent_id = body_table_id_map[parent_name]; - SpatialTransform joint_frame + SpatialTransform joint_frame = model_table["frames"][i]["joint_frame"].getDefault(SpatialTransform()); - Joint joint + Joint joint = model_table["frames"][i]["joint"].getDefault(Joint(JointTypeFixed)); Body body = model_table["frames"][i]["body"].getDefault(Body()); - unsigned int body_id + unsigned int body_id = model->AddBody (parent_id, joint_frame, joint, body, body_name); body_table_id_map[body_name] = body_id; if (verbose) { cout << "==== Added Body ====" << endl; cout << " body_name : " << body_name << endl; - cout << " body id : " << body_id << endl; + cout << " body id : " << body_id << endl; cout << " parent_id : " << parent_id << endl; cout << " joint dofs : " << joint.mDoFCount << endl; for (unsigned int j = 0; j < joint.mDoFCount; j++) { - cout << " " << j << ": " << joint.mJointAxes[j].transpose() << endl; + cout << " " << j << ": " << joint.mJointAxes[j].transpose() << endl; } cout << " joint_frame: " << joint_frame << endl; } @@ -389,177 +473,1518 @@ bool LuaModelReadFromTable (LuaTable &model_table, Model* model, bool verbose) { return true; } +//============================================================================== bool LuaModelReadConstraintsFromTable ( LuaTable &model_table, - Model *model, + const Model *model, std::vector& constraint_sets, const std::vector& constraint_set_names, bool verbose -) { +) +{ + std::string conName; + + std::vector< Vector3d > normalSets; + MatrixNd normalSetsMatrix; + Vector3d normal; + + MatrixNd axisSetsMatrix; + SpatialVector axis; + std::vector< SpatialVector > axisSets; + + std::vector< Point > pointSet; + bool pointSetLoaded = LuaModelReadPoints(model_table,model,pointSet,verbose); + + std::vector< LocalFrame > localFrameSet; + bool localFramesLoaded = + LuaModelReadLocalFrames(model_table,model,localFrameSet,verbose); + for(size_t i = 0; i < constraint_set_names.size(); ++i) { + conName = constraint_set_names[i]; if (verbose) { - std::cout << "==== Constraint Set: " << constraint_set_names[i] << std::endl; + std::cout << "==== Constraint Set: " << conName << std::endl; } - if(!model_table["constraint_sets"][constraint_set_names[i].c_str()] - .exists()) { - cerr << "Constraint set not existing: " << constraint_set_names[i] << "." - << endl; - assert(false); - abort(); + if(!model_table["constraint_sets"][conName.c_str()] + .exists()) { + ostringstream errormsg; + errormsg << "Constraint set not existing: " << conName << "." << endl; + throw Errors::RBDLFileParseError(errormsg.str()); } size_t num_constraints = model_table["constraint_sets"] - [constraint_set_names[i].c_str()] - .length(); + [conName.c_str()] + .length(); for(size_t ci = 0; ci < num_constraints; ++ci) { if (verbose) { - std::cout << "== Constraint " << ci << "/" << num_constraints << " ==" << std::endl; + std::cout << "== Constraint " << ci << "/" << num_constraints + << " ==" << std::endl; } if(!model_table["constraint_sets"] - [constraint_set_names[i].c_str()][ci + 1]["constraint_type"].exists()) { - cerr << "constraint_type not specified." << endl; - assert(false); - abort(); + [conName.c_str()][ci + 1]["constraint_type"].exists()) { + throw Errors::RBDLFileParseError("constraint_type not specified.\n"); } + string constraintType = model_table["constraint_sets"] - [constraint_set_names[i].c_str()][ci + 1]["constraint_type"] - .getDefault(""); - std::string constraint_name = model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] - ["name"].getDefault(""); + [conName.c_str()][ci + 1]["constraint_type"] + .getDefault(""); + std::string constraint_name = + model_table["constraint_sets"][conName.c_str()] + [ci + 1]["name"].getDefault(""); + + bool enable_stabilization = + model_table["constraint_sets"][conName.c_str()][ci + 1] + ["enable_stabilization"].getDefault(false); + double stabilization_parameter = 0.1; + + if (enable_stabilization) { + stabilization_parameter = + model_table["constraint_sets"][conName.c_str()][ci + 1] + ["stabilization_parameter"].getDefault(0.1); + if (stabilization_parameter <= 0.0) { + std::stringstream errormsg; + errormsg << "Invalid stabilization parameter: " + << stabilization_parameter + << " must be > 0.0" << std::endl; + throw Errors::RBDLFileParseError(errormsg.str()); + } + } + + //======================================================================== + //Contact + //======================================================================== if(constraintType == "contact") { - if(!model_table["constraint_sets"][constraint_set_names[i].c_str()] - [ci + 1]["body"].exists()) { - cerr << "body not specified." << endl; - assert(false); - abort(); + + unsigned int constraint_user_id = + std::numeric_limits::max(); + + if(model_table["constraint_sets"][conName.c_str()][ci + 1] + ["id"].exists()) { + constraint_user_id = unsigned(int( + model_table["constraint_sets"][conName.c_str()][ci + 1] + ["id"].getDefault(0.))); + } + + //Go get the body id and the local coordinates of the point: + // If a named point is given, go through the point set + // Otherwise look for the explicit fields + unsigned int bodyId; + Vector3d bodyPoint; + + if(model_table["constraint_sets"][conName.c_str()][ci+1] + ["point_name"].exists()){ + std::string pointName = model_table["constraint_sets"] + [conName.c_str()][ci+1]["point_name"].getDefault(""); + bool pointFound = false; + unsigned int pi=0; + while(pi < pointSet.size() && pointFound == false){ + if(std::strcmp(pointSet[pi].name.c_str(),pointName.c_str())==0){ + pointFound = true; + bodyId = pointSet[pi].body_id; + bodyPoint = pointSet[pi].point_local; + } + ++pi; + } + if(pointFound == false){ + ostringstream errormsg; + errormsg << "Could not find a point with the name: " << pointName + << " which is used in constraint set " << constraint_name + << endl; + throw Errors::RBDLFileParseError(errormsg.str()); + } + }else{ + if(!model_table["constraint_sets"][conName.c_str()] + [ci + 1]["body"].exists()) { + throw Errors::RBDLFileParseError("body not specified.\n"); + } + bodyId = model->GetBodyId(model_table["constraint_sets"] + [conName.c_str()][ci + 1]["body"] + .getDefault("").c_str()); + + bodyPoint = model_table["constraint_sets"] + [conName.c_str()][ci + 1] + ["point"].getDefault(Vector3d::Zero()); + } + + normalSets.resize(0); + normalSetsMatrix.resize(1,1); + + if(model_table["constraint_sets"][conName.c_str()][ci + 1] + ["normal_sets"].exists()) { + + normalSetsMatrix = + model_table["constraint_sets"][conName.c_str()] + [ci + 1]["normal_sets"].getDefault< MatrixNd >(MatrixNd::Zero(1,1)); + + if(normalSetsMatrix.cols() != 3 ) { + std::ostringstream errormsg; + errormsg << "The normal_sets field must be m x 3, the one read for " + << conName.c_str() << " has an normal_sets of size " + << normalSetsMatrix.rows() << " x " + << normalSetsMatrix.cols() + << ". In addition the normal_sets field should resemble:" + << endl; + errormsg << " normal_sets = {{1.,0.,0.,}, " << endl; + errormsg << " {0.,1.,0.,},}, " << endl; + throw Errors::RBDLFileParseError(errormsg.str()); + } + + + for(unsigned int r=0; r(Vector3d::Zero()); + normalSets.push_back(normal); + + } else { + std::ostringstream errormsg; + errormsg << "The ContactConstraint must have either normal_sets field " + "(which is a m x 3 matrix) or an normal field. Neither of " + "these fields was found in " + << conName.c_str() << endl; + throw Errors::RBDLFileParseError(errormsg.str()); + } + + std::string contactName = model_table["constraint_sets"] + [conName.c_str()][ci + 1] + ["name"].getDefault("").c_str(); + + for(unsigned int c=0; cGetBodyId(model_table["constraint_sets"] - [constraint_set_names[i].c_str()][ci + 1]["body"] - .getDefault("").c_str()) - , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] - ["point"].getDefault(Vector3d::Zero()) - , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] - ["normal"].getDefault(Vector3d::Zero()) - , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] - ["name"].getDefault("").c_str() - , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] - ["normal_acceleration"].getDefault(0.)); + if(verbose) { - cout << " type = contact" << endl; - cout << " name = " << constraint_name << std::endl; - cout << " body = " - << model_table["constraint_sets"][constraint_set_names[i].c_str()] - [ci + 1]["body"].getDefault("") << endl; - cout << " body point = " - << model_table["constraint_sets"][constraint_set_names[i].c_str()] - [ci + 1]["point"].getDefault(Vector3d::Zero()).transpose() - << endl; - cout << " world normal = " - << model_table["constraint_sets"][constraint_set_names[i].c_str()] - [ci + 1]["normal"].getDefault(Vector3d::Zero()).transpose() - << endl; - cout << " normal acceleration = " - << model_table["constraint_sets"][constraint_set_names[i].c_str()] - [ci + 1]["normal_acceleration"].getDefault(0.) << endl; + cout << " type = contact" << endl; + cout << " name = " << constraint_name << std::endl; + cout << " body = " + << model->GetBodyName(bodyId) << endl; + cout << " body point = " + << bodyPoint.transpose() + << endl; + cout << " world normal = " << endl; + for(unsigned int c=0; c::max(); + if(model_table["constraint_sets"][conName.c_str()][ci + 1] + ["id"].exists()) { + constraint_user_id = + unsigned(int(model_table["constraint_sets"][conName.c_str()] + [ci + 1]["id"].getDefault(0.))); + } + + //Get the local frames that this constraint will be applied to + // If named local frames have been given, use them + // Otherwise look for the individual fields + unsigned int idPredecessor; + unsigned int idSuccessor; + SpatialTransform Xp; + SpatialTransform Xs; + + if(model_table["constraint_sets"] + [conName.c_str()][ci + 1]["predecessor_local_frame"].exists()){ + + std::string localFrameName = model_table["constraint_sets"] + [conName.c_str()][ci + 1]["predecessor_local_frame"] + .getDefault(""); + bool frameFound = false; + unsigned int fi=0; + while(fi < localFrameSet.size() && frameFound == false){ + if(std::strcmp(localFrameSet[fi].name.c_str(), + localFrameName.c_str())==0){ + frameFound = true; + idPredecessor = localFrameSet[fi].body_id; + Xp.r = localFrameSet[fi].r; + Xp.E = localFrameSet[fi].E; + } + ++fi; + } + if(frameFound == false){ + ostringstream errormsg; + errormsg << "Could not find a local frame with the name: " + << localFrameName + << " which is used in constraint set " << constraint_name + << endl; + throw Errors::RBDLFileParseError(errormsg.str()); + } + }else{ + if(!model_table["constraint_sets"][conName.c_str()] + [ci + 1]["predecessor_body"].exists()) { + throw Errors::RBDLFileParseError( + "predecessor_body not specified.\n"); + } + + idPredecessor = + model->GetBodyId(model_table["constraint_sets"] + [conName.c_str()][ci + 1]["predecessor_body"] + .getDefault("").c_str()); + Xp = model_table["constraint_sets"][conName.c_str()] + [ci + 1]["predecessor_transform"] + .getDefault(SpatialTransform()); } - if(!model_table["constraint_sets"][constraint_set_names[i].c_str()] - [ci + 1]["successor_body"].exists()) { - cerr << "successor_body not specified." << endl; - assert(false); - abort(); + + if(model_table["constraint_sets"] + [conName.c_str()][ci + 1]["successor_local_frame"].exists()){ + + std::string localFrameName = model_table["constraint_sets"] + [conName.c_str()][ci + 1]["successor_local_frame"] + .getDefault(""); + bool frameFound = false; + unsigned int fi=0; + while(fi < localFrameSet.size() && frameFound == false){ + if(std::strcmp(localFrameSet[fi].name.c_str(), + localFrameName.c_str())==0){ + frameFound = true; + idSuccessor = localFrameSet[fi].body_id; + Xs.r = localFrameSet[fi].r; + Xs.E = localFrameSet[fi].E; + } + ++fi; + } + if(frameFound == false){ + ostringstream errormsg; + errormsg << "Could not find a local frame with the name: " + << localFrameName + << " which is used in constraint set " << constraint_name + << endl; + throw Errors::RBDLFileParseError(errormsg.str()); + } + + }else{ + + if(!model_table["constraint_sets"][conName.c_str()] + [ci + 1]["successor_body"].exists()) { + throw Errors::RBDLFileParseError("successor_body not specified.\n"); + } + + idSuccessor = + model->GetBodyId(model_table["constraint_sets"] + [conName.c_str()][ci + 1]["successor_body"] + .getDefault("").c_str()); + + + Xs = model_table["constraint_sets"][conName.c_str()] + [ci + 1]["successor_transform"] + .getDefault(SpatialTransform()); } + + // Add the loop constraint as a non-stablized constraint and compute // and set the actual stabilization cofficients for the Baumgarte // stabilization afterwards if enabled. - unsigned int constraint_id; - constraint_id = constraint_sets[i].AddLoopConstraint(model->GetBodyId - (model_table["constraint_sets"][constraint_set_names[i].c_str()] - [ci + 1]["predecessor_body"].getDefault("").c_str()) - , model->GetBodyId(model_table["constraint_sets"] - [constraint_set_names[i].c_str()][ci + 1]["successor_body"] - .getDefault("").c_str()) - , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] - ["predecessor_transform"].getDefault(SpatialTransform()) - , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] - ["successor_transform"].getDefault(SpatialTransform()) - , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] - ["axis"].getDefault(SpatialVector::Zero()) - , false - , 0.0 - , constraint_name.c_str()); - - bool enable_stabilization = model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] - ["enable_stabilization"].getDefault(false); - double stabilization_parameter = 0.0; - if (enable_stabilization) { - stabilization_parameter = model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] - ["stabilization_parameter"].getDefault(0.1); - if (stabilization_parameter <= 0.0) { - std::cerr << "Invalid stabilization parameter: " << stabilization_parameter - << " must be > 0.0" << std::endl; - abort(); + + axisSetsMatrix.resize(1,1); + axisSets.resize(0); + if(model_table["constraint_sets"][conName.c_str()][ci + 1] + ["axis_sets"].exists()) { + axisSetsMatrix = + model_table["constraint_sets"][conName.c_str()][ci + 1] + ["axis_sets"].getDefault< MatrixNd >( MatrixNd::Zero(1,1)); + + if(axisSetsMatrix.cols() != 6 ) { + std::stringstream errormsg; + errormsg << "The axis_sets field must be m x 6, the one read for " + << conName.c_str() << " has an axis_sets of size " + << axisSetsMatrix.rows() << " x " << axisSetsMatrix.cols() + << ". In addition the axis_sets field should resemble:" + << endl; + errormsg << " axis_sets = {{0.,0.,0.,1.,0.,0.,}, " <( SpatialVector::Zero()); + + axisSets.push_back(axis); + + } else { + std::stringstream errormsg; + errormsg << "The LoopConstraint must have either axis_sets field " + "(which is a m x 6 matrix) or an axis field. Neither of " + "these fields was found in " + << conName.c_str() << endl; + throw Errors::RBDLFileParseError(errormsg.str()); + } + + unsigned int constraint_id; + for(unsigned int r=0; r("") << endl; - cout << " successor body = " - << model_table["constraint_sets"][constraint_set_names[i].c_str()] - [ci + 1]["successor_body"].getDefault("") << endl; - cout << " predecessor body transform = " << endl - << model_table["constraint_sets"][constraint_set_names[i].c_str()] - [ci + 1]["predecessor_transform"] - .getDefault(SpatialTransform()) << endl; - cout << " successor body transform = " << endl - << model_table["constraint_sets"][constraint_set_names[i].c_str()] - [ci + 1]["successor_transform"] - .getDefault(SpatialTransform()) << endl; - cout << " constraint axis (in predecessor frame) = " - << model_table["constraint_sets"][constraint_set_names[i].c_str()] - [ci + 1]["axis"].getDefault(SpatialVector::Zero()) - .transpose() << endl; - cout << " enable_stabilization = " << enable_stabilization - << endl; + cout << " predecessor body = " + << model->GetBodyName(idPredecessor)<< endl; + cout << " successor body = " + << model->GetBodyName(idSuccessor) << endl; + cout << " predecessor body transform = " << endl + << Xp << endl; + cout << " successor body transform = " << endl + << Xs << endl; + cout << " constraint axis (in predecessor frame) = " << endl; + for(unsigned int c=0; c& upd_marker_set, + bool verbose) +{ + + LuaTable luaTable = LuaTable::fromFile (filename); + upd_marker_set.clear(); + + if(luaTable["frames"].exists()){ + unsigned int frameCount = luaTable["frames"].length(); + std::vector marker_keys; + MotionCaptureMarker marker; + std::string body_name; + unsigned int body_id; + for(unsigned int i=1; i(""); + body_id = model->GetBodyId(body_name.c_str()); + marker_keys = luaTable["frames"][i]["markers"].keys(); + + for(unsigned int j=0; j < marker_keys.size(); ++j){ + if (marker_keys[j].type != LuaKey::String) { + throw Errors::RBDLFileParseError( + "Invalid marker found: missing name!"); } - cout << " constraint name = " - << model_table["constraint_sets"][constraint_set_names[i].c_str()] - [ci + 1]["name"].getDefault("").c_str() << endl; + marker.name = marker_keys[j].string_value; + marker.body_name = body_name; + marker.body_id = body_id; + marker.point_local = luaTable["frames"][i]["markers"][marker.name.c_str()] + .getDefault(Vector3d::Zero()); + upd_marker_set.push_back(marker); } } - else { - cerr << "Invalid constraint type: " << constraintType << endl; - abort(); + } + } + + return true; +} +//============================================================================== +bool LuaModelReadLocalFrames ( + LuaTable &model_table, + const RigidBodyDynamics::Model *model, + std::vector& upd_local_frame_set, + bool verbose) +{ + //LuaTable luaTable = LuaTable::fromFile (filename); + upd_local_frame_set.clear(); + unsigned int localFrameCount = + unsigned(int(model_table["local_frames"].length())); + + if(localFrameCount > 0){ + upd_local_frame_set.resize(localFrameCount); + LocalFrame localFrame; + + for (unsigned int i = 1; i <= localFrameCount; ++i) { + + localFrame = model_table["local_frames"][signed(i)]; + + localFrame.body_id = model->GetBodyId (localFrame.body_name.c_str()); + upd_local_frame_set[i-1] = localFrame; + + if (verbose) { + cout << "LocalFrame '" << upd_local_frame_set[i-1].name + << "' (name = " << upd_local_frame_set[i-1].name << ")" << endl; + cout << " body = " << upd_local_frame_set[i-1].body_name + << " (id = " << upd_local_frame_set[i-1].body_id << ")" << endl; + cout << " r = '" << upd_local_frame_set[i-1].r.transpose() << endl; + cout << " E = '" << upd_local_frame_set[i-1].E << endl; + } + } + } + return true; +} + +//============================================================================== +bool LuaModelReadPoints ( + LuaTable &model_table, + const RigidBodyDynamics::Model *model, + std::vector& upd_point_set, + bool verbose) +{ + + upd_point_set.clear(); + unsigned int pointCount = unsigned(int(model_table["points"].length())); + + if(pointCount > 0){ + upd_point_set.resize(pointCount); + Point point; + + for (unsigned int i = 1; i <= pointCount; ++i) { + + point = model_table["points"][i]; + + point.body_id = model->GetBodyId (point.body_name.c_str()); + upd_point_set[i-1] = point; + + if (verbose) { + cout << "Point '" << upd_point_set[i-1].name + << "' (PointName = " << upd_point_set[i-1].name << ")" << endl; + cout << " body = " << upd_point_set[i-1].body_name + << " (id = " << upd_point_set[i-1].body_id << ")" << endl; + cout << " point_local = '" << upd_point_set[i-1].point_local.transpose() + << endl; } } } + return true; +} + +//============================================================================== +bool LuaModelReadHumanMetaData( + const char* filename, + HumanMetaData &human_meta_data, + bool verbose) +{ + + LuaTable luaTable = LuaTable::fromFile (filename); + unsigned int subjectCount = luaTable["human_meta_data"].length(); + + if(subjectCount != 1){ + ostringstream errormsg; + errormsg << "human_meta_data should contain data " + << "for only 1 subject but has data for " + << subjectCount << endl; + + throw Errors::RBDLError(errormsg.str()); + } + + human_meta_data = luaTable["human_meta_data"][1]; return true; } +//============================================================================== +#ifdef RBDL_BUILD_ADDON_MUSCLE +std::vector getQIndex( + const RigidBodyDynamics::Model *model, + const char *childBodyName) +{ + + unsigned int idChild = model->GetBodyId(childBodyName); + unsigned int idJoint=idChild; + while(idJoint > 1 && model->mBodies[idJoint-1].mIsVirtual){ + --idJoint; + } + + std::vector qIndex; + + unsigned int ccid; + + unsigned int id=idJoint; + unsigned int dof = 0; + + while(id <= idChild){ + if(model->mJoints[id].mJointType == JointTypeCustom){ + ccid = model->mJoints[id].custom_joint_index; + for(unsigned int i=0; imCustomJoints[ccid]->mDoFCount;++i){ + qIndex.push_back(model->mJoints[id].q_index+i); + } + dof = model->mCustomJoints[ccid]->mDoFCount; + }else{ + for(unsigned int i=0; imJoints[id].mDoFCount;++i){ + qIndex.push_back(model->mJoints[id].q_index+i); + } + dof = model->mJoints[id].mDoFCount; + } + id += dof; + } + //To get the extra index for the quaternion joints + for(id=idJoint;id<=idChild;++id){ + if(model->multdof3_w_index[id] > 0){ + qIndex.push_back(model->multdof3_w_index[id]); + } + } + return qIndex; +} + +//============================================================================== + +unsigned int getMillard2016TorqueMuscleTypeId(std::string name) +{ + + + unsigned int i = 0; + while (i != JointTorqueSet::Last) { + if (name.find( std::string(JointTorqueSet.names[i]) ) + != std::string::npos ) { + break; + } + ++i; + } + + if (i == JointTorqueSet::Last) { + cerr <<"Error: " << name << " does not contain the name of any registered" + <<" muscle torque generator. For the full list look for the definition" + <<" of JointTorqueSet::names[] in " + <<"addons/muscle/Millard2016TorqueMuscle.cc" << endl; + assert(0); + abort(); + } + + return i; + } +//============================================================================== +bool LuaModelReadMillard2016TorqueMuscleSets( + const char* filename, + const RigidBodyDynamics::Model *model, + const HumanMetaData &human_meta_data, + std::vector &updMtgSet, + std::vector &updMtgSetInfo, + bool verbose) +{ + + + LuaTable luaTable = LuaTable::fromFile (filename); + unsigned int mtgCount = luaTable["millard2016_torque_muscles"].length(); + + + updMtgSet.resize(mtgCount); + updMtgSetInfo.resize(mtgCount); + Millard2016TorqueMuscleConfig mtgInfo; + Millard2016TorqueMuscleConfig mtgInfoDefault; + unsigned int id; + + for(unsigned int i = 1; i <= mtgCount; ++i){ + mtgInfo = luaTable["millard2016_torque_muscles"][i]; + id = i-unsigned(int(1)); + + updMtgSetInfo[id] = mtgInfo; + + if (verbose) { + if(i == 1){ + std::cout << "Millard2016TorqueMuscle" << std::endl; + std::cout << std::setw(20) << "Name" + << std::setw(10) << "Angle-Sign" + << std::setw(11) << "Torque-Sign" + << std::endl; + } + std::cout << std::setw(24) << mtgInfo.name + << std::setw(3) << mtgInfo.angle_sign + << std::setw(3) << mtgInfo.torque_sign + << std::endl; + } + + } + + //Populate the subject information structure + SubjectInformation subjectInfo; + if (human_meta_data.gender == "male") { + subjectInfo.gender = GenderSet::Male; + }else if (human_meta_data.gender == "female") { + subjectInfo.gender = GenderSet::Female; + }else { + cerr << "Unknown gender in subject metadata, " << + human_meta_data.gender << endl; + abort(); + } + + if (human_meta_data.age_group == "Young18To25") { + subjectInfo.ageGroup = + AgeGroupSet::Young18To25; + }else if (human_meta_data.age_group == "Middle55to65") { + subjectInfo.ageGroup = + AgeGroupSet::Middle55To65; + }else if (human_meta_data.age_group == "SeniorOver65") { + subjectInfo.ageGroup = + AgeGroupSet::SeniorOver65; + }else { + cerr << "Unknown age group in subject metadata, " << + human_meta_data.age_group << endl; + abort(); + } + + subjectInfo.heightInMeters = human_meta_data.height; + subjectInfo.massInKg = human_meta_data.mass; + + + DataSet::item mtgDataSet; + SubjectInformation mtgSubjectInfo; + + for(unsigned int i = 0; i < mtgCount; ++i){ + mtgSubjectInfo = subjectInfo; + + //Get the data set for this MTG + if(updMtgSetInfo[i].data_set == "Gymnast"){ + mtgDataSet = DataSet::Gymnast; + }else if(updMtgSetInfo[i].data_set == "Anderson2007"){ + mtgDataSet = DataSet::Anderson2007; + }else{ + cerr << "Error: the data_set entry for " + << updMtgSetInfo[i].name << " is " + << updMtgSetInfo[i].data_set + << "which neither Gymnast nor Anderson2007."; + assert(0); + abort(); + } + + //Get the age group for this MTG + if(updMtgSetInfo[i].age_group == "Young18To25"){ + mtgSubjectInfo.ageGroup = AgeGroupSet::Young18To25; + }else if(updMtgSetInfo[i].age_group == "Middle55To65"){ + mtgSubjectInfo.ageGroup = AgeGroupSet::Middle55To65; + }else if(updMtgSetInfo[i].age_group == "SeniorOver65"){ + mtgSubjectInfo.ageGroup = AgeGroupSet::SeniorOver65; + }else{ + mtgSubjectInfo.ageGroup = subjectInfo.ageGroup; + } + + + //Get the gender for this MTG + if (updMtgSetInfo[i].gender == "male") { + mtgSubjectInfo.gender = GenderSet::Male; + }else if (updMtgSetInfo[i].gender == "female") { + mtgSubjectInfo.gender = GenderSet::Female; + }else { + mtgSubjectInfo.gender = subjectInfo.gender; + } + + if(updMtgSetInfo[i].body != mtgInfoDefault.body){ + unsigned int joint_offset = 0; + if(updMtgSetInfo[i].joint_index != mtgInfoDefault.joint_index){ + joint_offset =updMtgSetInfo[i].joint_index; + } + + //Go get the index of the first joint between the child body (given) + //and its parent. + std::vector qIndices = + getQIndex(model, updMtgSetInfo[i].body.c_str()); + + updMtgSetInfo[i].q_index = qIndices[0] + joint_offset; + updMtgSetInfo[i].qdot_index = updMtgSetInfo[i].q_index; + updMtgSetInfo[i].force_index = updMtgSetInfo[i].q_index; + + } + if(updMtgSetInfo[i].activation_index == mtgInfoDefault.activation_index){ + updMtgSetInfo[i].activation_index = i; + } + + updMtgSet[i] = Millard2016TorqueMuscle( + mtgDataSet, + mtgSubjectInfo, + int(getMillard2016TorqueMuscleTypeId(updMtgSetInfo[i].name)), + updMtgSetInfo[i].joint_angle_offset, + updMtgSetInfo[i].angle_sign, + updMtgSetInfo[i].torque_sign, + updMtgSetInfo[i].name); + + //Parameters for manual adjustment + if (!std::isnan(updMtgSetInfo[i].max_isometric_torque_scale)) { + double fiso = updMtgSet[i].getMaximumActiveIsometricTorque(); + double updFiso = fiso*updMtgSetInfo[i].max_isometric_torque_scale; + updMtgSet[i].setMaximumActiveIsometricTorque(updFiso); + } + + if (!std::isnan(updMtgSetInfo[i].max_angular_velocity_scale)) { + double omegaMax = + updMtgSet[i].getMaximumConcentricJointAngularVelocity(); + double updOmegaMax = + omegaMax*updMtgSetInfo[i].max_angular_velocity_scale; + updMtgSet[i].setMaximumConcentricJointAngularVelocity(updOmegaMax); + } + + + if (!std::isnan(updMtgSetInfo[i].passive_element_damping_coeff)) { + updMtgSet[i].setNormalizedDampingCoefficient( + updMtgSetInfo[i].passive_element_damping_coeff); + } + + if (!std::isnan(updMtgSetInfo[i].passive_element_torque_scale)) { + updMtgSet[i].setPassiveTorqueScale( + updMtgSetInfo[i].passive_element_torque_scale); + } + + if (!std::isnan(updMtgSetInfo[i].passive_element_angle_offset)) { + updMtgSet[i].setPassiveCurveAngleOffset( + updMtgSetInfo[i].passive_element_angle_offset); + } + + //Basic fitting of the passive curves + if (!std::isnan(updMtgSetInfo[i].fit_passive_torque_scale[0]) && + !std::isnan(updMtgSetInfo[i].fit_passive_torque_scale[1])) { + updMtgSet[i].fitPassiveTorqueScale( + updMtgSetInfo[i].fit_passive_torque_scale[0], + updMtgSetInfo[i].fit_passive_torque_scale[1]); + } + if (!std::isnan(updMtgSetInfo[i].fit_passive_torque_offset[0]) && + !std::isnan(updMtgSetInfo[i].fit_passive_torque_offset[1])) { + + updMtgSet[i].fitPassiveCurveAngleOffset( + updMtgSetInfo[i].fit_passive_torque_offset[0], + updMtgSetInfo[i].fit_passive_torque_offset[1]); + } + + //Fitting parameters from the fitting method + + if(!std::isnan(updMtgSetInfo[i].max_isometric_torque)){ + updMtgSet[i].setMaximumActiveIsometricTorque( + updMtgSetInfo[i].max_isometric_torque); + } + + if(!std::isnan(updMtgSetInfo[i].max_angular_velocity)){ + updMtgSet[i].setMaximumConcentricJointAngularVelocity( + updMtgSetInfo[i].max_angular_velocity); + } + if(!std::isnan(updMtgSetInfo[i].active_torque_angle_blending)) { + updMtgSet[i].setActiveTorqueAngleCurveBlendingVariable( + updMtgSetInfo[i].active_torque_angle_blending); + } + if(!std::isnan(updMtgSetInfo[i].passive_torque_angle_blending)) { + updMtgSet[i].setPassiveTorqueAngleCurveBlendingVariable( + updMtgSetInfo[i].passive_torque_angle_blending); + } + if(!std::isnan(updMtgSetInfo[i].torque_velocity_blending)) { + updMtgSet[i].setTorqueAngularVelocityCurveBlendingVariable( + updMtgSetInfo[i].torque_velocity_blending); + } + if(!std::isnan(updMtgSetInfo[i].active_torque_angle_scale)) { + updMtgSet[i].setActiveTorqueAngleCurveAngleScaling( + updMtgSetInfo[i].active_torque_angle_scale); + } + + + + } + + return true; +} +#endif +//============================================================================== +bool LuaModelAddHeaderGuards(const char* filename){ + + //Extract the file name and capitalize it to make a custom headerguard + std::string name; + std::string headerFileName(filename); + unsigned int idx0 = unsigned(int(headerFileName.find_last_of("/"))); + if(idx0 == headerFileName.size()){ + idx0 = unsigned(int(headerFileName.find_last_of("\\"))); + } + if(idx0==headerFileName.size()){ + idx0=0; + }else{ + ++idx0; + } + unsigned int idx1 = unsigned(int(headerFileName.find_last_of("."))); + if(idx1 == headerFileName.size()){ + idx1 = unsigned(int(headerFileName.size())); + } + name = headerFileName.substr(idx0, (idx1-idx0)); + + std::transform(name.begin(), name.end(), name.begin(), ::toupper); + + //Read in the entire header file + ifstream headerFileInput(headerFileName.c_str()); //taking file as inputstream + string headerFileText; + ostringstream ss; + if(headerFileInput) { + ss << headerFileInput.rdbuf(); // reading data + headerFileText = ss.str(); + } + headerFileInput.close(); + + ofstream headerFileOut(headerFileName.c_str(),std::ofstream::out); + headerFileOut << "#ifndef " << name << "_MAP\n"; + headerFileOut << "#define " << name << "_MAP\n"; + headerFileOut << headerFileText << "//" << name << "_MAP\n" << "#endif\n"; + + headerFileOut.flush(); + headerFileOut.close(); + + return true; + +} +//============================================================================== +void LuaModelGetCoordinateNames( + const Model* model, + std::vector< std::string >& qNames, + std::vector< std::string >& qDotNames, + std::vector< std::string >& tauNames) +{ + + qNames.resize(model->q_size); + qDotNames.resize(model->qdot_size); + tauNames.resize(model->qdot_size); + + unsigned int q_index; + unsigned int idChild; + unsigned int dof; + + std::ostringstream ss; + + std::string jointType; + std::string axisType; + unsigned int axisIndex; + unsigned int ccid; + std::string childName; + bool newBody=false; + + //Populate vectors that q, qdot, and tau + for(unsigned int idJoint=1;idJoint < model->mJoints.size();++idJoint){ + + //Get the first index in q associated with this joint + q_index = model->mJoints[idJoint].q_index; + + //Get the index of the child body to this joint. + idChild=idJoint; + while(idChild < model->mBodies.size() + && model->mBodies[idChild].mIsVirtual){ + ++idChild; + } + + + //Get the name of the child body associated with this index + for(map::const_iterator it=model->mBodyNameMap.begin(); + it != model->mBodyNameMap.end(); ++it){ + if(it->second == idChild){ + childName = it->first; + } + } + + //Get the name of the joint type + jointType = JointMap[ model->mJoints[idJoint].mJointType].abbr; + if(jointType.length()>0){ + jointType.append("_"); + } + childName.append("_"); + //If this is a canonical joint, go get the index of the spatial axis + if( model->mJoints[idJoint].mJointType==JointTypeCustom){ + ccid = model->mJoints[idJoint].custom_joint_index; + dof = model->mCustomJoints[ccid]->mDoFCount; + for(unsigned int i=0; imCustomJoints[ccid]->mDoFCount;++i){ + ss.str(""); + ss << childName << jointType << i ; + qNames[q_index+i] = std::string("Q_").append( ss.str()); + qDotNames[q_index+i] = std::string("QDot_").append( ss.str()); + tauNames[q_index+i] = std::string("Tau_").append( ss.str()); + } + }else{ + dof = model->mJoints[idJoint].mDoFCount; + axisIndex=6; + for(unsigned int j=0; j<6;++j){ + if( fabs( fabs(model->mJoints[idJoint].mJointAxes[0][j])-1.) + < std::numeric_limits::epsilon() ){ + axisIndex=j; + } + } + if(axisIndex == 6){ + axisType= std::to_string(axisIndex); + }else{ + axisType = AxisMap[ axisIndex ].name; + if(model->mJoints[idJoint].mJointAxes[0][axisIndex] < 0.){ + axisType.append("N"); + } + } + for(unsigned int i=0; imJoints[idJoint].mDoFCount;++i){ + + if(model->mJoints[idJoint].mDoFCount > 1){ + axisType = std::to_string(i); + } + + ss.str(""); + ss << childName << jointType << axisType ; + qNames[q_index+i] = std::string("Q_").append( ss.str()); + qDotNames[q_index+i] = std::string("QDot_").append( ss.str()); + tauNames[q_index+i] = std::string("Tau_").append( ss.str()); + } + //If there is a quaternion joint the 4th component (w) is stored + //in a different location + if(model->multdof3_w_index[idJoint] > 0.){ + + ss.str(""); + ss << childName << jointType << "w"; + qNames[ model->multdof3_w_index[idJoint] ] + = std::string("Q_").append( ss.str()); + } + + } + + + } + +} + +//============================================================================== +bool LuaModelWriteModelHeaderEntries(const char* filename, + const RigidBodyDynamics::Model &model, + bool append){ + + + + std::vector< std::string > bodyNames; + std::vector< unsigned int> bodyIndex; + std::vector< std::string > qNames; + std::vector< std::string > qDotNames; + std::vector< std::string > qDDotNames; + std::vector< std::string > tauNames; + std::string tempStr; + LuaModelGetCoordinateNames(&model, qNames,qDotNames, tauNames); + + for(unsigned int i=0; i::const_iterator it=model.mBodyNameMap.begin(); + it != model.mBodyNameMap.end(); ++it){ + if(it->second == idChild){ + childName = it->first; + } + } + newBody=true; + for(unsigned int i=0; i qIndex; + std::vector qDotIndex; + + qIndex.resize(qNames.size()); + qDotIndex.resize(qDotNames.size()); + for(unsigned int i=0; i::max()); + qIndex.push_back(std::numeric_limits::max()); + qDotIndex.push_back(std::numeric_limits::max()); + + appendEnumNameIndexStructToFileStream(headerFile,"BodyMap","BodyId", + bodyNames,"BodyId_",bodyIndex); + appendEnumNameIndexStructToFileStream(headerFile,"PositionMap","PositionId", + qNames,"",qIndex); + appendEnumNameIndexStructToFileStream(headerFile,"VelocityMap","VelocityId", + qDotNames,"",qDotIndex); + appendEnumNameIndexStructToFileStream(headerFile,"AccelerationMap", + "AccelerationId", qDDotNames,"",qDotIndex); + appendEnumNameIndexStructToFileStream(headerFile,"ForceMap","ForceId", + tauNames,"",qDotIndex); + + headerFile.flush(); + headerFile.close(); + + return true; +} +//============================================================================== +RBDL_DLLAPI +bool LuaModelWritePointsHeaderEntries(const char* header_file_name, + const std::vector &point_set, + bool append) +{ + + std::ofstream headerFile; + if(append){ + headerFile.open(header_file_name,std::ofstream::app); + }else{ + headerFile.open(header_file_name,std::ofstream::out); + } + + std::vector< std::string > names; + std::vector< unsigned int > indices; + + names.resize(point_set.size()); + indices.resize(point_set.size()); + + for(unsigned int i=0;i0){ + names.push_back("Last"); + appendEnumToFileStream(headerFile,"PointId", names, 0, "Point_"); + indices.push_back(std::numeric_limits::max()); + appendEnumNameIndexStructToFileStream(headerFile,"PointMap","PointId", + names,"Point_",indices); + } + headerFile.flush(); + headerFile.close(); + + return true; +} +//============================================================================== +RBDL_DLLAPI +bool LuaModelWriteMotionCaptureMarkerHeaderEntries( + const char* header_file_name, + const std::vector< MotionCaptureMarker > &marker_set, + bool append) +{ + std::ofstream headerFile; + if(append){ + headerFile.open(header_file_name,std::ofstream::app); + }else{ + headerFile.open(header_file_name,std::ofstream::out); + } + + std::vector< std::string > names; + std::vector< unsigned int > indices; + + names.resize(marker_set.size()); + indices.resize(marker_set.size()); + + for(unsigned int i=0;i0){ + names.push_back("Last"); + appendEnumToFileStream(headerFile,"MarkerId", names, 0, "Marker_"); + indices.push_back(std::numeric_limits::max()); + appendEnumNameIndexStructToFileStream(headerFile,"MotionCaptureMarkerMap", + "MarkerId",names,"Marker_",indices); + } + headerFile.flush(); + headerFile.close(); + + return true; + +} + +//============================================================================== +RBDL_DLLAPI +bool LuaModelWriteLocalFrameHeaderEntries(const char* header_file_name, + const std::vector &local_frame_set, + bool append) +{ + std::ofstream headerFile; + if(append){ + headerFile.open(header_file_name,std::ofstream::app); + }else{ + headerFile.open(header_file_name,std::ofstream::out); + } + + std::vector< std::string > names; + std::vector< unsigned int > indices; + + names.resize(local_frame_set.size()); + indices.resize(local_frame_set.size()); + + for(unsigned int i=0;i0){ + names.push_back("Last"); + appendEnumToFileStream(headerFile,"LocalFrameId", names, 0, "LocalFrame_"); + indices.push_back(std::numeric_limits::max()); + appendEnumNameIndexStructToFileStream(headerFile,"LocalFrameMap", + "LocalFrameId",names,"LocalFrame_", + indices); + } + headerFile.flush(); + headerFile.close(); + return true; +} + +//============================================================================== +RBDL_DLLAPI +bool LuaModelWriteConstraintSetPhaseHeaderEntries(const char* header_file_name, + const std::vector< std::string > &constraint_set_names, + const std::vector< unsigned int > &constraint_phases, + bool append) +{ + std::ofstream headerFile; + if(append){ + headerFile.open(header_file_name,std::ofstream::app); + }else{ + headerFile.open(header_file_name,std::ofstream::out); + } + + std::vector< std::string > phaseNames; + phaseNames.resize(constraint_phases.size()+1); + + std::vector< std::string > enumTypeAndFieldNames; + std::vector< std::vector< std::string > > enumEntries; + std::vector< std::string > entryPrefix; + + std::vector< std::string > indexTypeAndFieldNames; + std::vector< std::vector< unsigned int > > indexMatrix; + + indexTypeAndFieldNames.resize(0); + indexMatrix.resize(0); + + enumTypeAndFieldNames.resize(3); + enumEntries.resize(constraint_phases.size()+1); + enumTypeAndFieldNames[0] = "ConstraintSetPhaseId phase_id"; + enumTypeAndFieldNames[1] = "const char* name"; + enumTypeAndFieldNames[2] = "ConstraintSetId constraint_set_id"; + + entryPrefix.resize(3); + entryPrefix[0] = ""; + entryPrefix[1] = ""; + entryPrefix[2] = "CS_"; + for(unsigned int i=0;i0){ + appendEnumToFileStream(headerFile,"ConstraintSetPhaseId", phaseNames, 0, ""); + + appendEnumStructToFileStream( headerFile, + "ConstraintSetPhaseMap", + enumTypeAndFieldNames, + enumEntries, + entryPrefix, + indexTypeAndFieldNames, + indexMatrix); + + } + headerFile.flush(); + headerFile.close(); + return true; +} +//============================================================================== +RBDL_DLLAPI +bool LuaModelWriteConstraintSetHeaderEntries(const char* header_file_name, + const std::vector< std::string > &constraint_set_names, + const std::vector &constraint_sets, + bool append) +{ + + if(constraint_set_names.size()!=constraint_sets.size()){ + ostringstream errormsg; + errormsg << "constraint_set_names (size: " + << constraint_set_names.size() + << ") and " + << "constraint_sets " + << "(size: " + << constraint_sets.size() + << ")" + <<" must have the same size but do not." + << endl; + throw Errors::RBDLError(errormsg.str()); + } + + std::ofstream headerFile; + if(append){ + headerFile.open(header_file_name, std::ofstream::app); + }else{ + headerFile.open(header_file_name, std::ofstream::out); + } + + std::vector< std::string > conSetNames; + std::vector< unsigned int > conSetIndex; + + std::vector< std::string > groupNames; + + std::vector< std::string > conNames; + std::vector< std::string > conEnumPrefix; + std::vector< std::vector< std::string > > conEnumMatrix; + std::vector< std::string > conEnumVector; + std::vector< std::string > conEnumTypeAndFieldNames; + + conEnumPrefix.push_back("C_"); + conEnumPrefix.push_back("CS_"); + conEnumPrefix.push_back("CG_"); + conEnumVector.resize(3); + conEnumTypeAndFieldNames.push_back("ConstraintId constraint_id"); + conEnumTypeAndFieldNames.push_back("ConstraintSetId constraint_set_id"); + conEnumTypeAndFieldNames.push_back("ConstraintGroupId constraint_group_id"); + + std::vector< std::vector< unsigned int > > conIndexMatrix; + std::vector< unsigned int > conIndexVector; + std::vector< std::string > conIndexTypeAndFieldNames; + conIndexVector.resize(1); + + conIndexTypeAndFieldNames.push_back("unsigned int constraint_set_item_id"); + + std::string conTypeName; + std::stringstream ss; + + + + for(unsigned int i=0;igetName()) ); + } + + + for(unsigned int j=0; jgetConstraintSize();++k){ + + ss.str(""); + ss << constraint_set_names[i]; + ss << "_" << std::string(constraint_sets[i].constraints[j]->getName()); + ss << "_" << k; + conNames.push_back(ss.str()); + conEnumVector[0] = ss.str(); + conEnumVector[1] = constraint_set_names[i]; + conEnumVector[2] = + std::string(constraint_sets[i].constraints[j]->getName()); + conEnumMatrix.push_back(conEnumVector); + conIndexVector[0] = k; + conIndexMatrix.push_back(conIndexVector); + } + + } + } + + if(constraint_sets.size()>0){ + conSetNames.push_back("Last"); + appendEnumToFileStream(headerFile,"ConstraintSetId", conSetNames, 0, "CS_"); + + conSetIndex.push_back(std::numeric_limits::max()); + appendEnumNameIndexStructToFileStream(headerFile,"ConstraintSetMap", + "ConstraintSetId", + conSetNames,"CS_",conSetIndex); + groupNames.push_back("Last"); + appendEnumToFileStream(headerFile,"ConstraintGroupId",groupNames,0,"CG_"); + + for(unsigned int k=0; k < conEnumVector.size();++k){ + conEnumVector[k] = "Last"; + } + conEnumMatrix.push_back(conEnumVector); + for(unsigned int k=0; k < conIndexVector.size();++k){ + conIndexVector[k] = std::numeric_limits::max(); + } + conIndexMatrix.push_back(conIndexVector); + + //Unique set of enums for every combination of constraint x set + conNames.push_back("Last"); + appendEnumToFileStream(headerFile,"ConstraintId", conNames, 0, "C_"); + + //Table that has for every unique constraint, its set, and set index + //This table makes it possible for the human using this model to + //access a specific constraint using a human readable name. + appendEnumStructToFileStream(headerFile,"ConstraintMap", + conEnumTypeAndFieldNames, + conEnumMatrix, conEnumPrefix, + conIndexTypeAndFieldNames, conIndexMatrix); + + } + headerFile.flush(); + headerFile.close(); + + return true; +} +//============================================================================== +#ifdef RBDL_BUILD_ADDON_MUSCLE +RBDL_DLLAPI +bool LuaModelWriteMillard2016TorqueMuscleHeaderEntries( + const char* header_file_name, + const std::vector &mtg_set, + const std::vector &mtg_set_info, + bool append) +{ + + if(mtg_set.size()!=mtg_set_info.size()){ + ostringstream errormsg; + errormsg << "mtg_set (size: " + << mtg_set.size() + << ") and " + << "mtg_set_info " + << "(size: " + << mtg_set_info.size() + << ")" + <<" must have the same size but do not." + << endl; + throw Errors::RBDLError(errormsg.str()); + } + + std::ofstream headerFile; + if(append){ + headerFile.open(header_file_name,std::ofstream::app); + }else{ + headerFile.open(header_file_name,std::ofstream::out); + } + std::vector< std::string > names; + std::vector< unsigned int > indices; + + names.resize(mtg_set_info.size()); + indices.resize(mtg_set_info.size()); + + for(unsigned int i=0;i0){ + names.push_back("Last"); + appendEnumToFileStream(headerFile,"Millard2016TorqueMuscleId", + names, 0, "MTG_"); + indices.push_back(std::numeric_limits::max()); + appendEnumNameIndexStructToFileStream( + headerFile,"Millard2016TorqueMuscleMap", + "Millard2016TorqueMuscleId", names,"MTG_",indices); + } + headerFile.flush(); + headerFile.close(); + + return true; +} + +#endif + +//============================================================================== + +} } diff --git a/addons/luamodel/luamodel.h b/addons/luamodel/luamodel.h index 8ab6fba..32ea296 100644 --- a/addons/luamodel/luamodel.h +++ b/addons/luamodel/luamodel.h @@ -4,11 +4,24 @@ #include #include #include +#include "luastructs.h" + +#ifdef RBDL_BUILD_ADDON_MUSCLE +namespace RigidBodyDynamics{ + namespace Addons{ + namespace Muscle{ + class Millard2016TorqueMuscle; + } + } +} +#endif extern "C" { struct lua_State; }; + + namespace RigidBodyDynamics { struct Model; @@ -249,7 +262,7 @@ namespace Addons { /** \brief Reads a model from a Lua file. * * \param filename the name of the Lua file. - * \param model a pointer to the output Model structure. + * \param upd_model a pointer to the output Model structure. * \param verbose specifies wether information on the model should be printed * (default: true). * @@ -261,18 +274,85 @@ namespace Addons { RBDL_DLLAPI bool LuaModelReadFromFile ( const char* filename, - Model* model, + Model* upd_model, bool verbose = false); +RBDL_DLLAPI +void LuaModelGetCoordinateNames( + const Model* model, + std::vector< std::string >& updGeneralizedPositionNames, + std::vector< std::string >& updGeneralizedVelocityNames, + std::vector< std::string >& updGeneralizedForceNames); + /** \brief Reads a model file and returns the names of all constraint sets. */ RBDL_DLLAPI std::vector LuaModelGetConstraintSetNames(const char* filename); +RBDL_DLLAPI +bool LuaModelGetConstraintSetPhases(const char* filename, + const std::vector &constraint_set_names, + std::vector< unsigned int > &constraint_set_phases); + +/** + This function will load named body-fixed motion capture markers that are + attached to each body + + @param filename: name of the lua file + @param model: reference to the (loaded) multibody model. + @param upd_marker_set: an empty std::vector of Point structure + @param verbose: information will be printed to the command window if this + is set to true. + +*/ +RBDL_DLLAPI +bool LuaModelReadMotionCaptureMarkers ( + const char* filename, + const Model* model, + std::vector &upd_marker_set, + bool verbose=false); + +/** + This function will load the point information from the lua + file into the std::vector of Point structures. + + @param filename: name of the lua file + @param model: reference to the (loaded) multibody model. + @param upd_point_set: an empty std::vector of Point structurs + @param verbose: information will be printed to the command window if this + is set to true. + +*/ +RBDL_DLLAPI +bool LuaModelReadPoints ( + const char* filename, + const Model* model, + std::vector &upd_point_set, + bool verbose=false); + +/** + This function will load the local frames from the lua + file into the std::vector of LocalFrame structures. + + @param filename: name of the lua file + @param model: reference to the (loaded) multibody model. + @param upd_local_frame_set: an empty std::vector of LocalFrame structurs + @param verbose: information will be printed to the command window if this + is set to true. + +*/ +RBDL_DLLAPI +bool LuaModelReadLocalFrames ( + const char* filename, + const Model* model, + std::vector &upd_local_frame_set, + bool verbose=false); + + /** \brief Reads a model and constraint sets from a Lua file. * * \param filename the name of the Lua file. - * \param model a pointer to the output Model structure. + * \param upd_model a pointer to the output Model structure. * \param constraint_sets reference to a std::vector of ConstraintSet structures * in which to save the information read from the file. * \param constraint_set_names reference to a std::vector of std::string @@ -289,7 +369,7 @@ std::vector LuaModelGetConstraintSetNames(const char* filename); RBDL_DLLAPI bool LuaModelReadFromFileWithConstraints ( const char* filename, - Model* model, + Model* upd_model, std::vector& constraint_sets, const std::vector& constraint_set_names, bool verbose = false); @@ -309,6 +389,191 @@ bool LuaModelReadFromLuaState ( Model* model, bool verbose = false); +/** + This function will read in the data contained in the 'HumanMetaData' + table in the lua file. + + @param filename: name of the lua file + @param human_meta_data: an empty HumanMetaData structure + @param verbose: information will be printed to the command window if this + is set to true. +*/ +RBDL_DLLAPI +bool LuaModelReadHumanMetaData( + const char* filename, + HumanMetaData &human_meta_data, + bool verbose = false); + + +#ifdef RBDL_BUILD_ADDON_MUSCLE + +/** + This function will read the information for each millard2016_torque_muscle + into the vector of Millard2016TorqueMuscleConfig structs and populate a + corresponding vector of Millard2016TorqueMuscle objects. Note that the + struct Millard2016TorqueMuscleConfig contains information needed not just + to create an Millard2016TorqueMuscle object but also the information needed + to retrieve the correct q index, qdot index, and activation index from the + state vector to evaluate the joint torque and to update the generalized + torque vector. Please see the doxygen on Millard2016TorqueMuscle.h and + Millard2016TorqueMuscleConfig for more details. + + \param filename the name of the Lua file. + \param model a pointer to the populated Model structure + \param human_meta_data: the meta data for this subject. + \param updMtgSet: an empty vector of Millard2016TorqueMuscle objects + \param updMtgSetInfo: an empty vector of Millard2016TorqueMuscleConfig structs + \param verbose specifies wether information on the model should be printed + (default: true). + + \returns true if the model and constraint sets were read successfully. + +*/ +RBDL_DLLAPI +bool LuaModelReadMillard2016TorqueMuscleSets( + const char* filename, + const RigidBodyDynamics::Model *model, + const HumanMetaData &human_meta_data, + std::vector &updMtgSet, + std::vector &updMtgSetInfo, + bool verbose = false); + +#endif + +/** + Generates a header file with human-readable enums for each of the + generalized positions, velocity, accelerations, and forces of the model. + In addition, a struct is generated so that the names of these elements can + be written to file to make post-processing more systematic. This function + is quite comprehensive and will correctly handle the extra degrees of freedom + added, for example, to the generalized position vector by a quaternion joint. + + @param header_file_name the name of the header file + @param model a reference to the populated model. + @param append false: any existing text will be discarded, + true: this text will be appended to the file +*/ +RBDL_DLLAPI +bool LuaModelWriteModelHeaderEntries(const char* header_file_name, + const RigidBodyDynamics::Model &model, + bool append); +/** + Will prepend and append header guards where the name is given by + the capitlized form of the file name. + + @param header_file_name the name of the header file +*/ +RBDL_DLLAPI +bool LuaModelAddHeaderGuards(const char* header_file_name); + +/** + Generates a header file with human-readable enums for the points. In addition + a struct is generated so that text of these names can be used to write + post-processing scripts/generate sensible messages + + @param header_file_name the name of the header file + @param point_set a reference to the populated vector of points + @param append false: any existing text will be discarded, + true: this text will be appended to the file +*/ +RBDL_DLLAPI +bool LuaModelWritePointsHeaderEntries(const char* header_file_name, + const std::vector &point_set, + bool append); + +/** + Generates a header file with human-readable enums for the markers. In addition + a struct is generated so that text of these names can be used to write + post-processing scripts/generate sensible messages + + @param header_file_name the name of the header file + @param marker_set a reference to the populated vector of + motion capture markers + @param append false: any existing text will be discarded, + true: this text will be appended to the file +*/ +RBDL_DLLAPI +bool LuaModelWriteMotionCaptureMarkerHeaderEntries( + const char* header_file_name, + const std::vector< MotionCaptureMarker > &marker_set, + bool append); + +/** + Generates a header file with human-readable enums for the local frames. + In addition a struct is generated so that text of these names can be used to + write post-processing scripts/generate sensible messages + + @param header_file_name the name of the header file + @param local_frame_set a reference to the populated vector of + local frames + @param append false: any existing text will be discarded, + true: this text will be appended to the file +*/ +RBDL_DLLAPI +bool LuaModelWriteLocalFrameHeaderEntries(const char* header_file_name, + const std::vector &local_frame_set, + bool append); + +/** + Generates a number of enums and structures so that each individual + constraint row can be selected individually, by user-defined-group id, + or constraint set. Note: this header information must be appended prior to + the constraint phase data otherwise the header will not compile. + + @param header_file_name the name of the header file + @param constraint_set_names the names of the constraint sets returned by + the function LuaModelGetConstraintSetNames + @param constraint_sets the vector of populated constraint sets + @param append false: any existing text will be discarded, + true: this text will be appended to the file +*/ +RBDL_DLLAPI +bool LuaModelWriteConstraintSetHeaderEntries(const char* header_file_name, + const std::vector< std::string > &constraint_set_names, + const std::vector &constraint_sets, + bool append); + +/** + Generates an enum and a structure so that each constraint set associated + with a constraint phase can be easily retreived, and its name can be written + to file. + + @param header_file_name the name of the header file + @param constraint_set_names the names of the constraint sets returned by + the function LuaModelGetConstraintSetNames + @param constraint_phases the ordering of the constraint set indices returned + by LuaModelGetConstraintSetPhases + @param append false: any existing text will be discarded, + true: this text will be appended to the file +*/ + +RBDL_DLLAPI +bool LuaModelWriteConstraintSetPhaseHeaderEntries(const char* header_file_name, + const std::vector< std::string > &constraint_set_names, + const std::vector< unsigned int > &constraint_phases, + bool append); + +#ifdef RBDL_BUILD_ADDON_MUSCLE +/** + Generates an enum and a structure so that each Millard2016TorqueMuscle can + be easily retreived, and its name can be written to file. + + @param header_file_name the name of the header file + @param mtg_set the vector of populated Millard2016TorqueMuscle objects + @param mtg_set_info the vector of Millard2016TorqueMuscleConfig structures + @param append false: any existing text will be discarded, + true: this text will be appended to the file +*/ +RBDL_DLLAPI +bool LuaModelWriteMillard2016TorqueMuscleHeaderEntries( + const char* header_file_name, + const std::vector &mtg_set, + const std::vector &mtg_set_info, + bool append); + +#endif /** @} */ } diff --git a/addons/luamodel/luastructs.h b/addons/luamodel/luastructs.h new file mode 100644 index 0000000..3614861 --- /dev/null +++ b/addons/luamodel/luastructs.h @@ -0,0 +1,340 @@ +//============================================================================== +/* + * RBDL - Rigid Body Dynamics Library: Addon : luamodel structs + * Copyright (c) 2020 Matthew Millard + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef LUASTRUCTS_H +#define LUASTRUCTS_H + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +/** + A struct for a named body-fixed-point. The names used in this + struct should be updated but remain as seen below for historical + reasons. In the future perhaps something clearer such as + BodyFixedPoint should be used with fields of name, body_id, + body_name, and r. + + @param name the name of the point + @param body_id the integer id of the body that this point is fixed to + @param body_name the name of the body that this point is fixed to + @param point_local the coordinates of this point relative to the body's + origin in the coordinates of the body. +*/ +struct Point { + Point() : + name ("unknown"), + body_id (std::numeric_limits::signaling_NaN()), + body_name (""), + point_local ( + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN() + ) + { } + + std::string name; + unsigned int body_id; + std::string body_name; + RigidBodyDynamics::Math::Vector3d point_local; +}; + +/** + A struct for a named motion capture marker. + + @param name the name of the marker + @param body_id the integer id of the body that this point is fixed to + @param body_name the name of the body that this point is fixed to + @param point_local the coordinates of this point relative to the body's + origin in the coordinates of the body. +*/ +struct MotionCaptureMarker { + MotionCaptureMarker() : + name ("unknown"), + body_id (std::numeric_limits::signaling_NaN()), + body_name (""), + point_local ( + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN() + ) + { } + + std::string name; + unsigned int body_id; + std::string body_name; + RigidBodyDynamics::Math::Vector3d point_local; +}; + +/** + A struct for a named body fixed frame. + + @param name the name of the point + @param body_id the integer id of the body that this local frame is fixed to + @param body_name the name of the body that this local frame is fixed to + @param r the translation from the body's origin to the origin of the + local frame, in the coordinates of the body. + @param E the rotation matrix that transforms vectors from the + coordinates of the local frame to the coordinates of the body. +*/ +struct LocalFrame { + LocalFrame() : + name ("unknown"), + body_id (std::numeric_limits::signaling_NaN()), + body_name (""), + r ( std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN()), + E ( std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN()) + { } + std::string name; + unsigned int body_id; + std::string body_name; + RigidBodyDynamics::Math::Vector3d r; + RigidBodyDynamics::Math::Matrix3d E; +}; + +/** + Generic data about a human subject. + @param gender: "male" or "female" + @param age: in years + @param age_group : "Young18To25" / "Middle55to65" / "SeniorOver65" + @param height: in meters + @param mass: in kilograms +*/ +struct HumanMetaData { + HumanMetaData() : + gender("nan"), + age(std::numeric_limits::signaling_NaN()), + age_group("nan"), + height(std::numeric_limits::signaling_NaN()), + mass(std::numeric_limits::signaling_NaN()) + { } + std::string gender; + std::string age_group; + double age; + double height; + double mass; +}; + +#ifdef RBDL_BUILD_ADDON_MUSCLE +/** + A structure that contains all of the information needed to make a joint + specific Millard2016TorqueMuscle (MTG) using the Millard2016TorqueMuscle class + and to correctly evaluate its torque and update the model's generalized force + vector. + + Many of these parameters are used to map the default sign conventions of the + Millard2016TorqueMuscle (MTG) to the specific model that it is being applied + to. These mappings currently have to be done manually by making use of the + model's topology, the default sign conventions of the Millard2016TorqueMuscles + (see the diagrams that accompany the doxygen) and the right hand rule. Please + note that while the diagrams that appear in the Millard2016TorqueMuscle are + in units of degrees this is for intuition and convenience: the curves are + used in units of radians. + + @param name : [Manditory field] + The name of the Millard2016TorqueMuscle to choose from the data_set. + This name must contain an entry that appears in the JointTorque enum that + appears in Millard2016TOrqueMuscle. Extra text can be added to these names + to give each MTG a distinct name. For example "HipExtension" is an MTG + that appears in the JointTorque enum and so "HipExtension_R" and + "HipExtension_L" are names that will pick out the HipExtention MTG from + the data_set. + + @param angle_sign : [Manditory field] + see signOfJointAngleRelativeToDoxygenFigures in Millard2016TorqueMuscle + + @param torque_sign: [Manditory field] + see signOfJointTorqueToDoxygenFigures in Millard2016TorqueMuscle + + @param body : [Manditory field] + the MTG applies a torque to the parent joint of this body. For example, + if the "thigh" segment has a parent of the "pelvis", then a hip torque + would be applied to the "thigh". + + @param joint_index : + The index of the joint (to the parent) that this MTG applies torque to. + For example if the hip flexion MTG will apply torque to the 0th joint_index + of the "thigh" segment if it is attached to the "pelvis" segment through + a ball and socket joint with the 0th axis in the flexion/extension + direction. + + @param q_scale: + Linear scaling applied to the generalized coordinate prior to evaluating + the MTG torque. Please note that while the plots of the MTGs for + + @param activation_index: + Index of this MTG's activation signal in the state vector + + @param q_index: [Internal parameter] + Index of this MTG's generalized position coordinate in the state vector. + + @param qdot_index [Internal parameter] + Index of this MTG's generalized velocity coordinate in the state vector + + @param force_index [Internal parameter] + Index of this MTG's generalized force in the state vector + + @param data_set + The data set that is used to make the curves : see the DataSet struct in + Millard2016TorqueMuscle.h for details. By default the Gymnast data set + (the most complete data set) is used. + + @param joint_angle_offset : + see jointAngleOffsetRelativeToDoxygenFigures in Millard2016TorqueMuscle + + @param activation_time_constant + the activatation time constaint of this MTG + + @param deactivation_time_constant + the deactivatation time constaint of this MTG + + @param passive_element_damping_coeff + Manually sets the damping coefficient used for the passive element + + @param passive_element_torque_scale + Manually scales the passive curve + + @param passive_element_angle_offset + Manually shifts the passive torque angle curve + + @param max_isometric_torque + An output parameter from the + TorqueMuscleFittingToolkit::fitTorqueMuscleParameters + + @param max_angular_velocity + An output parameter from the + TorqueMuscleFittingToolkit::fitTorqueMuscleParameters + + @param active_torque_angle_blending + An output parameter from the + TorqueMuscleFittingToolkit::fitTorqueMuscleParameters + + @param passive_torque_angle_blending + An output parameter from the + TorqueMuscleFittingToolkit::fitTorqueMuscleParameters + + @param torque_velocity_blending + An output parameter from the + TorqueMuscleFittingToolkit::fitTorqueMuscleParameters + + @param active_torque_angle_scale + An output parameter from the + TorqueMuscleFittingToolkit::fitTorqueMuscleParameters + + @param fit_passive_torque_scale + Pass in tuple (an q value, a passive torque, 0) and the passive curve + will be scaled so that it passes through this point exactly by calling + Millard2016TorqueMuscle::fitPassiveTorqueScale. + + Note: use fit_passive_torque_scale or fit_passive_torque_offset but not + both + + @param fit_passive_torque_offset + Pass in tuple (an q value, a passive torque, 0) and the passive curve + will be shifted so that it passes through this point exactly + Millard2016TorqueMuscle::fitPassiveCurveAngleOffset + + Note: use fit_passive_torque_scale or fit_passive_torque_offset but not + both + + +*/ +struct Millard2016TorqueMuscleConfig { + Millard2016TorqueMuscleConfig() : + name(""), + angle_sign(std::numeric_limits::signaling_NaN()), + torque_sign(std::numeric_limits::signaling_NaN()), + body(""), + joint_index(0), + q_scale(1.0), + activation_index(std::numeric_limits::signaling_NaN()), + q_index(0), + qdot_index(0), + force_index(0), + data_set("Gymnast"), + age_group(""), + gender(""), + joint_angle_offset(0.0), + activation_time_constant(0.015), //Thelen 2003, Adjustment of muscle ... + deactivation_time_constant(0.05),//Thelen 2003, Adjustment of muscle ... + max_isometric_torque(std::numeric_limits::signaling_NaN()), + max_isometric_torque_scale(std::numeric_limits::signaling_NaN()), + max_angular_velocity(std::numeric_limits::signaling_NaN()), + max_angular_velocity_scale(std::numeric_limits::signaling_NaN()), + passive_element_damping_coeff(std::numeric_limits::signaling_NaN()), + passive_element_torque_scale(std::numeric_limits::signaling_NaN()), + passive_element_angle_offset(std::numeric_limits::signaling_NaN()), + active_torque_angle_blending(std::numeric_limits::signaling_NaN()), + passive_torque_angle_blending(std::numeric_limits::signaling_NaN()), + torque_velocity_blending(std::numeric_limits::signaling_NaN()), + active_torque_angle_scale(std::numeric_limits::signaling_NaN()), + fit_passive_torque_scale(RigidBodyDynamics::Math::Vector3d( + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN())), + fit_passive_torque_offset(RigidBodyDynamics::Math::Vector3d( + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN(), + std::numeric_limits::signaling_NaN())){ + } + std::string name; + double angle_sign; + double torque_sign; + std::string body; + unsigned int joint_index; + double q_scale; + unsigned int activation_index; + unsigned int q_index; //internal parameter + unsigned int qdot_index; //internal parameter + unsigned int force_index; //internal parameter + std::string data_set; + std::string age_group; + std::string gender; + //double angle_scale; + double joint_angle_offset; + double activation_time_constant; + double deactivation_time_constant; + double max_isometric_torque; + double max_angular_velocity; + double max_isometric_torque_scale; + double max_angular_velocity_scale; + double passive_element_damping_coeff; + double passive_element_torque_scale; + double passive_element_angle_offset; + double active_torque_angle_blending; + double passive_torque_angle_blending; + double torque_velocity_blending; + double active_torque_angle_scale; + //double torque_velocity_scaling; + //double passive_torque_angle_scaling; + RigidBodyDynamics::Math::Vector3d fit_passive_torque_scale; + RigidBodyDynamics::Math::Vector3d fit_passive_torque_offset; +}; +#endif + +/* LUASTRUCTS_H */ +#endif diff --git a/addons/luamodel/luatables.cc b/addons/luamodel/luatables.cc index 752529f..2c351d5 100644 --- a/addons/luamodel/luatables.cc +++ b/addons/luamodel/luatables.cc @@ -42,891 +42,937 @@ extern "C" #include /* defines FILENAME_MAX */ #if defined(WIN32) || defined (_WIN32) - #include - #define get_current_dir _getcwd - #define DIRECTORY_SEPARATOR "\\" + #include + #define get_current_dir _getcwd + #define DIRECTORY_SEPARATOR "\\" #elif defined(linux) || defined (__linux) || defined(__linux__) || defined(__APPLE__) - #include - #define get_current_dir getcwd - #define DIRECTORY_SEPARATOR "/" + #include + #define get_current_dir getcwd + #define DIRECTORY_SEPARATOR "/" #else - #error Platform not supported! + #error Platform not supported! #endif using namespace std; -std::string get_file_directory (const char* filename) { - string name (filename); - string result = name.substr(0, name.find_last_of (DIRECTORY_SEPARATOR) + 1); - - if (result == "") - result = "./"; -#if defined (WIN32) || defined (_WIN32) - else if (result.substr(1,2) != ":\\") - result = string(".\\") + result; -#else - else if (result.substr(0,string(DIRECTORY_SEPARATOR).size()) != DIRECTORY_SEPARATOR && result[0] != '.') - result = string("./") + result; -#endif - - return result; +std::string get_file_directory (const char* filename) +{ + string name (filename); + string result = name.substr(0, name.find_last_of (DIRECTORY_SEPARATOR) + 1); + + if (result == "") { + result = "./"; + } + #if defined (WIN32) || defined (_WIN32) + else if (result.substr(1,2) != ":\\") { + result = string(".\\") + result; + } + #else + else if (result.substr(0, + string(DIRECTORY_SEPARATOR).size()) != DIRECTORY_SEPARATOR && + result[0] != '.') { + result = string("./") + result; + } + #endif + + return result; } // char encoded serialize function that is available in plaintext in // utils/serialize.lua. Converted using lua auto.lua serialize.lua const char serialize_lua[] = { - 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x69, 0x73, 0x6c, - 0x69, 0x73, 0x74, 0x20, 0x28, 0x74, 0x29, 0x0a, - 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x69, 0x74, 0x65, 0x6d, 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x3d, - 0x20, 0x30, 0x0a, - 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, - 0x20, 0x6e, 0x69, 0x6c, 0x0a, - 0x09, 0x66, 0x6f, 0x72, 0x20, 0x6b, 0x2c, 0x76, 0x20, 0x69, 0x6e, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x28, - 0x74, 0x29, 0x20, 0x64, 0x6f, 0x0a, - 0x09, 0x09, 0x69, 0x74, 0x65, 0x6d, 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x3d, 0x20, 0x69, 0x74, 0x65, 0x6d, - 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x2b, 0x20, 0x31, 0x0a, - 0x09, 0x09, 0x69, 0x66, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, 0x3d, 0x20, - 0x6e, 0x69, 0x6c, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, 0x20, 0x74, 0x79, 0x70, - 0x65, 0x28, 0x76, 0x29, 0x0a, - 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x09, 0x09, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x6c, 0x61, - 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x6f, 0x72, 0x20, 0x28, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, - 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, - 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, - 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, - 0x62, 0x6f, 0x6f, 0x6c, 0x65, 0x61, 0x6e, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, - 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x29, 0x20, 0x74, 0x68, 0x65, - 0x6e, 0x0a, - 0x09, 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, - 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x09, 0x0a, - 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, 0x20, 0x74, 0x79, 0x70, 0x65, - 0x28, 0x76, 0x29, 0x0a, - 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x09, 0x69, 0x66, 0x20, 0x69, 0x74, 0x65, 0x6d, 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x7e, 0x3d, 0x20, 0x23, - 0x74, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, - 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x74, 0x72, 0x75, 0x65, 0x0a, - 0x65, 0x6e, 0x64, 0x0a, - 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x63, 0x6d, 0x70, - 0x5f, 0x61, 0x6c, 0x70, 0x68, 0x61, 0x6e, 0x75, 0x6d, 0x65, 0x72, 0x69, 0x63, 0x20, 0x28, 0x61, 0x2c, 0x20, - 0x62, 0x29, 0x0a, - 0x09, 0x69, 0x66, 0x20, 0x28, 0x74, 0x79, 0x70, 0x65, 0x28, 0x61, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x73, - 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x62, 0x29, - 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x6f, 0x72, 0x20, 0x74, 0x79, - 0x70, 0x65, 0x28, 0x61, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, - 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x62, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, 0x75, - 0x6d, 0x62, 0x65, 0x72, 0x22, 0x29, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x61, 0x20, 0x3c, 0x20, 0x62, 0x0a, - 0x09, 0x65, 0x6c, 0x73, 0x65, 0x0a, - 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x61, 0x29, 0x20, 0x3c, - 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x62, 0x29, 0x0a, - 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x65, 0x6e, 0x64, 0x0a, - 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x67, 0x65, 0x6e, - 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x20, 0x28, - 0x74, 0x29, 0x0a, - 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, - 0x69, 0x63, 0x65, 0x73, 0x20, 0x3d, 0x20, 0x7b, 0x7d, 0x0a, - 0x09, 0x66, 0x6f, 0x72, 0x20, 0x6b, 0x20, 0x69, 0x6e, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x20, 0x28, 0x74, - 0x29, 0x20, 0x64, 0x6f, 0x0a, - 0x09, 0x09, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x2e, 0x69, 0x6e, 0x73, 0x65, 0x72, 0x74, 0x20, 0x28, 0x6f, 0x72, - 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x2c, 0x20, 0x6b, 0x29, 0x0a, - 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x09, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x2e, 0x73, 0x6f, 0x72, 0x74, 0x20, 0x28, 0x6f, 0x72, 0x64, 0x65, 0x72, - 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x2c, 0x20, 0x63, 0x6d, 0x70, 0x5f, 0x61, 0x6c, - 0x70, 0x68, 0x61, 0x6e, 0x75, 0x6d, 0x65, 0x72, 0x69, 0x63, 0x29, 0x0a, - 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, - 0x64, 0x69, 0x63, 0x65, 0x73, 0x0a, - 0x65, 0x6e, 0x64, 0x0a, - 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x6f, 0x72, 0x64, - 0x65, 0x72, 0x65, 0x64, 0x5f, 0x6e, 0x65, 0x78, 0x74, 0x20, 0x28, 0x74, 0x2c, 0x20, 0x73, 0x74, 0x61, 0x74, - 0x65, 0x29, 0x0a, - 0x09, 0x69, 0x66, 0x20, 0x73, 0x74, 0x61, 0x74, 0x65, 0x20, 0x3d, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x20, 0x74, - 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, - 0x65, 0x73, 0x20, 0x3d, 0x20, 0x67, 0x65, 0x6e, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, - 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x20, 0x28, 0x74, 0x29, 0x0a, - 0x09, 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x3d, 0x20, 0x74, 0x2e, 0x5f, 0x5f, - 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x5b, 0x31, 0x5d, 0x0a, - 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6b, 0x65, 0x79, 0x2c, 0x20, 0x74, 0x5b, 0x6b, 0x65, - 0x79, 0x5d, 0x0a, - 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x0a, - 0x09, 0x66, 0x6f, 0x72, 0x20, 0x69, 0x20, 0x3d, 0x20, 0x31, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x2e, - 0x67, 0x65, 0x74, 0x6e, 0x28, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, - 0x64, 0x69, 0x63, 0x65, 0x73, 0x29, 0x20, 0x64, 0x6f, 0x0a, - 0x09, 0x09, 0x69, 0x66, 0x20, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, - 0x64, 0x69, 0x63, 0x65, 0x73, 0x5b, 0x69, 0x5d, 0x20, 0x3d, 0x3d, 0x20, 0x73, 0x74, 0x61, 0x74, 0x65, 0x20, - 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x09, 0x6b, 0x65, 0x79, 0x20, 0x3d, 0x20, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, - 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x5b, 0x69, 0x20, 0x2b, 0x20, 0x31, 0x5d, 0x0a, - 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x09, 0x69, 0x66, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6b, 0x65, 0x79, 0x2c, 0x20, 0x74, 0x5b, 0x6b, 0x65, - 0x79, 0x5d, 0x0a, - 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x09, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, 0x65, - 0x73, 0x20, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x0a, - 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x0a, - 0x65, 0x6e, 0x64, 0x0a, - 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x6f, 0x72, 0x64, - 0x65, 0x72, 0x65, 0x64, 0x5f, 0x70, 0x61, 0x69, 0x72, 0x73, 0x20, 0x28, 0x74, 0x29, 0x0a, - 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x6e, 0x65, - 0x78, 0x74, 0x2c, 0x20, 0x74, 0x2c, 0x20, 0x6e, 0x69, 0x6c, 0x0a, - 0x65, 0x6e, 0x64, 0x0a, - 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, 0x7a, 0x65, - 0x20, 0x28, 0x6f, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x0a, - 0x20, 0x20, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x22, - 0x22, 0x0a, - 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x5f, 0x66, 0x75, 0x6e, 0x63, 0x20, - 0x3d, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x0a, - 0x09, 0x69, 0x66, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x70, 0x61, 0x69, 0x72, 0x73, 0x5f, 0x66, 0x75, 0x6e, 0x63, 0x20, 0x3d, 0x20, 0x6f, 0x72, 0x64, - 0x65, 0x72, 0x65, 0x64, 0x5f, 0x70, 0x61, 0x69, 0x72, 0x73, 0x0a, - 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x20, 0x20, 0x0a, - 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x3d, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x20, 0x74, - 0x68, 0x65, 0x6e, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x3d, 0x20, 0x22, 0x22, 0x0a, - 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, - 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, - 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, - 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6f, 0x29, 0x0a, - 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, - 0x3d, 0x20, 0x22, 0x62, 0x6f, 0x6f, 0x6c, 0x65, 0x61, 0x6e, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, - 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6f, 0x29, 0x0a, - 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, - 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, - 0x2e, 0x2e, 0x20, 0x22, 0x5c, 0x22, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x6f, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x5c, - 0x22, 0x22, 0x0a, - 0x09, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, 0x3d, - 0x20, 0x22, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x69, 0x73, 0x6c, 0x69, 0x73, - 0x74, 0x28, 0x6f, 0x29, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, - 0x2e, 0x2e, 0x20, 0x22, 0x7b, 0x22, 0x0a, - 0x09, 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, - 0x75, 0x62, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x20, 0x3d, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, - 0x09, 0x09, 0x66, 0x6f, 0x72, 0x20, 0x69, 0x2c, 0x76, 0x20, 0x69, 0x6e, 0x20, 0x69, 0x70, 0x61, 0x69, 0x72, - 0x73, 0x28, 0x6f, 0x29, 0x20, 0x64, 0x6f, 0x0a, - 0x09, 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, 0x75, 0x62, 0x74, 0x61, 0x62, - 0x6c, 0x65, 0x20, 0x3d, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, - 0x09, 0x09, 0x09, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, - 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, 0x75, 0x62, 0x74, 0x61, - 0x62, 0x6c, 0x65, 0x20, 0x3d, 0x20, 0x74, 0x72, 0x75, 0x65, 0x0a, - 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, - 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x5c, 0x6e, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, - 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, - 0x7a, 0x65, 0x20, 0x28, 0x76, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, - 0x22, 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x22, 0x0a, - 0x09, 0x09, 0x09, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, - 0x3d, 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, - 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x5c, 0x22, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x76, 0x20, 0x2e, 0x2e, - 0x20, 0x22, 0x5c, 0x22, 0x2c, 0x22, 0x0a, - 0x09, 0x09, 0x09, 0x65, 0x6c, 0x73, 0x65, 0x0a, - 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, - 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, - 0x6e, 0x67, 0x20, 0x28, 0x76, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x22, 0x0a, - 0x09, 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x09, 0x09, 0x69, 0x66, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, 0x75, 0x62, 0x74, - 0x61, 0x62, 0x6c, 0x65, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, - 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x5c, 0x6e, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x0a, - 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, - 0x2e, 0x2e, 0x20, 0x22, 0x7d, 0x22, 0x0a, - 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, - 0x3d, 0x20, 0x22, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x20, 0x20, 0x20, 0x69, 0x66, 0x20, 0x6f, 0x2e, 0x64, 0x6f, 0x6e, 0x74, 0x5f, 0x73, 0x65, 0x72, 0x69, 0x61, - 0x6c, 0x69, 0x7a, 0x65, 0x5f, 0x6d, 0x65, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x22, 0x7b, 0x7d, 0x22, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, - 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x7b, 0x5c, 0x6e, 0x22, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x6b, 0x2c, 0x76, 0x20, 0x69, 0x6e, 0x20, 0x70, 0x61, 0x69, - 0x72, 0x73, 0x5f, 0x66, 0x75, 0x6e, 0x63, 0x28, 0x6f, 0x29, 0x20, 0x64, 0x6f, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, - 0x3d, 0x20, 0x22, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x2d, 0x2d, 0x20, 0x6d, 0x61, 0x6b, 0x65, 0x20, 0x73, 0x75, - 0x72, 0x65, 0x20, 0x74, 0x68, 0x61, 0x74, 0x20, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x65, 0x64, 0x20, 0x6b, - 0x65, 0x79, 0x73, 0x20, 0x61, 0x72, 0x65, 0x20, 0x70, 0x72, 0x6f, 0x70, 0x65, 0x72, 0x6c, 0x79, 0x20, 0x61, - 0x72, 0x65, 0x20, 0x69, 0x6e, 0x64, 0x65, 0x78, 0x69, 0x66, 0x69, 0x65, 0x64, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6b, 0x29, - 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x09, 0x09, 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x3d, - 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, - 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, - 0x75, 0x6c, 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, - 0x5b, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6b, 0x29, 0x20, - 0x2e, 0x2e, 0x20, 0x22, 0x5d, 0x20, 0x3d, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, - 0x69, 0x6e, 0x67, 0x28, 0x76, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x5c, 0x6e, 0x22, 0x0a, - 0x09, 0x09, 0x09, 0x09, 0x09, 0x65, 0x6c, 0x73, 0x65, 0x0a, - 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, - 0x75, 0x6c, 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, - 0x5b, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6b, 0x29, 0x20, - 0x2e, 0x2e, 0x20, 0x22, 0x5d, 0x20, 0x3d, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, - 0x6c, 0x69, 0x7a, 0x65, 0x20, 0x28, 0x76, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, - 0x20, 0x20, 0x22, 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, - 0x5c, 0x6e, 0x22, 0x0a, - 0x09, 0x09, 0x09, 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x0a, - 0x09, 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, - 0x6c, 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, 0x22, - 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x20, 0x28, 0x6b, 0x29, 0x20, 0x2e, - 0x2e, 0x20, 0x22, 0x20, 0x3d, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, - 0x7a, 0x65, 0x28, 0x76, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, 0x22, - 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x5c, 0x6e, 0x22, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, - 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x7d, 0x22, 0x0a, - 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x0a, - 0x20, 0x20, 0x20, 0x20, 0x70, 0x72, 0x69, 0x6e, 0x74, 0x20, 0x28, 0x22, 0x6e, 0x6f, 0x74, 0x20, 0x73, 0x65, - 0x72, 0x69, 0x61, 0x6c, 0x69, 0x7a, 0x69, 0x6e, 0x67, 0x20, 0x65, 0x6e, 0x74, 0x72, 0x79, 0x20, 0x6f, 0x66, - 0x20, 0x74, 0x79, 0x70, 0x65, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, - 0x20, 0x29, 0x0a, - 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, - 0x20, 0x20, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x0a, - 0x65, 0x6e, 0x64, 0x0a, - 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, 0x7a, 0x65, 0x0a, + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x69, 0x73, 0x6c, + 0x69, 0x73, 0x74, 0x20, 0x28, 0x74, 0x29, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x69, 0x74, 0x65, 0x6d, 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x3d, + 0x20, 0x30, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, + 0x20, 0x6e, 0x69, 0x6c, 0x0a, + 0x09, 0x66, 0x6f, 0x72, 0x20, 0x6b, 0x2c, 0x76, 0x20, 0x69, 0x6e, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x28, + 0x74, 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x09, 0x09, 0x69, 0x74, 0x65, 0x6d, 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x3d, 0x20, 0x69, 0x74, 0x65, 0x6d, + 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x2b, 0x20, 0x31, 0x0a, + 0x09, 0x09, 0x69, 0x66, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, 0x3d, 0x20, + 0x6e, 0x69, 0x6c, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, 0x20, 0x74, 0x79, 0x70, + 0x65, 0x28, 0x76, 0x29, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x09, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x6c, 0x61, + 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x6f, 0x72, 0x20, 0x28, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, + 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, + 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, + 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, + 0x62, 0x6f, 0x6f, 0x6c, 0x65, 0x61, 0x6e, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, + 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x29, 0x20, 0x74, 0x68, 0x65, + 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x0a, + 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, 0x20, 0x74, 0x79, 0x70, 0x65, + 0x28, 0x76, 0x29, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x69, 0x74, 0x65, 0x6d, 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x7e, 0x3d, 0x20, 0x23, + 0x74, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x74, 0x72, 0x75, 0x65, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x63, 0x6d, 0x70, + 0x5f, 0x61, 0x6c, 0x70, 0x68, 0x61, 0x6e, 0x75, 0x6d, 0x65, 0x72, 0x69, 0x63, 0x20, 0x28, 0x61, 0x2c, 0x20, + 0x62, 0x29, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x28, 0x74, 0x79, 0x70, 0x65, 0x28, 0x61, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x73, + 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x62, 0x29, + 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x6f, 0x72, 0x20, 0x74, 0x79, + 0x70, 0x65, 0x28, 0x61, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, + 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x62, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, 0x75, + 0x6d, 0x62, 0x65, 0x72, 0x22, 0x29, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x61, 0x20, 0x3c, 0x20, 0x62, 0x0a, + 0x09, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x61, 0x29, 0x20, 0x3c, + 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x62, 0x29, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x67, 0x65, 0x6e, + 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x20, 0x28, + 0x74, 0x29, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, + 0x69, 0x63, 0x65, 0x73, 0x20, 0x3d, 0x20, 0x7b, 0x7d, 0x0a, + 0x09, 0x66, 0x6f, 0x72, 0x20, 0x6b, 0x20, 0x69, 0x6e, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x20, 0x28, 0x74, + 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x09, 0x09, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x2e, 0x69, 0x6e, 0x73, 0x65, 0x72, 0x74, 0x20, 0x28, 0x6f, 0x72, + 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x2c, 0x20, 0x6b, 0x29, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x2e, 0x73, 0x6f, 0x72, 0x74, 0x20, 0x28, 0x6f, 0x72, 0x64, 0x65, 0x72, + 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x2c, 0x20, 0x63, 0x6d, 0x70, 0x5f, 0x61, 0x6c, + 0x70, 0x68, 0x61, 0x6e, 0x75, 0x6d, 0x65, 0x72, 0x69, 0x63, 0x29, 0x0a, + 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, + 0x64, 0x69, 0x63, 0x65, 0x73, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x6f, 0x72, 0x64, + 0x65, 0x72, 0x65, 0x64, 0x5f, 0x6e, 0x65, 0x78, 0x74, 0x20, 0x28, 0x74, 0x2c, 0x20, 0x73, 0x74, 0x61, 0x74, + 0x65, 0x29, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x73, 0x74, 0x61, 0x74, 0x65, 0x20, 0x3d, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x20, 0x74, + 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, + 0x65, 0x73, 0x20, 0x3d, 0x20, 0x67, 0x65, 0x6e, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, + 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x20, 0x28, 0x74, 0x29, 0x0a, + 0x09, 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x3d, 0x20, 0x74, 0x2e, 0x5f, 0x5f, + 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x5b, 0x31, 0x5d, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6b, 0x65, 0x79, 0x2c, 0x20, 0x74, 0x5b, 0x6b, 0x65, + 0x79, 0x5d, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x0a, + 0x09, 0x66, 0x6f, 0x72, 0x20, 0x69, 0x20, 0x3d, 0x20, 0x31, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x2e, + 0x67, 0x65, 0x74, 0x6e, 0x28, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, + 0x64, 0x69, 0x63, 0x65, 0x73, 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x09, 0x09, 0x69, 0x66, 0x20, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, + 0x64, 0x69, 0x63, 0x65, 0x73, 0x5b, 0x69, 0x5d, 0x20, 0x3d, 0x3d, 0x20, 0x73, 0x74, 0x61, 0x74, 0x65, 0x20, + 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x6b, 0x65, 0x79, 0x20, 0x3d, 0x20, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, + 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x5b, 0x69, 0x20, 0x2b, 0x20, 0x31, 0x5d, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6b, 0x65, 0x79, 0x2c, 0x20, 0x74, 0x5b, 0x6b, 0x65, + 0x79, 0x5d, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, 0x65, + 0x73, 0x20, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x0a, + 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x6f, 0x72, 0x64, + 0x65, 0x72, 0x65, 0x64, 0x5f, 0x70, 0x61, 0x69, 0x72, 0x73, 0x20, 0x28, 0x74, 0x29, 0x0a, + 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x6e, 0x65, + 0x78, 0x74, 0x2c, 0x20, 0x74, 0x2c, 0x20, 0x6e, 0x69, 0x6c, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, 0x7a, 0x65, + 0x20, 0x28, 0x6f, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x0a, + 0x20, 0x20, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x22, + 0x22, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x5f, 0x66, 0x75, 0x6e, 0x63, 0x20, + 0x3d, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x70, 0x61, 0x69, 0x72, 0x73, 0x5f, 0x66, 0x75, 0x6e, 0x63, 0x20, 0x3d, 0x20, 0x6f, 0x72, 0x64, + 0x65, 0x72, 0x65, 0x64, 0x5f, 0x70, 0x61, 0x69, 0x72, 0x73, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x0a, + 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x3d, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x20, 0x74, + 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x3d, 0x20, 0x22, 0x22, 0x0a, + 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, + 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6f, 0x29, 0x0a, + 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, + 0x3d, 0x20, 0x22, 0x62, 0x6f, 0x6f, 0x6c, 0x65, 0x61, 0x6e, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6f, 0x29, 0x0a, + 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, + 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x5c, 0x22, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x6f, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x5c, + 0x22, 0x22, 0x0a, + 0x09, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, 0x3d, + 0x20, 0x22, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x69, 0x73, 0x6c, 0x69, 0x73, + 0x74, 0x28, 0x6f, 0x29, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x7b, 0x22, 0x0a, + 0x09, 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, + 0x75, 0x62, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x20, 0x3d, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x66, 0x6f, 0x72, 0x20, 0x69, 0x2c, 0x76, 0x20, 0x69, 0x6e, 0x20, 0x69, 0x70, 0x61, 0x69, 0x72, + 0x73, 0x28, 0x6f, 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x09, 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, 0x75, 0x62, 0x74, 0x61, 0x62, + 0x6c, 0x65, 0x20, 0x3d, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, + 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, 0x75, 0x62, 0x74, 0x61, + 0x62, 0x6c, 0x65, 0x20, 0x3d, 0x20, 0x74, 0x72, 0x75, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x5c, 0x6e, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, + 0x7a, 0x65, 0x20, 0x28, 0x76, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, + 0x22, 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, + 0x3d, 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x5c, 0x22, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x76, 0x20, 0x2e, 0x2e, + 0x20, 0x22, 0x5c, 0x22, 0x2c, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, + 0x6e, 0x67, 0x20, 0x28, 0x76, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x09, 0x69, 0x66, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, 0x75, 0x62, 0x74, + 0x61, 0x62, 0x6c, 0x65, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, + 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x5c, 0x6e, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x7d, 0x22, 0x0a, + 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, + 0x3d, 0x20, 0x22, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x69, 0x66, 0x20, 0x6f, 0x2e, 0x64, 0x6f, 0x6e, 0x74, 0x5f, 0x73, 0x65, 0x72, 0x69, 0x61, + 0x6c, 0x69, 0x7a, 0x65, 0x5f, 0x6d, 0x65, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x22, 0x7b, 0x7d, 0x22, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x7b, 0x5c, 0x6e, 0x22, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x6b, 0x2c, 0x76, 0x20, 0x69, 0x6e, 0x20, 0x70, 0x61, 0x69, + 0x72, 0x73, 0x5f, 0x66, 0x75, 0x6e, 0x63, 0x28, 0x6f, 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, + 0x3d, 0x20, 0x22, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x2d, 0x2d, 0x20, 0x6d, 0x61, 0x6b, 0x65, 0x20, 0x73, 0x75, + 0x72, 0x65, 0x20, 0x74, 0x68, 0x61, 0x74, 0x20, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x65, 0x64, 0x20, 0x6b, + 0x65, 0x79, 0x73, 0x20, 0x61, 0x72, 0x65, 0x20, 0x70, 0x72, 0x6f, 0x70, 0x65, 0x72, 0x6c, 0x79, 0x20, 0x61, + 0x72, 0x65, 0x20, 0x69, 0x6e, 0x64, 0x65, 0x78, 0x69, 0x66, 0x69, 0x65, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6b, 0x29, + 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x3d, + 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, + 0x75, 0x6c, 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, + 0x5b, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6b, 0x29, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x5d, 0x20, 0x3d, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, + 0x69, 0x6e, 0x67, 0x28, 0x76, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x5c, 0x6e, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, + 0x75, 0x6c, 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, + 0x5b, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6b, 0x29, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x5d, 0x20, 0x3d, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, + 0x6c, 0x69, 0x7a, 0x65, 0x20, 0x28, 0x76, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, + 0x20, 0x20, 0x22, 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, + 0x5c, 0x6e, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, + 0x6c, 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, 0x22, + 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x20, 0x28, 0x6b, 0x29, 0x20, 0x2e, + 0x2e, 0x20, 0x22, 0x20, 0x3d, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, + 0x7a, 0x65, 0x28, 0x76, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, 0x22, + 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x5c, 0x6e, 0x22, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x7d, 0x22, 0x0a, + 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x70, 0x72, 0x69, 0x6e, 0x74, 0x20, 0x28, 0x22, 0x6e, 0x6f, 0x74, 0x20, 0x73, 0x65, + 0x72, 0x69, 0x61, 0x6c, 0x69, 0x7a, 0x69, 0x6e, 0x67, 0x20, 0x65, 0x6e, 0x74, 0x72, 0x79, 0x20, 0x6f, 0x66, + 0x20, 0x74, 0x79, 0x70, 0x65, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, + 0x20, 0x29, 0x0a, + 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, 0x7a, 0x65, 0x0a, }; // // Lua Helper Functions // -void bail(lua_State *L, const char *msg){ - std::cerr << msg << lua_tostring(L, -1) << endl; - abort(); -} - -void stack_print (const char *file, int line, lua_State *L) { - cout << file << ":" << line << ": stack size: " << lua_gettop(L) << endl;; - for (int i = 1; i < lua_gettop(L) + 1; i++) { - cout << file << ":" << line << ": "; - cout << i << ": "; - if (lua_istable (L, i)) - cout << " table: " << lua_topointer (L, i) << endl; - else if (lua_isnumber (L, i)) - cout << " number: " << lua_tonumber (L, i) << endl; - else if (lua_isuserdata (L, i)) { - void* userdata = (void*) lua_touserdata (L, i); - cout << " userdata (" << userdata << ")" << endl; - } else if (lua_isstring (L, i)) - cout << " string: " << lua_tostring(L, i) << endl; - else if (lua_isfunction (L, i)) - cout << " function" << endl; - else if (lua_isnil (L, i)) - cout << " nil" << endl; - else - cout << " unknown: " << lua_typename (L, lua_type (L, i)) << endl; - } -} - -void l_push_LuaKey (lua_State *L, const LuaKey &key) { - if (key.type == LuaKey::Integer) - lua_pushnumber (L, key.int_value); - else - lua_pushstring(L, key.string_value.c_str()); -} - -bool query_key_stack (lua_State *L, std::vector key_stack) { - for (int i = key_stack.size() - 1; i >= 0; i--) { - // get the global value when the result of a lua expression was not - // pushed onto the stack via the return statement. - if (lua_gettop(L) == 0) { - lua_getglobal (L, key_stack[key_stack.size() - 1].string_value.c_str()); - - if (lua_isnil(L, -1)) { - return false; - } - - continue; - } - - l_push_LuaKey (L, key_stack[i]); - - lua_gettable (L, -2); - - // return if key is not found - if (lua_isnil(L, -1)) { - return false; - } - } - - return true; -} - -void create_key_stack (lua_State *L, std::vector key_stack) { - for (int i = key_stack.size() - 1; i > 0; i--) { - // get the global value when the result of a lua expression was not - // pushed onto the stack via the return statement. - if (lua_gettop(L) == 0) { - lua_getglobal (L, key_stack[key_stack.size() - 1].string_value.c_str()); - - if (lua_isnil(L, -1)) { - lua_pop(L, 1); - lua_newtable(L); - lua_pushvalue(L, -1); - lua_setglobal(L, key_stack[key_stack.size() - 1].string_value.c_str()); - } - - continue; - } - - l_push_LuaKey (L, key_stack[i]); - - lua_pushvalue (L, -1); - lua_gettable (L, -3); - - if (lua_isnil(L, -1)) { - // parent, key, nil - lua_pop(L, 1); // parent, key - lua_newtable(L); // parent, key, table - lua_insert(L, -2); // parent, table, key - lua_pushvalue(L, -2); // parent, table, key, table - lua_settable (L, -4); // parent, table - } - } +void bail(lua_State *L, const char *msg) +{ + std::ostringstream errormsg; + errormsg << msg << lua_tostring(L, -1) << endl; + throw RigidBodyDynamics::Errors::RBDLFileParseError(errormsg.str()); +} + +void stack_print (const char *file, int line, lua_State *L) +{ + cout << file << ":" << line << ": stack size: " << lua_gettop(L) << endl;; + for (int i = 1; i < lua_gettop(L) + 1; i++) { + cout << file << ":" << line << ": "; + cout << i << ": "; + if (lua_istable (L, i)) { + cout << " table: " << lua_topointer (L, i) << endl; + } else if (lua_isnumber (L, i)) { + cout << " number: " << lua_tonumber (L, i) << endl; + } else if (lua_isuserdata (L, i)) { + void* userdata = (void*) lua_touserdata (L, i); + cout << " userdata (" << userdata << ")" << endl; + } else if (lua_isstring (L, i)) { + cout << " string: " << lua_tostring(L, i) << endl; + } else if (lua_isfunction (L, i)) { + cout << " function" << endl; + } else if (lua_isnil (L, i)) { + cout << " nil" << endl; + } else { + cout << " unknown: " << lua_typename (L, lua_type (L, i)) << endl; + } + } +} + +void l_push_LuaKey (lua_State *L, const LuaKey &key) +{ + if (key.type == LuaKey::Integer) { + lua_pushnumber (L, key.int_value); + } else { + lua_pushstring(L, key.string_value.c_str()); + } +} + +bool query_key_stack (lua_State *L, std::vector key_stack) +{ + for (int i = key_stack.size() - 1; i >= 0; i--) { + // get the global value when the result of a lua expression was not + // pushed onto the stack via the return statement. + if (lua_gettop(L) == 0) { + lua_getglobal (L, key_stack[key_stack.size() - 1].string_value.c_str()); + + if (lua_isnil(L, -1)) { + return false; + } + + continue; + } + + l_push_LuaKey (L, key_stack[i]); + + lua_gettable (L, -2); + + // return if key is not found + if (lua_isnil(L, -1)) { + return false; + } + } + + return true; +} + +void create_key_stack (lua_State *L, std::vector key_stack) +{ + for (int i = key_stack.size() - 1; i > 0; i--) { + // get the global value when the result of a lua expression was not + // pushed onto the stack via the return statement. + if (lua_gettop(L) == 0) { + lua_getglobal (L, key_stack[key_stack.size() - 1].string_value.c_str()); + + if (lua_isnil(L, -1)) { + lua_pop(L, 1); + lua_newtable(L); + lua_pushvalue(L, -1); + lua_setglobal(L, key_stack[key_stack.size() - 1].string_value.c_str()); + } + + continue; + } + + l_push_LuaKey (L, key_stack[i]); + + lua_pushvalue (L, -1); + lua_gettable (L, -3); + + if (lua_isnil(L, -1)) { + // parent, key, nil + lua_pop(L, 1); // parent, key + lua_newtable(L); // parent, key, table + lua_insert(L, -2); // parent, table, key + lua_pushvalue(L, -2); // parent, table, key, table + lua_settable (L, -4); // parent, table + } + } } // // LuaTableNode // -std::vector LuaTableNode::getKeyStack() { - std::vector result; +std::vector LuaTableNode::getKeyStack() +{ + std::vector result; - const LuaTableNode *node_ptr = this; + const LuaTableNode *node_ptr = this; - do { - result.push_back (node_ptr->key); - node_ptr = node_ptr->parent; - } while (node_ptr != NULL); + do { + result.push_back (node_ptr->key); + node_ptr = node_ptr->parent; + } while (node_ptr != NULL); - return result; + return result; } -std::string LuaTableNode::keyStackToString() { - std::vector key_stack = getKeyStack(); +std::string LuaTableNode::keyStackToString() +{ + std::vector key_stack = getKeyStack(); - ostringstream result_stream (""); - for (int i = key_stack.size() - 1; i >= 0; i--) { - if (key_stack[i].type == LuaKey::String) - result_stream << "[\"" << key_stack[i].string_value << "\"]"; - else - result_stream << "[" << key_stack[i].int_value << "]"; - } + ostringstream result_stream (""); + for (int i = key_stack.size() - 1; i >= 0; i--) { + if (key_stack[i].type == LuaKey::String) { + result_stream << "[\"" << key_stack[i].string_value << "\"]"; + } else { + result_stream << "[" << key_stack[i].int_value << "]"; + } + } - return result_stream.str(); + return result_stream.str(); } -bool LuaTableNode::stackQueryValue() { - luaTable->pushRef(); +bool LuaTableNode::stackQueryValue() +{ + luaTable->pushRef(); - lua_State *L = luaTable->L; - stackTop = lua_gettop(L); + lua_State *L = luaTable->L; + stackTop = lua_gettop(L); - std::vector key_stack = getKeyStack(); + std::vector key_stack = getKeyStack(); - return query_key_stack (L, key_stack); + return query_key_stack (L, key_stack); } -void LuaTableNode::stackCreateValue() { - luaTable->pushRef(); +void LuaTableNode::stackCreateValue() +{ + luaTable->pushRef(); - lua_State *L = luaTable->L; - stackTop = lua_gettop(L); + lua_State *L = luaTable->L; + stackTop = lua_gettop(L); - std::vector key_stack = getKeyStack(); + std::vector key_stack = getKeyStack(); - create_key_stack (L, key_stack); + create_key_stack (L, key_stack); } -LuaTable LuaTableNode::stackQueryTable() { - luaTable->pushRef(); +LuaTable LuaTableNode::stackQueryTable() +{ + luaTable->pushRef(); - lua_State *L = luaTable->L; - stackTop = lua_gettop(L); + lua_State *L = luaTable->L; + stackTop = lua_gettop(L); - std::vector key_stack = getKeyStack(); + std::vector key_stack = getKeyStack(); - if (!query_key_stack (L, key_stack)) { - std::cerr << "Error: could not query table " << key << "." << std::endl; - abort(); - } + if (!query_key_stack (L, key_stack)) { + std::ostringstream errormsg; + errormsg << "Error: could not query table " << key << "." << std::endl; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); + } - return LuaTable::fromLuaState (L); + return LuaTable::fromLuaState (L); } -LuaTable LuaTableNode::stackCreateLuaTable() { - luaTable->pushRef(); - - lua_State *L = luaTable->L; - stackTop = lua_gettop(L); +LuaTable LuaTableNode::stackCreateLuaTable() +{ + luaTable->pushRef(); - std::vector key_stack = getKeyStack(); + lua_State *L = luaTable->L; + stackTop = lua_gettop(L); - create_key_stack (L, key_stack); + std::vector key_stack = getKeyStack(); - // create new table for the CustomType - lua_newtable(luaTable->L); // parent, CustomTable - // add table of CustomType to the parent - stackPushKey(); // parent, CustomTable, key - lua_pushvalue(luaTable->L, -2); // parent, CustomTable, key, CustomTable - lua_settable(luaTable->L, -4); + create_key_stack (L, key_stack); - LuaTable result; - result.luaStateRef = luaTable->luaStateRef->acquire(); - lua_pushvalue (result.luaStateRef->L, -1); - result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); + // create new table for the CustomType + lua_newtable(luaTable->L); // parent, CustomTable + // add table of CustomType to the parent + stackPushKey(); // parent, CustomTable, key + lua_pushvalue(luaTable->L, -2); // parent, CustomTable, key, CustomTable + lua_settable(luaTable->L, -4); - lua_pop (luaTable->luaStateRef->L, 2); + LuaTable result; + result.luaStateRef = luaTable->luaStateRef->acquire(); + lua_pushvalue (result.luaStateRef->L, -1); + result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); - return result; -} + lua_pop (luaTable->luaStateRef->L, 2); -void LuaTableNode::stackPushKey() { - l_push_LuaKey (luaTable->L, key); + return result; } -void LuaTableNode::stackRestore() { - lua_pop (luaTable->L, lua_gettop(luaTable->L) - stackTop); - - luaTable->popRef(); +void LuaTableNode::stackPushKey() +{ + l_push_LuaKey (luaTable->L, key); } -bool LuaTableNode::exists() { - bool result = true; - - if (!stackQueryValue()) - result = false; - - stackRestore(); +void LuaTableNode::stackRestore() +{ + lua_pop (luaTable->L, lua_gettop(luaTable->L) - stackTop); - return result; + luaTable->popRef(); } -void LuaTableNode::remove() { - if (stackQueryValue()) { - lua_pop(luaTable->L, 1); +bool LuaTableNode::exists() +{ + bool result = true; + + if (!stackQueryValue()) { + result = false; + } - if (lua_gettop(luaTable->L) != 0) { - l_push_LuaKey (luaTable->L, key); - lua_pushnil (luaTable->L); - lua_settable (luaTable->L, -3); - } else { - lua_pushnil (luaTable->L); - lua_setglobal (luaTable->L, key.string_value.c_str()); - } - } + stackRestore(); - stackRestore(); + return result; } -size_t LuaTableNode::length() { - size_t result = 0; - - if (stackQueryValue()) { -#if LUA_VERSION_NUM == 501 - result = lua_objlen(luaTable->L, -1); -#elif LUA_VERSION_NUM >= 502 - result = lua_rawlen(luaTable->L, -1); -#endif - } +void LuaTableNode::remove() +{ + if (stackQueryValue()) { + lua_pop(luaTable->L, 1); - stackRestore(); + if (lua_gettop(luaTable->L) != 0) { + l_push_LuaKey (luaTable->L, key); + lua_pushnil (luaTable->L); + lua_settable (luaTable->L, -3); + } else { + lua_pushnil (luaTable->L); + lua_setglobal (luaTable->L, key.string_value.c_str()); + } + } - return result; + stackRestore(); } -std::vector LuaTableNode::keys() { - std::vector result; - - if (stackQueryValue()) { - // loop over all keys - lua_pushnil(luaTable->L); - while (lua_next(luaTable->L, -2) != 0) { - if (lua_isnumber(luaTable->L, -2)) { - double number = lua_tonumber (luaTable->L, -2); - double frac; - if (modf (number, &frac) == 0) { - LuaKey key (static_cast(number)); - result.push_back (key); - } - } else if (lua_isstring (luaTable->L, -2)) { - LuaKey key (lua_tostring(luaTable->L, -2)); - result.push_back (key); - } else { - cerr << "Warning: invalid LuaKey type for key " << lua_typename(luaTable->L, lua_type(luaTable->L, -2)) << "!" << endl; - } +size_t LuaTableNode::length() +{ + size_t result = 0; - lua_pop(luaTable->L, 1); - } - } + if (stackQueryValue()) { + #if LUA_VERSION_NUM == 501 + result = lua_objlen(luaTable->L, -1); + #elif LUA_VERSION_NUM >= 502 + result = lua_rawlen(luaTable->L, -1); + #endif + } - stackRestore(); + stackRestore(); - return result; + return result; } +std::vector LuaTableNode::keys() +{ + std::vector result; + + if (stackQueryValue()) { + // loop over all keys + lua_pushnil(luaTable->L); + while (lua_next(luaTable->L, -2) != 0) { + if (lua_isnumber(luaTable->L, -2)) { + double number = lua_tonumber (luaTable->L, -2); + double frac; + if (modf (number, &frac) == 0) { + LuaKey key (static_cast(number)); + result.push_back (key); + } + } else if (lua_isstring (luaTable->L, -2)) { + LuaKey key (lua_tostring(luaTable->L, -2)); + result.push_back (key); + } else { + cerr << "Warning: invalid LuaKey type for key " << lua_typename(luaTable->L, + lua_type(luaTable->L, -2)) << "!" << endl; + } + + lua_pop(luaTable->L, 1); + } + } + + stackRestore(); + + return result; +} + + +template<> bool LuaTableNode::getDefault(const bool &default_value) +{ + bool result = default_value; -template<> bool LuaTableNode::getDefault(const bool &default_value) { - bool result = default_value; - - if (stackQueryValue()) { - result = lua_toboolean (luaTable->L, -1); - } + if (stackQueryValue()) { + result = lua_toboolean (luaTable->L, -1); + } - stackRestore(); + stackRestore(); - return result; + return result; } -template<> float LuaTableNode::getDefault(const float &default_value) { - float result = default_value; +template<> float LuaTableNode::getDefault(const float &default_value) +{ + float result = default_value; - if (stackQueryValue()) { - result = static_cast(lua_tonumber (luaTable->L, -1)); - } + if (stackQueryValue()) { + result = static_cast(lua_tonumber (luaTable->L, -1)); + } - stackRestore(); + stackRestore(); - return result; + return result; } -template<> double LuaTableNode::getDefault(const double &default_value) { - double result = default_value; +template<> double LuaTableNode::getDefault(const double &default_value) +{ + double result = default_value; - if (stackQueryValue()) { - result = lua_tonumber (luaTable->L, -1); - } + if (stackQueryValue()) { + result = lua_tonumber (luaTable->L, -1); + } - stackRestore(); + stackRestore(); - return result; + return result; } -template<> std::string LuaTableNode::getDefault(const std::string &default_value) { - std::string result = default_value; +template<> std::string LuaTableNode::getDefault +(const std::string &default_value) +{ + std::string result = default_value; - if (stackQueryValue() && lua_isstring(luaTable->L, -1)) { - result = lua_tostring (luaTable->L, -1); - } + if (stackQueryValue() && lua_isstring(luaTable->L, -1)) { + result = lua_tostring (luaTable->L, -1); + } - stackRestore(); + stackRestore(); - return result; + return result; } -template<> void LuaTableNode::set(const bool &value) { - stackCreateValue(); +template<> void LuaTableNode::set(const bool &value) +{ + stackCreateValue(); - l_push_LuaKey (luaTable->L, key); - lua_pushboolean(luaTable->L, value); - // stack: parent, key, value - lua_settable (luaTable->L, -3); + l_push_LuaKey (luaTable->L, key); + lua_pushboolean(luaTable->L, value); + // stack: parent, key, value + lua_settable (luaTable->L, -3); - stackRestore(); + stackRestore(); } -template<> void LuaTableNode::set(const float &value) { - stackCreateValue(); +template<> void LuaTableNode::set(const float &value) +{ + stackCreateValue(); - l_push_LuaKey (luaTable->L, key); - lua_pushnumber(luaTable->L, static_cast(value)); - // stack: parent, key, value - lua_settable (luaTable->L, -3); + l_push_LuaKey (luaTable->L, key); + lua_pushnumber(luaTable->L, static_cast(value)); + // stack: parent, key, value + lua_settable (luaTable->L, -3); - stackRestore(); + stackRestore(); } -template<> void LuaTableNode::set(const double &value) { - stackCreateValue(); +template<> void LuaTableNode::set(const double &value) +{ + stackCreateValue(); - l_push_LuaKey (luaTable->L, key); - lua_pushnumber(luaTable->L, value); - // stack: parent, key, value - lua_settable (luaTable->L, -3); + l_push_LuaKey (luaTable->L, key); + lua_pushnumber(luaTable->L, value); + // stack: parent, key, value + lua_settable (luaTable->L, -3); - stackRestore(); + stackRestore(); } -template<> void LuaTableNode::set(const std::string &value) { - stackCreateValue(); +template<> void LuaTableNode::set(const std::string &value) +{ + stackCreateValue(); - l_push_LuaKey (luaTable->L, key); - lua_pushstring(luaTable->L, value.c_str()); - // stack: parent, key, value - lua_settable (luaTable->L, -3); + l_push_LuaKey (luaTable->L, key); + lua_pushstring(luaTable->L, value.c_str()); + // stack: parent, key, value + lua_settable (luaTable->L, -3); - stackRestore(); + stackRestore(); } // // LuaTable // LuaTable::LuaTable (const LuaTable &other) : - filename (other.filename), - referencesGlobal (other.referencesGlobal) { - if (other.luaStateRef) { - luaStateRef = other.luaStateRef->acquire(); - - if (!referencesGlobal) { - lua_rawgeti(luaStateRef->L, LUA_REGISTRYINDEX, other.luaRef); - luaRef = luaL_ref (luaStateRef->L, LUA_REGISTRYINDEX); - } - } -} - -LuaTable& LuaTable::operator= (const LuaTable &other) { - if (&other != this) { - if (luaStateRef) { - // cleanup any existing reference - luaL_unref (luaStateRef->L, LUA_REGISTRYINDEX, luaRef); - - // if this is the last, delete the Lua state - int ref_count = luaStateRef->release(); - - if (ref_count == 0) { - if (luaStateRef->freeOnZeroRefs) { - lua_close (luaStateRef->L); - } - delete luaStateRef; - luaStateRef = NULL; - } - } - - filename = other.filename; - luaStateRef = other.luaStateRef; - referencesGlobal = other.referencesGlobal; - - if (other.luaStateRef) { - luaStateRef = other.luaStateRef->acquire(); - - if (!referencesGlobal) { - lua_rawgeti(luaStateRef->L, LUA_REGISTRYINDEX, other.luaRef); - luaRef = luaL_ref (luaStateRef->L, LUA_REGISTRYINDEX); - } - } - } - - return *this; -} - -LuaTable::~LuaTable() { - if (luaRef != -1) { - luaL_unref (luaStateRef->L, LUA_REGISTRYINDEX, luaRef); - } - - if (luaStateRef) { - int ref_count = luaStateRef->release(); - - if (ref_count == 0) { - if (luaStateRef->freeOnZeroRefs) { - lua_close (luaStateRef->L); - } - delete luaStateRef; - luaStateRef = NULL; - } - } -} - -int LuaTable::length() { - pushRef(); - - if ((lua_gettop(L) == 0) || (lua_type (L, -1) != LUA_TTABLE)) { - cerr << "Error: cannot query table length. No table on stack!" << endl; - abort(); - } - size_t result = 0; - -#if LUA_VERSION_NUM == 501 - result = lua_objlen(L, -1); -#elif LUA_VERSION_NUM >= 502 - result = lua_rawlen(L, -1); -#endif - - popRef(); + filename (other.filename), + referencesGlobal (other.referencesGlobal) +{ + if (other.luaStateRef) { + luaStateRef = other.luaStateRef->acquire(); - return result; + if (!referencesGlobal) { + lua_rawgeti(luaStateRef->L, LUA_REGISTRYINDEX, other.luaRef); + luaRef = luaL_ref (luaStateRef->L, LUA_REGISTRYINDEX); + } + } } -void LuaTable::pushRef() { - assert (luaStateRef); - assert (luaStateRef->L); +LuaTable& LuaTable::operator= (const LuaTable &other) +{ + if (&other != this) { + if (luaStateRef) { + // cleanup any existing reference + luaL_unref (luaStateRef->L, LUA_REGISTRYINDEX, luaRef); + + // if this is the last, delete the Lua state + int ref_count = luaStateRef->release(); + + if (ref_count == 0) { + if (luaStateRef->freeOnZeroRefs) { + lua_close (luaStateRef->L); + } + delete luaStateRef; + luaStateRef = NULL; + } + } + + filename = other.filename; + luaStateRef = other.luaStateRef; + referencesGlobal = other.referencesGlobal; + + if (other.luaStateRef) { + luaStateRef = other.luaStateRef->acquire(); + + if (!referencesGlobal) { + lua_rawgeti(luaStateRef->L, LUA_REGISTRYINDEX, other.luaRef); + luaRef = luaL_ref (luaStateRef->L, LUA_REGISTRYINDEX); + } + } + } - if (!referencesGlobal) { - lua_rawgeti(luaStateRef->L, LUA_REGISTRYINDEX, luaRef); - } - L = luaStateRef->L; + return *this; } -void LuaTable::popRef() { - if (!referencesGlobal) { - lua_pop (luaStateRef->L, 1); - } - L = NULL; +LuaTable::~LuaTable() +{ + if (luaRef != -1) { + luaL_unref (luaStateRef->L, LUA_REGISTRYINDEX, luaRef); + } + + if (luaStateRef) { + int ref_count = luaStateRef->release(); + + if (ref_count == 0) { + if (luaStateRef->freeOnZeroRefs) { + lua_close (luaStateRef->L); + } + delete luaStateRef; + luaStateRef = NULL; + } + } } -LuaTable LuaTable::fromFile (const char* _filename) { - LuaTable result; - - result.filename = _filename; - result.luaStateRef = new LuaStateRef(); - result.luaStateRef->L = luaL_newstate(); - result.luaStateRef->count = 1; - luaL_openlibs (result.luaStateRef->L); +int LuaTable::length() +{ + pushRef(); - // Add the directory of _filename to package.path - result.addSearchPath(get_file_directory (_filename).c_str()); + if ((lua_gettop(L) == 0) || (lua_type (L, -1) != LUA_TTABLE)) { + throw RigidBodyDynamics::Errors::RBDLError("Error: cannot query table length. No table on stack!\n"); + } + size_t result = 0; - // run the file we - if (luaL_dofile (result.luaStateRef->L, _filename)) { - bail (result.luaStateRef->L, "Error running file: "); - } + #if LUA_VERSION_NUM == 501 + result = lua_objlen(L, -1); + #elif LUA_VERSION_NUM >= 502 + result = lua_rawlen(L, -1); + #endif - result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); + popRef(); - return result; + return result; } -LuaTable LuaTable::fromLuaExpression (const char* lua_expr) { - LuaTable result; +void LuaTable::pushRef() +{ + assert (luaStateRef); + assert (luaStateRef->L); - result.luaStateRef = new LuaStateRef(); - result.luaStateRef->L = luaL_newstate(); - result.luaStateRef->count = 1; - luaL_openlibs (result.luaStateRef->L); + if (!referencesGlobal) { + lua_rawgeti(luaStateRef->L, LUA_REGISTRYINDEX, luaRef); + } + L = luaStateRef->L; +} - if (luaL_loadstring (result.luaStateRef->L, lua_expr)) { - bail (result.luaStateRef->L, "Error compiling expression!"); - } +void LuaTable::popRef() +{ + if (!referencesGlobal) { + lua_pop (luaStateRef->L, 1); + } + L = NULL; +} - if (lua_pcall (result.luaStateRef->L, 0, LUA_MULTRET, 0)) { - bail (result.luaStateRef->L, "Error running expression!"); - } +LuaTable LuaTable::fromFile (const char* _filename) +{ + LuaTable result; - if (lua_gettop(result.luaStateRef->L) != 0) { - result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); - } else { - result.referencesGlobal = true; - } + result.filename = _filename; + result.luaStateRef = new LuaStateRef(); + result.luaStateRef->L = luaL_newstate(); + result.luaStateRef->count = 1; + luaL_openlibs (result.luaStateRef->L); - return result; -} + // Add the directory of _filename to package.path + result.addSearchPath(get_file_directory (_filename).c_str()); -LuaTable LuaTable::fromLuaState (lua_State* L) { - LuaTable result; + // run the file we + if (luaL_dofile (result.luaStateRef->L, _filename)) { + bail (result.luaStateRef->L, "Error running file: "); + } - result.luaStateRef = new LuaStateRef(); - result.luaStateRef->L = L; - result.luaStateRef->count = 1; - result.luaStateRef->freeOnZeroRefs = false; + result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); - lua_pushvalue (result.luaStateRef->L, -1); - result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); - - return result; + return result; } -void LuaTable::addSearchPath(const char* path) { - if (luaStateRef->L == NULL) { - cerr << "Error: Cannot add search path: Lua state is not initialized!" << endl; - abort(); - } +LuaTable LuaTable::fromLuaExpression (const char* lua_expr) +{ + LuaTable result; + + result.luaStateRef = new LuaStateRef(); + result.luaStateRef->L = luaL_newstate(); + result.luaStateRef->count = 1; + luaL_openlibs (result.luaStateRef->L); - lua_getglobal(luaStateRef->L, "package"); - lua_getfield (luaStateRef->L, -1, "path"); - if (lua_type(luaStateRef->L, -1) != LUA_TSTRING) { - cerr << "Error: could not get package.path!" << endl; - abort(); - } + if (luaL_loadstring (result.luaStateRef->L, lua_expr)) { + bail (result.luaStateRef->L, "Error compiling expression!"); + } - string package_path = lua_tostring (luaStateRef->L, -1); - package_path = package_path + string (";") + string(path) + "?.lua;"; + if (lua_pcall (result.luaStateRef->L, 0, LUA_MULTRET, 0)) { + bail (result.luaStateRef->L, "Error running expression!"); + } - lua_pushstring(luaStateRef->L, package_path.c_str()); - lua_setfield (luaStateRef->L, -3, "path"); + if (lua_gettop(result.luaStateRef->L) != 0) { + result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); + } else { + result.referencesGlobal = true; + } - lua_pop(luaStateRef->L, 2); + return result; } -std::string LuaTable::serialize() { - pushRef(); +LuaTable LuaTable::fromLuaState (lua_State* L) +{ + LuaTable result; - std::string result; + result.luaStateRef = new LuaStateRef(); + result.luaStateRef->L = L; + result.luaStateRef->count = 1; + result.luaStateRef->freeOnZeroRefs = false; - int current_top = lua_gettop(L); - if (lua_gettop(L) != 0) { - if (luaL_loadstring(L, serialize_lua)) { - bail (L, "Error loading serialization function: "); - } + lua_pushvalue (result.luaStateRef->L, -1); + result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); + + return result; +} - if (lua_pcall(L, 0, 0, 0)) { - bail (L, "Error compiling serialization function: " ); - } +void LuaTable::addSearchPath(const char* path) +{ + if (luaStateRef->L == NULL) { + throw RigidBodyDynamics::Errors::RBDLError("Error: Cannot add search path: Lua state is not initialized!\n"); + } - lua_getglobal (L, "serialize"); - assert (lua_isfunction (L, -1)); - lua_pushvalue (L, -2); - if (lua_pcall (L, 1, 1, 0)) { - bail (L, "Error while serializing: "); - } - result = string("return ") + lua_tostring (L, -1); - } else { - cerr << "Cannot serialize global Lua state!" << endl; - abort(); - } + lua_getglobal(luaStateRef->L, "package"); + lua_getfield (luaStateRef->L, -1, "path"); + if (lua_type(luaStateRef->L, -1) != LUA_TSTRING) { + throw RigidBodyDynamics::Errors::RBDLError("Error: could not get package.path!\n"); + } - lua_pop (L, lua_gettop(L) - current_top); + string package_path = lua_tostring (luaStateRef->L, -1); + package_path = package_path + string (";") + string(path) + "?.lua;"; - popRef(); + lua_pushstring(luaStateRef->L, package_path.c_str()); + lua_setfield (luaStateRef->L, -3, "path"); - return result; + lua_pop(luaStateRef->L, 2); } -std::string LuaTable::orderedSerialize() { - pushRef(); +std::string LuaTable::serialize() +{ + pushRef(); - std::string result; + std::string result; - int current_top = lua_gettop(L); - if (lua_gettop(L) != 0) { - if (luaL_loadstring(L, serialize_lua)) { - bail (L, "Error loading serialization function: "); - } + int current_top = lua_gettop(L); + if (lua_gettop(L) != 0) { + if (luaL_loadstring(L, serialize_lua)) { + bail (L, "Error loading serialization function: "); + } - if (lua_pcall(L, 0, 0, 0)) { - bail (L, "Error compiling serialization function: " ); - } + if (lua_pcall(L, 0, 0, 0)) { + bail (L, "Error compiling serialization function: " ); + } - lua_getglobal (L, "serialize"); - assert (lua_isfunction (L, -1)); - lua_pushvalue (L, -2); - lua_pushstring (L, ""); - lua_pushboolean (L, true); - if (lua_pcall (L, 3, 1, 0)) { - bail (L, "Error while serializing: "); - } - result = string("return ") + lua_tostring (L, -1); - } else { - cerr << "Cannot serialize global Lua state!" << endl; - abort(); - } + lua_getglobal (L, "serialize"); + assert (lua_isfunction (L, -1)); + lua_pushvalue (L, -2); + if (lua_pcall (L, 1, 1, 0)) { + bail (L, "Error while serializing: "); + } + result = string("return ") + lua_tostring (L, -1); + } else { + throw RigidBodyDynamics::Errors::RBDLError("Cannot serialize global Lua state!"); + } - lua_pop (L, lua_gettop(L) - current_top); + lua_pop (L, lua_gettop(L) - current_top); - popRef(); + popRef(); - return result; + return result; +} + +std::string LuaTable::orderedSerialize() +{ + pushRef(); + + std::string result; + + int current_top = lua_gettop(L); + if (lua_gettop(L) != 0) { + if (luaL_loadstring(L, serialize_lua)) { + bail (L, "Error loading serialization function: "); + } + + if (lua_pcall(L, 0, 0, 0)) { + bail (L, "Error compiling serialization function: " ); + } + + lua_getglobal (L, "serialize"); + assert (lua_isfunction (L, -1)); + lua_pushvalue (L, -2); + lua_pushstring (L, ""); + lua_pushboolean (L, true); + if (lua_pcall (L, 3, 1, 0)) { + bail (L, "Error while serializing: "); + } + result = string("return ") + lua_tostring (L, -1); + } else { + throw RigidBodyDynamics::Errors::RBDLError("Cannot serialize global Lua state!\n"); + } + + lua_pop (L, lua_gettop(L) - current_top); + + popRef(); + + return result; } diff --git a/addons/luamodel/luatables.h b/addons/luamodel/luatables.h index 4571dd6..19c06ad 100644 --- a/addons/luamodel/luatables.h +++ b/addons/luamodel/luatables.h @@ -27,152 +27,169 @@ #define LUATABLES_H #include +#include #include #include #include #include #include +#include // Forward declaration for Lua extern "C" { -struct lua_State; + struct lua_State; } struct RBDL_DLLAPI LuaKey { - enum Type { - String, - Integer - }; - - Type type; - int int_value; - std::string string_value; - - bool operator<( const LuaKey& rhs ) const { - if (type == String && rhs.type == Integer) { - return false; - } else if (type == Integer && rhs.type == String) { - return true; - } else if (type == Integer && rhs.type == Integer) { - return int_value < rhs.int_value; - } - - return string_value < rhs.string_value; - } - - LuaKey (const char* key_value) : - type (String), - int_value (0), - string_value (key_value) { } - LuaKey (int key_value) : - type (Integer), - int_value (key_value), - string_value ("") {} + enum Type { + String, + Integer + }; + + Type type; + int int_value; + std::string string_value; + + bool operator<( const LuaKey& rhs ) const + { + if (type == String && rhs.type == Integer) { + return false; + } else if (type == Integer && rhs.type == String) { + return true; + } else if (type == Integer && rhs.type == Integer) { + return int_value < rhs.int_value; + } + + return string_value < rhs.string_value; + } + + LuaKey (const char* key_value) : + type (String), + int_value (0), + string_value (key_value) { } + LuaKey (int key_value) : + type (Integer), + int_value (key_value), + string_value ("") {} }; -inline std::ostream& operator<<(std::ostream& output, const LuaKey &key) { - if (key.type == LuaKey::Integer) - output << key.int_value << "(int)"; - else - output << key.string_value << "(string)"; - return output; +inline std::ostream& operator<<(std::ostream& output, const LuaKey &key) +{ + if (key.type == LuaKey::Integer) { + output << key.int_value << "(int)"; + } else { + output << key.string_value << "(string)"; + } + return output; } struct RBDL_DLLAPI LuaTable; struct RBDL_DLLAPI LuaTableNode; struct RBDL_DLLAPI LuaTableNode { - LuaTableNode() : - parent (NULL), - luaTable (NULL), - key("") - { } - LuaTableNode operator[](const char *child_str) { - LuaTableNode child_node; - - child_node.luaTable = luaTable; - child_node.parent = this; - child_node.key = LuaKey (child_str); - - return child_node; - } - LuaTableNode operator[](int child_index) { - LuaTableNode child_node; - - child_node.luaTable = luaTable; - child_node.parent = this; - child_node.key = LuaKey (child_index); - - return child_node; - } - bool stackQueryValue(); - void stackPushKey(); - void stackCreateValue(); - void stackRestore(); - LuaTable stackQueryTable(); - LuaTable stackCreateLuaTable(); - - std::vector getKeyStack(); - std::string keyStackToString(); - - bool exists(); - void remove(); - size_t length(); - std::vector keys(); - - // Templates for setters and getters. Can be specialized for custom - // types. - template - void set (const T &value); - template - T getDefault (const T &default_value); - - template - T get() { - if (!exists()) { - std::cerr << "Error: could not find value " << keyStackToString() << "." << std::endl; - abort(); - } - return getDefault (T()); - } - - // convenience operators (assignment, conversion, comparison) - template - void operator=(const T &value) { - set(value); - } - template - operator T() { - return get(); - } - template - bool operator==(T value) { - return value == get(); - } - template - bool operator!=(T value) { - return value != get(); - } - - LuaTableNode *parent; - LuaTable *luaTable; - LuaKey key; - int stackTop; + LuaTableNode() : + parent (NULL), + luaTable (NULL), + key("") + { } + LuaTableNode operator[](const char *child_str) + { + LuaTableNode child_node; + + child_node.luaTable = luaTable; + child_node.parent = this; + child_node.key = LuaKey (child_str); + + return child_node; + } + LuaTableNode operator[](int child_index) + { + LuaTableNode child_node; + + child_node.luaTable = luaTable; + child_node.parent = this; + child_node.key = LuaKey (child_index); + + return child_node; + } + bool stackQueryValue(); + void stackPushKey(); + void stackCreateValue(); + void stackRestore(); + LuaTable stackQueryTable(); + LuaTable stackCreateLuaTable(); + + std::vector getKeyStack(); + std::string keyStackToString(); + + bool exists(); + void remove(); + size_t length(); + std::vector keys(); + + // Templates for setters and getters. Can be specialized for custom + // types. + template + void set (const T &value); + template + T getDefault (const T &default_value); + + template + T get() + { + if (!exists()) { + std::ostringstream errormsg; + errormsg << "Error: could not find value " << keyStackToString() << "." << + std::endl; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); + } + return getDefault (T()); + } + + // convenience operators (assignment, conversion, comparison) + template + void operator=(const T &value) + { + set(value); + } + template + operator T() + { + return get(); + } + template + bool operator==(T value) + { + return value == get(); + } + template + bool operator!=(T value) + { + return value != get(); + } + + LuaTableNode *parent; + LuaTable *luaTable; + LuaKey key; + int stackTop; }; template -bool operator==(T value, LuaTableNode node) { - return value == (T) node; +bool operator==(T value, LuaTableNode node) +{ + return value == (T) node; } template -bool operator!=(T value, LuaTableNode node) { - return value != (T) node; +bool operator!=(T value, LuaTableNode node) +{ + return value != (T) node; } template<> bool LuaTableNode::getDefault(const bool &default_value); template<> double LuaTableNode::getDefault(const double &default_value); template<> float LuaTableNode::getDefault(const float &default_value); -template<> std::string LuaTableNode::getDefault(const std::string &default_value); +template<> std::string LuaTableNode::getDefault +(const std::string &default_value); template<> void LuaTableNode::set(const bool &value); template<> void LuaTableNode::set(const float &value); @@ -181,80 +198,84 @@ template<> void LuaTableNode::set(const std::string &value); /// Reference counting Lua state struct RBDL_DLLAPI LuaStateRef { - LuaStateRef () : - L (NULL), - count (0), - freeOnZeroRefs(true) - {} - - LuaStateRef* acquire() { - count = count + 1; - return this; - } - - int release() { - count = count - 1; - return count; - } - - lua_State *L; - unsigned int count; - bool freeOnZeroRefs; + LuaStateRef () : + L (NULL), + count (0), + freeOnZeroRefs(true) + {} + + LuaStateRef* acquire() + { + count = count + 1; + return this; + } + + int release() + { + count = count - 1; + return count; + } + + lua_State *L; + unsigned int count; + bool freeOnZeroRefs; }; struct RBDL_DLLAPI LuaTable { - LuaTable () : - filename (""), - luaStateRef (NULL), - luaRef(-1), - L (NULL), - referencesGlobal (false) - {} - LuaTable (const LuaTable &other); - LuaTable& operator= (const LuaTable &other); - ~LuaTable(); - - LuaTableNode operator[] (const char* key) { - LuaTableNode root_node; - root_node.key = LuaKey (key); - root_node.parent = NULL; - root_node.luaTable = this; - - return root_node; - } - LuaTableNode operator[] (int key) { - LuaTableNode root_node; - root_node.key = LuaKey (key); - root_node.parent = NULL; - root_node.luaTable = this; - - return root_node; - } - int length(); - void addSearchPath (const char* path); - std::string serialize (); - - /// Serializes the data in a predictable ordering. - std::string orderedSerialize (); - - /// Pushes the Lua table onto the stack of the internal Lua state. - // I.e. makes the Lua table active that is associated with this - // LuaTable. - void pushRef(); - /// Pops the Lua table from the stack of the internal Lua state. - // Cleans up a previous pushRef() - void popRef(); - - static LuaTable fromFile (const char *_filename); - static LuaTable fromLuaExpression (const char* lua_expr); - static LuaTable fromLuaState (lua_State *L); - - std::string filename; - LuaStateRef *luaStateRef; - int luaRef; - lua_State *L; - - bool referencesGlobal; + LuaTable () : + filename (""), + luaStateRef (NULL), + luaRef(-1), + L (NULL), + referencesGlobal (false) + {} + LuaTable (const LuaTable &other); + LuaTable& operator= (const LuaTable &other); + ~LuaTable(); + + LuaTableNode operator[] (const char* key) + { + LuaTableNode root_node; + root_node.key = LuaKey (key); + root_node.parent = NULL; + root_node.luaTable = this; + + return root_node; + } + LuaTableNode operator[] (int key) + { + LuaTableNode root_node; + root_node.key = LuaKey (key); + root_node.parent = NULL; + root_node.luaTable = this; + + return root_node; + } + int length(); + void addSearchPath (const char* path); + std::string serialize (); + + /// Serializes the data in a predictable ordering. + std::string orderedSerialize (); + + /// Pushes the Lua table onto the stack of the internal Lua state. + // I.e. makes the Lua table active that is associated with this + // LuaTable. + void pushRef(); + /// Pops the Lua table from the stack of the internal Lua state. + // Cleans up a previous pushRef() + void popRef(); + + static LuaTable fromFile (const char *_filename); + static LuaTable fromLuaExpression (const char* lua_expr); + static LuaTable fromLuaState (lua_State *L); + + std::string filename; + LuaStateRef *luaStateRef; + int luaRef; + lua_State *L; + + bool referencesGlobal; }; /* LUATABLES_H */ diff --git a/addons/luamodel/luatypes.h b/addons/luamodel/luatypes.h new file mode 100644 index 0000000..13b4260 --- /dev/null +++ b/addons/luamodel/luatypes.h @@ -0,0 +1,554 @@ +//============================================================================== +/* + * RBDL - Rigid Body Dynamics Library: Addon : luamodel types + * Copyright (c) 2013 Martin Felis . + * 2020 Matthew Millard (extension) + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef LUATYPES_H +#define LUATYPES_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "luastructs.h" + + +//============================================================================== +template<> +RigidBodyDynamics::Math::Vector3d +LuaTableNode::getDefault( + const RigidBodyDynamics::Math::Vector3d &default_value) +{ + RigidBodyDynamics::Math::Vector3d result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + if (vector_table.length() != 3) { + throw RigidBodyDynamics::Errors::RBDLFileParseError( + "LuaModel Error: invalid 3d vector!"); + } + + result[0] = vector_table[1]; + result[1] = vector_table[2]; + result[2] = vector_table[3]; + } + + stackRestore(); + + return result; +} + +//============================================================================== +template<> +RigidBodyDynamics::Math::SpatialVector +LuaTableNode::getDefault( + const RigidBodyDynamics::Math::SpatialVector &default_value +) +{ + RigidBodyDynamics::Math::SpatialVector result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + //! [Parse Failed] + if (vector_table.length() != 6) { + throw RigidBodyDynamics::Errors::RBDLFileParseError( + "LuaModel Error: invalid 6d vector!"); + } + //! [Parse Failed] + + result[0] = vector_table[1]; + result[1] = vector_table[2]; + result[2] = vector_table[3]; + result[3] = vector_table[4]; + result[4] = vector_table[5]; + result[5] = vector_table[6]; + } + + stackRestore(); + + return result; +} + +//============================================================================== +template<> +RigidBodyDynamics::Math::MatrixNd +LuaTableNode::getDefault( + const RigidBodyDynamics::Math::MatrixNd &default_value) +{ + RigidBodyDynamics::Math::MatrixNd result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + result.resize( int(vector_table.length()), + int(vector_table[1].length())); + + for(int r=0; r +RigidBodyDynamics::Math::Matrix3d +LuaTableNode::getDefault( + const RigidBodyDynamics::Math::Matrix3d &default_value) +{ + RigidBodyDynamics::Math::Matrix3d result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + if (vector_table.length() != 3) { + throw RigidBodyDynamics::Errors::RBDLFileParseError( + "LuaModel Error: invalid 3d matrix!"); + } + + if (vector_table[1].length() != 3 + || vector_table[2].length() != 3 + || vector_table[3].length() != 3) { + throw RigidBodyDynamics::Errors::RBDLFileParseError( + "LuaModel Error: invalid 3d matrix!"); + } + + result(0,0) = vector_table[1][1]; + result(0,1) = vector_table[1][2]; + result(0,2) = vector_table[1][3]; + + result(1,0) = vector_table[2][1]; + result(1,1) = vector_table[2][2]; + result(1,2) = vector_table[2][3]; + + result(2,0) = vector_table[3][1]; + result(2,1) = vector_table[3][2]; + result(2,2) = vector_table[3][3]; + } + + stackRestore(); + + return result; +} +//============================================================================== +template<> +RigidBodyDynamics::Math::SpatialTransform +LuaTableNode::getDefault( + const RigidBodyDynamics::Math::SpatialTransform &default_value +) +{ + RigidBodyDynamics::Math::SpatialTransform result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + result.r = vector_table["r"].getDefault( + RigidBodyDynamics::Math::Vector3d::Zero()); + result.E = vector_table["E"].getDefault( + RigidBodyDynamics::Math::Matrix3d::Identity ()); + } + + stackRestore(); + + return result; +} + +//============================================================================== +template<> +RigidBodyDynamics::Joint +LuaTableNode::getDefault( + const RigidBodyDynamics::Joint &default_value) +{ + RigidBodyDynamics::Joint result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + int joint_dofs = vector_table.length(); + + if (joint_dofs == 1) { + std::string dof_string = vector_table[1].getDefault(""); + if (dof_string == "JointTypeSpherical") { + stackRestore(); + return RigidBodyDynamics::Joint( + RigidBodyDynamics::JointTypeSpherical); + } else if (dof_string == "JointTypeEulerZYX") { + stackRestore(); + return RigidBodyDynamics::Joint( + RigidBodyDynamics::JointTypeEulerZYX); + } + if (dof_string == "JointTypeEulerXYZ") { + stackRestore(); + return RigidBodyDynamics::Joint( + RigidBodyDynamics::JointTypeEulerXYZ); + } + if (dof_string == "JointTypeEulerYXZ") { + stackRestore(); + return RigidBodyDynamics::Joint( + RigidBodyDynamics::JointTypeEulerYXZ); + } + if (dof_string == "JointTypeTranslationXYZ") { + stackRestore(); + return RigidBodyDynamics::Joint( + RigidBodyDynamics::JointTypeTranslationXYZ); + } + if (dof_string == "JointTypeFloatingBase") { + stackRestore(); + return RigidBodyDynamics::Joint( + RigidBodyDynamics::JointTypeFloatingBase); + } + } + + if (joint_dofs > 0) { + if (vector_table[1].length() != 6) { + std::ostringstream errormsg; + errormsg << "LuaModel Error: invalid joint motion " + << "subspace description at " + << this->keyStackToString() << std::endl; + throw RigidBodyDynamics::Errors::RBDLFileParseError(errormsg.str()); + } + } + switch (joint_dofs) { + case 0: + result = RigidBodyDynamics::Joint( + RigidBodyDynamics::JointTypeFixed); + break; + case 1: + result = RigidBodyDynamics::Joint ( + vector_table[1].get()); + break; + case 2: + result = RigidBodyDynamics::Joint( + vector_table[1].get(), + vector_table[2].get() + ); + break; + case 3: + result = RigidBodyDynamics::Joint( + vector_table[1].get(), + vector_table[2].get(), + vector_table[3].get() + ); + break; + case 4: + result = RigidBodyDynamics::Joint( + vector_table[1].get(), + vector_table[2].get(), + vector_table[3].get(), + vector_table[4].get() + ); + break; + case 5: + result = RigidBodyDynamics::Joint( + vector_table[1].get(), + vector_table[2].get(), + vector_table[3].get(), + vector_table[4].get(), + vector_table[5].get() + ); + break; + case 6: + result = RigidBodyDynamics::Joint( + vector_table[1].get(), + vector_table[2].get(), + vector_table[3].get(), + vector_table[4].get(), + vector_table[5].get(), + vector_table[6].get() + ); + break; + default: + throw RigidBodyDynamics::Errors::RBDLFileParseError( + "Invalid number of DOFs for joint."); + break; + } + } + + stackRestore(); + + return result; +} + + +//============================================================================== +template<> +RigidBodyDynamics::Body +LuaTableNode::getDefault( + const RigidBodyDynamics::Body &default_value) +{ + RigidBodyDynamics::Body result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + double mass = 0.; + RigidBodyDynamics::Math::Vector3d com( + RigidBodyDynamics::Math::Vector3d::Zero()); + RigidBodyDynamics::Math::Matrix3d inertia( + RigidBodyDynamics::Math::Matrix3d::Identity()); + + mass = vector_table["mass"]; + com = vector_table["com"] + .getDefault(com); + inertia = vector_table["inertia"] + .getDefault(inertia); + + result = RigidBodyDynamics::Body (mass, com, inertia); + } + + stackRestore(); + + return result; +} + +//============================================================================== + +template<> Point +LuaTableNode::getDefault( + const Point &default_value) +{ + Point result = default_value; + + if (stackQueryValue()) { + LuaTable point_table = LuaTable::fromLuaState (luaTable->L); + + result.name = point_table["name"].get(); + result.point_local = point_table["point"] + .getDefault( + RigidBodyDynamics::Math::Vector3d::Zero()); + result.body_name = point_table["body"].get(); + } + + stackRestore(); + + return result; +} + +//============================================================================== + +template<> MotionCaptureMarker +LuaTableNode::getDefault( + const MotionCaptureMarker &default_value) +{ + MotionCaptureMarker result = default_value; + + if (stackQueryValue()) { + LuaTable marker_table = LuaTable::fromLuaState (luaTable->L); + + result.name = marker_table["name"].get(); + result.point_local = marker_table["point"] + .getDefault( + RigidBodyDynamics::Math::Vector3d::Zero()); + result.body_name = marker_table["body"].get(); + } + + stackRestore(); + + return result; +} + +//============================================================================== + +template<> LocalFrame +LuaTableNode::getDefault( + const LocalFrame &default_value) +{ + LocalFrame result = default_value; + + if (stackQueryValue()) { + LuaTable local_frame_table = LuaTable::fromLuaState (luaTable->L); + + result.name = local_frame_table["name"].get(); + result.body_name= local_frame_table["body"].get(); + + result.r = local_frame_table["r"] + .getDefault( + RigidBodyDynamics::Math::Vector3d::Zero()); + result.E = local_frame_table["E"] + .getDefault( + RigidBodyDynamics::Math::Matrix3d::Identity()); + } + + stackRestore(); + + return result; +} +//============================================================================== + +template<> HumanMetaData + LuaTableNode::getDefault( + const HumanMetaData &default_value) +{ + HumanMetaData result = default_value; + + if (stackQueryValue()) { + LuaTable metadata_table = LuaTable::fromLuaState (luaTable->L); + + result.gender = + metadata_table["gender"].get(); + + result.age = metadata_table["age"]; + result.height = metadata_table["height"]; + result.mass = metadata_table["weight"]; + result.age_group = metadata_table["age_group"].get(); + } + + stackRestore(); + return result; +} +//============================================================================== +#ifdef RBDL_BUILD_ADDON_MUSCLE +template<> Millard2016TorqueMuscleConfig + LuaTableNode::getDefault( + const Millard2016TorqueMuscleConfig &default_value) +{ + Millard2016TorqueMuscleConfig result = default_value; + + if (stackQueryValue()) { + LuaTable mtg_table = LuaTable::fromLuaState (luaTable->L); + + // First read mandatory fields + result.name = mtg_table["name"].get(); + result.angle_sign = mtg_table["angle_sign"].get(); + result.torque_sign = mtg_table["torque_sign"].get(); + result.body = mtg_table["body"].get(); + + if(mtg_table["joint_index"].exists()){ + result.joint_index = + unsigned(int(mtg_table["joint_index"].get())); + } + + //Optional parameters + if(mtg_table["data_set"].exists()){ + result.data_set = mtg_table["data_set"].get(); + } + if(mtg_table["age_group"].exists()){ + result.age_group = mtg_table["age_group"].get(); + } + if(mtg_table["gender"].exists()){ + result.gender = mtg_table["gender"].get(); + } + + if (mtg_table["q_scale"].exists()) { + result.q_scale = mtg_table["q_scale"].get(); + } + if (mtg_table["activation_index"].exists()) { + result.activation_index = + unsigned(int(mtg_table["activation_index"].get())); + } + if (mtg_table["data_set"].exists()) { + result.data_set = mtg_table["data_set"].get(); + } + //if (mtg_table["angle_scale"].exists()) { + // result.angle_scale = mtg_table["angle_scale"].get(); + //} + if (mtg_table["joint_angle_offset"].exists()) { + result.joint_angle_offset = + mtg_table["joint_angle_offset"].get(); + } + if (mtg_table["act_time"].exists()) { + result.activation_time_constant = + mtg_table["act_time"].get(); + } + if (mtg_table["deact_time"].exists()) { + result.deactivation_time_constant = + mtg_table["deact_time"].get(); + } + + //Parameters for manual tuning + if (mtg_table["max_isometric_torque_scale"].exists()) { + result.max_isometric_torque_scale = + mtg_table["max_isometric_torque_scale"].get(); + } + if (mtg_table["max_angular_velocity_scale"].exists()) { + result.max_angular_velocity_scale = + mtg_table["max_angular_velocity_scale"].get(); + } + + if (mtg_table["passive_element_torque_scale"].exists()) { + result.passive_element_torque_scale = + mtg_table["passive_element_torque_scale"].get(); + } + if (mtg_table["passive_element_angle_offset"].exists()) { + result.passive_element_angle_offset = + mtg_table["passive_element_angle_offset"].get(); + } + if (mtg_table["passive_element_damping_coeff"].exists()) { + result.passive_element_damping_coeff = + mtg_table["passive_element_damping_coeff"].get(); + } + + //Optional passive-curve fitting coordinates + if (mtg_table["fit_passive_torque_scale"].exists()) { + result.fit_passive_torque_scale = + mtg_table["fit_passive_torque_scale"] + .get(); + } + if (mtg_table["fit_passive_torque_offset"].exists()) { + result.fit_passive_torque_offset = + mtg_table["fit_passive_torque_offset"] + .get(); + } + + //Optional fitting parameters from the fitting routine + if (mtg_table["max_isometric_torque"].exists()) { + result.max_isometric_torque = + mtg_table["max_isometric_torque"].get(); + } + if (mtg_table["max_angular_velocity"].exists()) { + result.max_angular_velocity = + mtg_table["max_angular_velocity"].get(); + } + if (mtg_table["active_torque_angle_blending"].exists()) { + result.active_torque_angle_blending = + mtg_table["active_torque_angle_blending"].get(); + } + if (mtg_table["passive_torque_angle_blending"].exists()) { + result.passive_torque_angle_blending = + mtg_table["passive_torque_angle_blending"].get(); + } + if (mtg_table["torque_velocity_blending"].exists()) { + result.torque_velocity_blending = + mtg_table["torque_velocity_blending"].get(); + } + if (mtg_table["active_torque_angle_scale"].exists()) { + result.active_torque_angle_scale = + mtg_table["active_torque_angle_scale"].get(); + } + //if (mtg_table["passive_torque_angle_scale"].exists()) { + // result.passive_torque_angle_scale = + // mtg_table["passive_torque_angle_scale"].get(); + //} + //if (mtg_table["torque_velocity_scaling"].exists()) { + // result.torque_velocity_scaling = + // mtg_table["torque_velocity_scaling"].get(); + //} + + } + stackRestore(); + return result; +} +#endif + +/* LUASTRUCTS_H */ +#endif diff --git a/addons/luamodel/sampleconstrainedmodel.lua b/addons/luamodel/sampleconstrainedmodel.lua index b93186e..1a7ab24 100644 --- a/addons/luamodel/sampleconstrainedmodel.lua +++ b/addons/luamodel/sampleconstrainedmodel.lua @@ -78,6 +78,12 @@ meshes = { }, } +eye33 = { + {1.,0.,0.,}, + {0.,1.,0.,}, + {0.,0.,1.,}, +} + model = { gravity = {0, 0, 0}, @@ -133,37 +139,49 @@ model = { }, + --Named body fixed points. The names of the fields have been set for + --historical reasons. It would make more sense if this was named instead + -- local_points, with a field of 'r' rather than 'point'. + points = { + {name = 'base_origin', body='base', point={0.,0.,0.},}, + }, + + --Named local frames + local_frames = { + {name = 'l12_A', body='l12', r={l2,0.,0.}, E=eye33,}, + {name = 'l22_B', body='l22', r={0.,0.,0.}, E=eye33,}, + }, + constraint_set_phases = { + "individual_constraints", + "constraints_in_sets", + "constraints_in_sets", + "individual_constraints", + }, constraint_sets = { - loop_constraints = { - { - constraint_type = 'loop', - predecessor_body = 'l12', - successor_body = 'l22', - predecessor_transform = { - E = { - {1, 0, 0}, - {0, 1, 0}, - {0, 0, 1}, - }, - r = {l2, 0, 0}, - }, - successor_transform = { - E = { - {1, 0, 0}, - {0, 1, 0}, - {0, 0, 1}, - }, - r = {0, 0, 0}, - }, - axis = {0, 0, 0, 1, 0, 0}, + individual_constraints = { + { -- Contact Constraint: verbose syntax + -- Note: Contact Constraints do not have the 'enable_stablization' flag + -- exposed (it is currently ignored if present) because this + -- type of constraint is very well numerically behaved and rarely + -- suffers from drift. + constraint_type = 'contact', name = 'contactBaseX', id = 2, + body = 'base', point = {0, 0, 0}, normal = {1., 0., 0.}, + }, + { -- Contact Constraint: contact syntax - makes use of named points + -- and passes a set of normals + constraint_type = 'contact', name = 'contactBaseYZ', id = 3, + point_name = 'base_origin', normal_sets = {{0.,1.,0.,},{0.,0.,1.}}, + }, + { -- Loop Constraint: compact syntax - makes use of named local_frames + constraint_type = 'loop', name = 'loopL12L22Tx', id = 1, + predecessor_local_frame = 'l12_A', + successor_local_frame = 'l22_B', + axis = {0., 0., 0., 1., 0., 0.}, stabilization_coefficient = 0.1, - name = 'linkTX', }, - - { - constraint_type = 'loop', + { -- Loop Constraint: verbose syntax. + constraint_type = 'loop', name = 'loopL12L22Ty', id = 2, predecessor_body = 'l12', - successor_body = 'l22', predecessor_transform = { E = { {1, 0, 0}, @@ -172,56 +190,7 @@ model = { }, r = {l2, 0, 0}, }, - successor_transform = { - E = { - {1, 0, 0}, - {0, 1, 0}, - {0, 0, 1}, - }, - r = {0, 0, 0}, - }, - axis = {0, 0, 0, 0, 1, 0}, - stabilization_coefficient = 0.1, - name = 'linkTY', - }, - }, - - all_constraints = { - { - constraint_type = 'contact', - body = 'base', - point = {0, 0, 0}, - normal = {1, 0, 0}, - name = 'baseTX', - normal_acceleration = 0, - }, - - { - constraint_type = 'contact', - body = 'base', - normal = {0, 1, 0}, - name = 'baseTY', - }, - - { - constraint_type = 'contact', - body = 'base', - normal = {0, 0, 1}, - name = 'baseTZ', - }, - - { - constraint_type = 'loop', - predecessor_body = 'l12', successor_body = 'l22', - predecessor_transform = { - E = { - {1, 0, 0}, - {0, 1, 0}, - {0, 0, 1}, - }, - r = {l2, 0, 0}, - }, successor_transform = { E = { {1, 0, 0}, @@ -230,37 +199,32 @@ model = { }, r = {0, 0, 0}, }, - axis = {0, 0, 0, 1, 0, 0}, + axis = {0., 0., 0., 0., 1., 0.}, stabilization_coefficient = 0.1, - name = 'linkTX', + enable_stabilization = true, + }, + }, + constraints_in_sets = { + { -- Contact Constraint: compact syntax - use of named points and + -- normal sets + constraint_type = 'contact',name = 'contactBaseXYZ',id = 2, + point_name = 'base_origin', + normal_sets = {{1., 0., 0.}, + {0., 1., 0.}, + {0., 0., 1.},}, }, - - { - constraint_type = 'loop', - predecessor_body = 'l12', - successor_body = 'l22', - predecessor_transform = { - E = { - {1, 0, 0}, - {0, 1, 0}, - {0, 0, 1}, - }, - r = {l2, 0, 0}, - }, - successor_transform = { - E = { - {1, 0, 0}, - {0, 1, 0}, - {0, 0, 1}, - }, - r = {0, 0, 0}, - }, - axis = {0, 0, 0, 0, 1, 0}, - stabilization_coefficient = 0.1, - name = 'linkTY', + { -- Loop Constraint: compact syntax - use of named local_frames + -- and normal sets + constraint_type = 'loop', name = 'loopL12L22TxTy', id = 1, + predecessor_local_frame = 'l12_A', + successor_local_frame = 'l22_B', + axis_sets = {{0., 0., 0., 1., 0., 0.}, + {0., 0., 0., 0., 1., 0.},}, + stabilization_coefficient = 0.1, + enable_stabilization = false, }, }, - }, + }, } diff --git a/addons/luamodel/samplemodel.lua b/addons/luamodel/samplemodel.lua index d2d501a..fdf1314 100644 --- a/addons/luamodel/samplemodel.lua +++ b/addons/luamodel/samplemodel.lua @@ -1,7 +1,7 @@ inertia = { {1.1, 0.1, 0.2}, {0.3, 1.2, 0.4}, - {0.5, 0.6, 1.3} + {0.5, 0.6, 1.3}, } pelvis = { mass = 9.3, com = { 1.1, 1.2, 1.3}, inertia = inertia } @@ -13,8 +13,10 @@ bodies = { pelvis = pelvis, thigh_right = thigh, shank_right = shank, + foot_right = foot, thigh_left = thigh, - shank_left = shank + shank_left = shank, + foot_left = foot, } joints = { @@ -24,64 +26,89 @@ joints = { { 0., 0., 0., 0., 0., 1.}, { 0., 0., 1., 0., 0., 0.}, { 0., 1., 0., 0., 0., 0.}, - { 1., 0., 0., 0., 0., 0.} + { 1., 0., 0., 0., 0., 0.}, }, - spherical_zyx = { + euler_zyx = { { 0., 0., 1., 0., 0., 0.}, { 0., 1., 0., 0., 0., 0.}, - { 1., 0., 0., 0., 0., 0.} + { 1., 0., 0., 0., 0., 0.}, }, rotational_y = { - { 0., 1., 0., 0., 0., 0.} + { 0., 1., 0., 0., 0., 0.}, }, - fixed = {} + fixed = {}, } -model = { - frames = { - { - name = "pelvis", - parent = "ROOT", - body = bodies.pelvis, - joint = joints.freeflyer, - }, - { - name = "thigh_right", - parent = "pelvis", - body = bodies.thigh_right, - joint = joints.spherical_zyx, - }, - { - name = "shank_right", - parent = "thigh_right", - body = bodies.thigh_right, - joint = joints.rotational_y - }, - { - name = "foot_right", - parent = "shank_right", - body = bodies.thigh_right, - joint = joints.fixed - }, - { - name = "thigh_left", - parent = "pelvis", - body = bodies.thigh_left, - joint = joints.spherical_zyx - }, - { - name = "shank_left", - parent = "thigh_left", - body = bodies.thigh_left, - joint = joints.rotational_y - }, - { - name = "foot_left", - parent = "shank_left", - body = bodies.thigh_left, - joint = joints.fixed - }, - } -} +eye33 = {{1.0,0.0,0.,}, + {0.0,1.0,0.,}, + {0.0,0.0,1.,},} + -return model +return { + --Named body fixed points. The names of the fields have been set for + --historical reasons. It would make more sense if this was named instead + -- local_points, with a field of 'r' rather than 'point'. +points = { + {name = "Heel_Medial_L", body = "foot_right", point = {-0.080000, -0.042000, -0.091000,},}, + {name = "Heel_Lateral_L", body = "foot_right", point = {-0.080000, 0.042000, -0.091000,},}, + {name = "ForeFoot_Medial_L", body = "foot_right", point = {0.181788, -0.054000, -0.091000,},}, + {name = "ForeFoot_Lateral_L", body = "foot_right", point = {0.181788, 0.054000, -0.091000,},}, +}, +--Named local frames +local_frames = { + {name = "Pocket_L", body="thigh_left", r={0., 0.2, 0.}, E=eye33, }, + {name = "Pocket_R", body="thigh_right", r={0., -0.2, 0.}, E=eye33, }, +}, +frames = { + { + name = "pelvis", + parent = "ROOT", + body = bodies.pelvis, + joint = joints.freeflyer, + markers = { + LASI = { 0.047794, 0.200000, 0.070908,}, + RASI = { 0.047794, -0.200000, 0.070908,}, + LPSI = { -0.106106, 0.200000, 0.070908,}, + RPSI = { -0.106106, -0.200000, 0.070908,},}, + }, + { + name = "thigh_right", + parent = "pelvis", + body = bodies.thigh_right, + joint = joints.euler_zyx, + markers = { + RTHI = { -0.007376, -0.000000, -0.243721,}, + RKNE = { -0.011611, -0.000000, -0.454494,},}, + }, + { + name = "shank_right", + parent = "thigh_right", + body = bodies.thigh_right, + joint = joints.rotational_y, + }, + { + name = "foot_right", + parent = "shank_right", + body = bodies.foot_right, + joint = joints.fixed, + }, + { + name = "thigh_left", + parent = "pelvis", + body = bodies.thigh_left, + joint = joints.euler_zyx, + }, + { + name = "shank_left", + parent = "thigh_left", + body = bodies.shank_left, + joint = joints.rotational_y, + }, + { + name = "foot_left", + parent = "shank_left", + body = bodies.foot_left, + joint = joints.fixed, + }, +}, +} \ No newline at end of file diff --git a/addons/luamodel/samplemodelwithtorquemuscles.lua b/addons/luamodel/samplemodelwithtorquemuscles.lua new file mode 100644 index 0000000..ac48089 --- /dev/null +++ b/addons/luamodel/samplemodelwithtorquemuscles.lua @@ -0,0 +1,139 @@ +inertia = { + {1.1, 0.1, 0.2}, + {0.3, 1.2, 0.4}, + {0.5, 0.6, 1.3}, +} + +pelvis = { mass = 9.3, com = { 1.1, 1.2, 1.3}, inertia = inertia } +thigh = { mass = 4.2, com = { 1.1, 1.2, 1.3}, inertia = inertia } +shank = { mass = 4.1, com = { 1.1, 1.2, 1.3}, inertia = inertia } +foot = { mass = 1.1, com = { 1.1, 1.2, 1.3}, inertia = inertia } + +bodies = { + pelvis = pelvis, + thigh_right = thigh, + shank_right = shank, + foot_right = foot, + thigh_left = thigh, + shank_left = shank, + foot_left = foot, +} + +joints = { + freeflyer = { + { 0., 0., 0., 1., 0., 0.}, + { 0., 0., 0., 0., 1., 0.}, + { 0., 0., 0., 0., 0., 1.}, + { 0., 0., 1., 0., 0., 0.}, + { 0., 1., 0., 0., 0., 0.}, + { 1., 0., 0., 0., 0., 0.}, + }, + euler_zyx = { + { 0., 0., 1., 0., 0., 0.}, + { 0., 1., 0., 0., 0., 0.}, + { 1., 0., 0., 0., 0., 0.}, + }, + rotational_y = { + { 0., 1., 0., 0., 0., 0.}, + }, + fixed = {}, +} + +eye33 = {{1.0,0.0,0.,}, + {0.0,1.0,0.,}, + {0.0,0.0,1.,},} + + +return { + --Named body fixed points. The names of the fields have been set for + --historical reasons. It would make more sense if this was named instead + -- local_points, with a field of 'r' rather than 'point'. +points = { + {name = "Heel_Medial_L", body = "foot_right", point = {-0.080000, -0.042000, -0.091000,},}, + {name = "Heel_Lateral_L", body = "foot_right", point = {-0.080000, 0.042000, -0.091000,},}, + {name = "ForeFoot_Medial_L", body = "foot_right", point = {0.181788, -0.054000, -0.091000,},}, + {name = "ForeFoot_Lateral_L", body = "foot_right", point = {0.181788, 0.054000, -0.091000,},}, +}, +--Named local frames +local_frames = { + {name = "Pocket_L", body="thigh_left", r={0., 0.2, 0.}, E=eye33, }, + {name = "Pocket_R", body="thigh_right", r={0., -0.2, 0.}, E=eye33, }, +}, +--subject data +human_meta_data = { + {age_group = "Young18To25", + age = 35.0, + height = 1.73, + weight = 81.68, + gender = "male"}, +}, +--torque muscle models +--The name must contain a name of one of the MTGs listed in JointTorqueSet : line 60 of addons/muscle/Millard2016TorqueMuscle.cc +millard2016_torque_muscles = { + { name = "HipExtension_R", angle_sign = -1, torque_sign = 1, body ="thigh_right", joint_index=1, act_time = 0.05, deact_time = 0.05,}, + { name = "HipFlexion_R", angle_sign = -1, torque_sign = -1, body ="thigh_right", joint_index=1, act_time = 0.05, deact_time = 0.05,}, + { name = "KneeExtension_R", angle_sign = 1, torque_sign = -1, body ="shank_right", act_time = 0.05, deact_time = 0.05,}, + { name = "KneeFlexion_R", angle_sign = 1, torque_sign = 1, body ="shank_right", act_time = 0.05, deact_time = 0.05,}, + { name = "AnkleExtension_R", angle_sign = -1, torque_sign = 1, body ="foot_right" , act_time = 0.05, deact_time = 0.05,}, + { name = "AnkleFlexion_R", angle_sign = -1, torque_sign = -1, body ="foot_right" , act_time = 0.05, deact_time = 0.05,}, + { name = "AnkleFlexion_R_FpeHalfScale", angle_sign = -1, torque_sign = -1, body ="foot_right" , act_time = 0.05, deact_time = 0.05, passive_element_torque_scale = 0.5,}, + { name = "AnkleFlexion_R_FisoHalfScale", angle_sign = -1, torque_sign = -1, body ="foot_right" , act_time = 0.05, deact_time = 0.05, max_isometric_torque_scale = 0.5,}, + { name = "AnkleFlexion_R_OmegaHalfScale", angle_sign = -1, torque_sign = -1, body ="foot_right" , act_time = 0.05, deact_time = 0.05, max_angular_velocity_scale = 0.5,}, + { name = "UnitExtensor_R", angle_sign = -1, torque_sign = 1, body ="thigh_right", joint_index=2 }, + { name = "UnitFlexor_R", angle_sign = -1, torque_sign = -1, body ="thigh_right", joint_index=2 }, + { name = "KneeExtension_R_Anderson2007", angle_sign = -1, torque_sign = -1, body ="foot_right" , act_time = 0.05, deact_time = 0.05, max_angular_velocity_scale = 0.5, + data_set = "Anderson2007", age_group = "SeniorOver65", gender="female", active_torque_angle_scale = 2.0}, +}, +frames = { + { + name = "pelvis", + parent = "ROOT", + body = bodies.pelvis, + joint = joints.freeflyer, + markers = { + LASI = { 0.047794, 0.200000, 0.070908,}, + RASI = { 0.047794, -0.200000, 0.070908,}, + LPSI = { -0.106106, 0.200000, 0.070908,}, + RPSI = { -0.106106, -0.200000, 0.070908,},}, + }, + { + name = "thigh_right", + parent = "pelvis", + body = bodies.thigh_right, + joint = joints.euler_zyx, + markers = { + RTHI = { -0.007376, -0.000000, -0.243721,}, + RKNE = { -0.011611, -0.000000, -0.454494,},}, + }, + { + name = "shank_right", + parent = "thigh_right", + body = bodies.thigh_right, + joint = joints.rotational_y, + }, + { + name = "foot_right", + parent = "shank_right", + body = bodies.foot_right, + joint = joints.rotational_y, + }, + { + name = "thigh_left", + parent = "pelvis", + body = bodies.thigh_left, + joint = joints.euler_zyx, + }, + { + name = "shank_left", + parent = "thigh_left", + body = bodies.shank_left, + joint = joints.rotational_y, + }, + { + name = "foot_left", + parent = "shank_left", + body = bodies.foot_left, + joint = joints.rotational_y, + }, +}, +} \ No newline at end of file diff --git a/addons/luamodel/tests/CMakeLists.txt b/addons/luamodel/tests/CMakeLists.txt new file mode 100644 index 0000000..a9e86b4 --- /dev/null +++ b/addons/luamodel/tests/CMakeLists.txt @@ -0,0 +1,86 @@ +CMAKE_MINIMUM_REQUIRED (VERSION 3.0) + +SET ( RBDL_LUAMODEL_TESTS_VERSION_MAJOR 1 ) +SET ( RBDL_LUAMODEL_TESTS_VERSION_MINOR 0 ) +SET ( RBDL_LUAMODEL_TESTS_VERSION_PATCH 0 ) + +SET ( RBDL_LUAMODEL_TESTS_VERSION + ${RBDL_LUAMODEL_TESTS_VERSION_MAJOR}.${RBDL_LUAMODEL_TESTS_VERSION_MINOR}.${RBDL_LUAMODEL_TESTS_VERSION_PATCH} +) + + +PROJECT (RBDL_LUAMODEL_TESTS VERSION ${RBDL_LUAMODEL_TESTS_VERSION}) + +# Look for catch2 +FIND_PACKAGE(Catch2 REQUIRED) +FIND_PACKAGE (Lua51 REQUIRED) +INCLUDE_DIRECTORIES (${CMAKE_CURRENT_BINARY_DIR}/include + ${LUA_INCLUDE_DIR} + ${CMAKE_SOURCE_DIR}/tests + ../) +# on unix systems add /usr/local/include to include dirs +# this was added since macos homebrew installs many things there but macos +# does not automatically search their, and on linux systems this does not hurt +IF (NOT WIN32) + INCLUDE_DIRECTORIES(/usr/local/include) +ENDIF (NOT WIN32) + +SET ( LUAMODEL_TESTS_SRCS + testLuaModel.cc + ../luamodel.h + ../luatables.h + ../luamodel.cc + ../luatables.cc + ) + +IF ( ${CMAKE_VERSION} VERSION_LESS 3.12.0 ) + ADD_DEFINITIONS (-DRBDL_LUAMODEL_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}/..") +ELSE( ${CMAKE_VERSION} VERSION_LESS 3.12.0 ) + ADD_COMPILE_DEFINITIONS (RBDL_LUAMODEL_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}/..") +ENDIF( ${CMAKE_VERSION} VERSION_LESS 3.12.0 ) + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX +) + +ADD_EXECUTABLE ( rbdl_luamodel_tests ${LUAMODEL_TESTS_SRCS} ) + +SET_TARGET_PROPERTIES ( rbdl_luamodel_tests PROPERTIES + LINKER_LANGUAGE CXX + OUTPUT_NAME luamodel_tests + ) + +SET (RBDL_LIBRARY rbdl) +IF (RBDL_BUILD_STATIC) + SET (RBDL_LIBRARY rbdl-static) +ENDIF (RBDL_BUILD_STATIC) + +SET (MUSCLE_ADDON_LIBRARY rbdl_muscle) +IF (RBDL_BUILD_STATIC) + SET (MUSCLE_ADDON_LIBRARY rbdl_muscle-static) +ENDIF (RBDL_BUILD_STATIC) + + +IF(RBDL_BUILD_ADDON_MUSCLE) + TARGET_LINK_LIBRARIES ( rbdl_luamodel_tests + ${MUSCLE_ADDON_LIBRARY} + ${RBDL_LIBRARY} + ${LUA_LIBRARIES} + ) +ELSE(RBDL_BUILD_ADDON_MUSCLE) + TARGET_LINK_LIBRARIES ( rbdl_luamodel_tests + ${RBDL_LIBRARY} + ${LUA_LIBRARIES} + ) +ENDIF(RBDL_BUILD_ADDON_MUSCLE) + +OPTION (RUN_AUTOMATIC_TESTS "Perform automatic tests after compilation?" OFF) + +IF (RUN_AUTOMATIC_TESTS) + +ADD_CUSTOM_COMMAND (TARGET rbdl_luamodel_tests + POST_BUILD + COMMAND ./luamodel_tests + COMMENT "Running automated addon luamodel tests..." + ) +ENDIF (RUN_AUTOMATIC_TESTS) diff --git a/addons/luamodel/tests/testLuaModel.cc b/addons/luamodel/tests/testLuaModel.cc new file mode 100644 index 0000000..fe2401e --- /dev/null +++ b/addons/luamodel/tests/testLuaModel.cc @@ -0,0 +1,926 @@ +/* + * RBDL - Rigid Body Dynamics Library: Addon : muscle + * Copyright (c) 2016 Matthew Millard + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +//============================================================================== +// INCLUDES +//============================================================================== +#define CATCH_CONFIG_MAIN +#include "luamodel.h" +#include "luatables.h" +#include +#include +#include +#include + +#include "rbdl_tests.h" + +#ifdef RBDL_BUILD_ADDON_MUSCLE +#include "../muscle/Millard2016TorqueMuscle.h" +#endif + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; +using namespace RigidBodyDynamics::Addons; + +using namespace std; + +const double TEST_PREC = 1.0e-11; + +std::string rbdlSourcePath = RBDL_LUAMODEL_SOURCE_DIR; + +TEST_CASE(__FILE__"_LoadLuaModel", "") +{ + Model model; + std::string modelFile = rbdlSourcePath; + modelFile.append("/samplemodel.lua"); + bool modelLoaded = LuaModelReadFromFile(modelFile.c_str(), &model, false); + CHECK(modelLoaded); +} +TEST_CASE(__FILE__"_LoadMotionCaptureMarkers", "") +{ + Model model; + std::string modelFile = rbdlSourcePath; + modelFile.append("/samplemodel.lua"); + bool modelLoaded = LuaModelReadFromFile(modelFile.c_str(), &model, false); + std::vector< MotionCaptureMarker > updMarkerSet; + bool markersLoaded = LuaModelReadMotionCaptureMarkers(modelFile.c_str(), + &model, updMarkerSet,false); + CHECK(updMarkerSet.size()==6); + //The markers come out of order which makes testing a bit tricky. + for(unsigned int i=0; i updLocalFrameSet; + bool localFramesLoaded = LuaModelReadLocalFrames(modelFile.c_str(),&model, + updLocalFrameSet,false); + + CHECK(updLocalFrameSet.size()==2); + + unsigned int thighLeftId = model.GetBodyId("thigh_left"); + unsigned int thighRightId = model.GetBodyId("thigh_right"); + + CHECK(std::strcmp("Pocket_L",updLocalFrameSet[0].name.c_str())==0); + CHECK(updLocalFrameSet[0].body_id == thighLeftId); + + CHECK_THAT(updLocalFrameSet[0].r[0], + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[0].r[1], + IsClose(0.2, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[0].r[2], + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + + CHECK_THAT(updLocalFrameSet[0].E(0,0), + IsClose(1.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[0].E(0,1), + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[0].E(0,2), + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + + CHECK_THAT(updLocalFrameSet[0].E(1,0), + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[0].E(1,1), + IsClose(1.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[0].E(1,2), + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + + CHECK_THAT(updLocalFrameSet[0].E(2,0), + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[0].E(2,1), + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[0].E(2,2), + IsClose(1.0, TEST_PREC, TEST_PREC) + ); + + + CHECK(std::strcmp("Pocket_R",updLocalFrameSet[1].name.c_str())==0); + CHECK(updLocalFrameSet[1].body_id == thighRightId); + CHECK_THAT(updLocalFrameSet[1].r[0], + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[1].r[1], + IsClose(-0.2, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[1].r[2], + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + + CHECK_THAT(updLocalFrameSet[1].E(0,0), + IsClose(1.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[1].E(0,1), + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[1].E(0,2), + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + + CHECK_THAT(updLocalFrameSet[1].E(1,0), + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[1].E(1,1), + IsClose(1.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[1].E(1,2), + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + + CHECK_THAT(updLocalFrameSet[1].E(2,0), + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[1].E(2,1), + IsClose(0.0, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updLocalFrameSet[1].E(2,2), + IsClose(1.0, TEST_PREC, TEST_PREC) + ); + + +} +TEST_CASE(__FILE__"_LoadPoints", "") +{ + Model model; + std::string modelFile = rbdlSourcePath; + modelFile.append("/samplemodel.lua"); + bool modelLoaded = LuaModelReadFromFile(modelFile.c_str(), &model, false); + std::vector< Point > updPointSet; + bool pointsLoaded = LuaModelReadPoints(modelFile.c_str(),&model, + updPointSet,false); + CHECK(updPointSet.size()==4); + + unsigned int bodyId = model.GetBodyId("foot_right"); + + CHECK( strcmp( updPointSet[0].name.c_str(),"Heel_Medial_L") == 0); + CHECK( strcmp( updPointSet[1].name.c_str(),"Heel_Lateral_L") == 0); + CHECK( strcmp( updPointSet[2].name.c_str(),"ForeFoot_Medial_L") == 0); + CHECK( strcmp( updPointSet[3].name.c_str(),"ForeFoot_Lateral_L") == 0); + + CHECK( updPointSet[0].body_id == bodyId ); + CHECK( updPointSet[1].body_id == bodyId ); + CHECK( updPointSet[2].body_id == bodyId ); + CHECK( updPointSet[3].body_id == bodyId ); + + CHECK_THAT(updPointSet[0].point_local[0], + IsClose(-0.080, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updPointSet[0].point_local[1], + IsClose(-0.042, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updPointSet[0].point_local[2], + IsClose(-0.091, TEST_PREC, TEST_PREC) + ); + + CHECK_THAT(updPointSet[1].point_local[0], + IsClose(-0.080, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updPointSet[1].point_local[1], + IsClose(0.042, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updPointSet[1].point_local[2], + IsClose(-0.091, TEST_PREC, TEST_PREC) + ); + + CHECK_THAT(updPointSet[2].point_local[0], + IsClose(0.181788, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updPointSet[2].point_local[1], + IsClose(-0.054000, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updPointSet[2].point_local[2], + IsClose(-0.091000, TEST_PREC, TEST_PREC) + ); + + CHECK_THAT(updPointSet[3].point_local[0], + IsClose(0.181788, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updPointSet[3].point_local[1], + IsClose(0.054000, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(updPointSet[3].point_local[2], + IsClose(-0.091000, TEST_PREC, TEST_PREC) + ); +} + +TEST_CASE(__FILE__"_LoadConstrainedLuaModel", "") +{ + RigidBodyDynamics::Model model; + std::string modelFile = rbdlSourcePath; + modelFile.append("/sampleconstrainedmodel.lua"); + + std::vector constraintSetNames = + LuaModelGetConstraintSetNames(modelFile.c_str()); + std::vector constraintSets; + + constraintSets.resize(constraintSetNames.size()); + for(unsigned int i=0; i bodyIds = + constraintSets[0].contactConstraints[0]->getBodyIds(); + CHECK(bodyIds.size() == 2); + CHECK(bodyIds[0] == baseId); + CHECK(bodyIds[1] == rootId); + + std::vector< Vector3d > normalVectors = + constraintSets[0].contactConstraints[0]->getConstraintNormalVectors(); + + // (all contact constraints between the same pair of bodies are grouped) + CHECK(normalVectors.size()==1); + CHECK_THAT(normalVectors[0][0], + IsClose(1., TEST_PREC, TEST_PREC) + ); + CHECK_THAT(normalVectors[0][1], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT(normalVectors[0][2], + IsClose(0., TEST_PREC, TEST_PREC) + ); + + //MM 17/5/2020 + //Contract constraints currently do not have the Baumgarte stabilization + //parameter exposed: these kinds of constraints are so well numerically + //behaved that this kind of constraint stabilization is normally not required. + CHECK(constraintSets[0].isBaumgarteStabilizationEnabled(groupIndex)==false); + + // Contact Constraint YZ + groupIndex = constraintSets[0].getGroupIndexByName("contactBaseYZ"); + CHECK(constraintSets[0].getGroupSize(groupIndex) == 2); + CHECK(constraintSets[0].getGroupType(groupIndex) == ConstraintTypeContact); + userDefinedId = constraintSets[0].getGroupId(groupIndex); + CHECK(userDefinedId == 3); + + normalVectors = + constraintSets[0].contactConstraints[1]->getConstraintNormalVectors(); + CHECK(normalVectors.size()==2); + + CHECK_THAT(normalVectors[0][0], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT(normalVectors[0][1], + IsClose(1., TEST_PREC, TEST_PREC) + ); + CHECK_THAT(normalVectors[0][2], + IsClose(0., TEST_PREC, TEST_PREC) + ); + + CHECK_THAT(normalVectors[1][0], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT(normalVectors[1][1], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT(normalVectors[1][2], + IsClose(1., TEST_PREC, TEST_PREC) + ); + + //MM 17/5/2020 + //Contract constraints currently do not have the Baumgarte stabilization + //parameter exposed: these kinds of constraints are so well numerically + //behaved that this kind of constraint stabilization is normally not required. + CHECK(constraintSets[0].isBaumgarteStabilizationEnabled(groupIndex) == false); + + // Loop Constraint X + groupIndex = constraintSets[0].getGroupIndexByName("loopL12L22Tx"); + + CHECK(constraintSets[0].getGroupSize(groupIndex) == 1); + CHECK(constraintSets[0].getGroupType(groupIndex) == ConstraintTypeLoop); + userDefinedId = constraintSets[0].getGroupId(groupIndex); + CHECK(userDefinedId == 1); + + bodyIds = constraintSets[0].loopConstraints[0]->getBodyIds(); + CHECK(bodyIds.size()==2); + CHECK(bodyIds[0] == l12Id); + CHECK(bodyIds[1] == l22Id); + + //Loop constraints often require stabilization so the Baumgarte + //stabilization parameters are exposed + CHECK(constraintSets[0].isBaumgarteStabilizationEnabled(groupIndex) == false); + + std::vector< SpatialVector > axis = + constraintSets[0].loopConstraints[0]->getConstraintAxes(); + CHECK(axis.size()==1); + CHECK_THAT( axis[0][0], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][1], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][2], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][3], + IsClose(1., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][4], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][5], + IsClose(0., TEST_PREC, TEST_PREC) + ); + + // Loop Constraint Y + groupIndex = constraintSets[0].getGroupIndexByName("loopL12L22Ty"); + CHECK(constraintSets[0].getGroupSize(groupIndex) == 1); + CHECK(constraintSets[0].getGroupType(groupIndex) == ConstraintTypeLoop); + userDefinedId = constraintSets[0].getGroupId(groupIndex); + CHECK(userDefinedId == 2); + + axis =constraintSets[0].loopConstraints[1]->getConstraintAxes(); + CHECK(axis.size()==1); + + CHECK_THAT( axis[0][0], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][1], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][2], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][3], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][4], + IsClose(1., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][5], + IsClose(0., TEST_PREC, TEST_PREC) + ); + + //Loop constraints often require stabilization so the Baumgarte + //stabilization parameters are exposed + CHECK(constraintSets[0].isBaumgarteStabilizationEnabled(groupIndex) == true); + + + // Contact Constraint XYZ + groupIndex = constraintSets[1].getGroupIndexByName("contactBaseXYZ"); + CHECK(constraintSets[1].getGroupSize(groupIndex) == 3); + CHECK(constraintSets[1].getGroupType(groupIndex) == ConstraintTypeContact); + CHECK(constraintSets[1].getGroupId(groupIndex) == 2); + + bodyIds = constraintSets[1].contactConstraints[0]->getBodyIds(); + CHECK(bodyIds.size()==2); + CHECK(bodyIds[0] == baseId); + CHECK(bodyIds[1] == rootId); + + normalVectors = + constraintSets[1].contactConstraints[0]->getConstraintNormalVectors(); + CHECK(normalVectors.size()==3); + CHECK_THAT(normalVectors[0][0], + IsClose(1., TEST_PREC, TEST_PREC) + ); + CHECK_THAT(normalVectors[0][1], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT(normalVectors[0][2], + IsClose(0., TEST_PREC, TEST_PREC) + ); + + CHECK_THAT(normalVectors[1][0], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT(normalVectors[1][1], + IsClose(1., TEST_PREC, TEST_PREC) + ); + CHECK_THAT(normalVectors[1][2], + IsClose(0., TEST_PREC, TEST_PREC) + ); + + CHECK_THAT(normalVectors[2][0], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT(normalVectors[2][1], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT(normalVectors[2][2], + IsClose(1., TEST_PREC, TEST_PREC) + ); + + CHECK(constraintSets[1].isBaumgarteStabilizationEnabled(groupIndex) == false); + + // Loop Constraint Tx Ty + groupIndex = constraintSets[1].getGroupIndexByName("loopL12L22TxTy"); + CHECK(constraintSets[1].getGroupSize(groupIndex) == 2); + CHECK(constraintSets[1].getGroupType(groupIndex) == ConstraintTypeLoop); + CHECK(constraintSets[1].getGroupId(groupIndex) == 1); + + bodyIds = constraintSets[1].loopConstraints[0]->getBodyIds(); + CHECK(bodyIds.size()==2); + CHECK(bodyIds[0] == l12Id); + CHECK(bodyIds[1] == l22Id); + + axis = + constraintSets[1].loopConstraints[0]->getConstraintAxes(); + CHECK(axis.size()==2); + CHECK_THAT( axis[0][0], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][1], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][2], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][3], + IsClose(1., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][4], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[0][5], + IsClose(0., TEST_PREC, TEST_PREC) + ); + + CHECK_THAT( axis[1][0], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[1][1], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[1][2], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[1][3], + IsClose(0., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[1][4], + IsClose(1., TEST_PREC, TEST_PREC) + ); + CHECK_THAT( axis[1][5], + IsClose(0., TEST_PREC, TEST_PREC) + ); + + CHECK(constraintSets[1].isBaumgarteStabilizationEnabled(groupIndex) == false); + + std::vector phasing; + bool constraintSetPhasingLoaded = + LuaModelGetConstraintSetPhases(modelFile.c_str(),constraintSetNames, + phasing); + CHECK(constraintSetPhasingLoaded); + + CHECK(phasing[0]==0); + CHECK(phasing[1]==1); + CHECK(phasing[2]==1); + CHECK(phasing[3]==0); + +} + +#ifdef RBDL_BUILD_ADDON_MUSCLE +TEST_CASE(__FILE__"_LoadMuscleTorqueGenerators", "") +{ + RigidBodyDynamics::Model model; + std::string modelFile = rbdlSourcePath; + modelFile.append("/samplemodelwithtorquemuscles.lua"); + + + + bool modelLoaded = LuaModelReadFromFile( modelFile.c_str(), + &model, + false); + + CHECK(modelLoaded); + + HumanMetaData humanData; + bool humanDataLoaded = + LuaModelReadHumanMetaData(modelFile.c_str(),humanData,false); + CHECK(humanDataLoaded); + + CHECK(std::fabs(humanData.age - 35.0) < TEST_PREC); + CHECK(std::fabs(humanData.height - 1.73) < TEST_PREC); + CHECK(std::fabs(humanData.height - 1.73) < TEST_PREC); + CHECK(std::strcmp(humanData.age_group.c_str(),"Young18To25")==0); + CHECK(std::strcmp(humanData.gender.c_str(),"male")==0); + + + std::vector < Muscle::Millard2016TorqueMuscle > mtgSet; + std::vector< Millard2016TorqueMuscleConfig > mtgInfoSet; + + bool torqueMusclesLoaded = LuaModelReadMillard2016TorqueMuscleSets( + modelFile.c_str(),&model,humanData,mtgSet,mtgInfoSet,false); + + CHECK(torqueMusclesLoaded); + CHECK(mtgSet.size() == 12); + CHECK(mtgInfoSet.size() == 12); + + unsigned int i=0; + + //Check that the data is being loaded as it is written in the file for the + //full right leg + i=0; + CHECK(std::strcmp(mtgInfoSet[i].name.c_str(),"HipExtension_R")==0); + CHECK(std::fabs(mtgInfoSet[i].angle_sign - (-1)) < TEST_PREC); + CHECK(std::fabs(mtgInfoSet[i].torque_sign - ( 1)) < TEST_PREC); + CHECK(std::strcmp(mtgInfoSet[i].body.c_str(),"thigh_right")== 0); + CHECK(mtgInfoSet[i].joint_index - 1 == 0); + CHECK(std::fabs(mtgInfoSet[i].activation_time_constant - 0.05) < TEST_PREC); + CHECK(std::fabs(mtgInfoSet[i].deactivation_time_constant - 0.05) < TEST_PREC); + i=1; + CHECK(std::strcmp(mtgInfoSet[i].name.c_str(),"HipFlexion_R")==0); + CHECK(std::fabs(mtgInfoSet[i].angle_sign - (-1)) < TEST_PREC); + CHECK(std::fabs(mtgInfoSet[i].torque_sign - (-1)) < TEST_PREC); + CHECK(std::strcmp(mtgInfoSet[i].body.c_str(),"thigh_right")== 0); + CHECK(mtgInfoSet[i].joint_index - 1 == 0); + CHECK(std::fabs(mtgInfoSet[i].activation_time_constant - 0.05) < TEST_PREC); + CHECK(std::fabs(mtgInfoSet[i].deactivation_time_constant - 0.05) < TEST_PREC); + i=2; + CHECK(std::strcmp(mtgInfoSet[i].name.c_str(),"KneeExtension_R")==0); + CHECK(std::fabs(mtgInfoSet[i].angle_sign - ( 1)) < TEST_PREC); + CHECK(std::fabs(mtgInfoSet[i].torque_sign - (-1)) < TEST_PREC); + CHECK(std::strcmp(mtgInfoSet[i].body.c_str(),"shank_right")== 0); + CHECK(std::fabs(mtgInfoSet[i].activation_time_constant - 0.05) < TEST_PREC); + CHECK(std::fabs(mtgInfoSet[i].deactivation_time_constant - 0.05) < TEST_PREC); + i=3; + CHECK(std::strcmp(mtgInfoSet[i].name.c_str(),"KneeFlexion_R")==0); + CHECK(std::fabs(mtgInfoSet[i].angle_sign - ( 1)) < TEST_PREC); + CHECK(std::fabs(mtgInfoSet[i].torque_sign - ( 1)) < TEST_PREC); + CHECK(std::strcmp(mtgInfoSet[i].body.c_str(),"shank_right")== 0); + CHECK(std::fabs(mtgInfoSet[i].activation_time_constant - 0.05) < TEST_PREC); + CHECK(std::fabs(mtgInfoSet[i].deactivation_time_constant - 0.05) < TEST_PREC); + i=4; + CHECK(std::strcmp(mtgInfoSet[i].name.c_str(),"AnkleExtension_R")==0); + CHECK(std::fabs(mtgInfoSet[i].angle_sign - (-1)) < TEST_PREC); + CHECK(std::fabs(mtgInfoSet[i].torque_sign - ( 1)) < TEST_PREC); + CHECK(std::strcmp(mtgInfoSet[i].body.c_str(),"foot_right")== 0); + CHECK(std::fabs(mtgInfoSet[i].activation_time_constant - 0.05) < TEST_PREC); + CHECK(std::fabs(mtgInfoSet[i].deactivation_time_constant - 0.05) < TEST_PREC); + i=5; + CHECK(std::strcmp(mtgInfoSet[i].name.c_str(),"AnkleFlexion_R")==0); + CHECK(std::fabs(mtgInfoSet[i].angle_sign - (-1)) < TEST_PREC); + CHECK(std::fabs(mtgInfoSet[i].torque_sign - (-1)) < TEST_PREC); + CHECK(std::strcmp(mtgInfoSet[i].body.c_str(),"foot_right")== 0); + CHECK(std::fabs(mtgInfoSet[i].activation_time_constant - 0.05) < TEST_PREC); + CHECK(std::fabs(mtgInfoSet[i].deactivation_time_constant - 0.05) < TEST_PREC); + i=6; + //Check that the passive_element_torque_scale is working + CHECK(std::strcmp(mtgInfoSet[i].name.c_str(), + "AnkleFlexion_R_FpeHalfScale")==0); + unsigned int iRef = 5; + CHECK_THAT(mtgSet[i].getPassiveTorqueScale(), + IsClose(0.5, TEST_PREC, TEST_PREC) + ); + Muscle::TorqueMuscleInfo tmi, tmiRef; + mtgSet[i].calcTorqueMuscleInfo(1,0,0,tmi); + mtgSet[iRef].calcTorqueMuscleInfo(1,0,0,tmiRef); + CHECK(tmiRef.fiberPassiveTorqueAngleMultiplier > 0.); + CHECK_THAT(tmiRef.fiberPassiveTorqueAngleMultiplier, + IsClose(tmi.fiberPassiveTorqueAngleMultiplier*2.0, + TEST_PREC, TEST_PREC) + ); + i=7; + //Check that the max_isometric_torque_scale is working + CHECK(std::strcmp(mtgInfoSet[i].name.c_str(), + "AnkleFlexion_R_FisoHalfScale")==0); + CHECK_THAT(mtgSet[iRef].getMaximumActiveIsometricTorque(), + IsClose(mtgSet[i].getMaximumActiveIsometricTorque()*2.0, + TEST_PREC, TEST_PREC) + ); + i=8; + //Check that max_angular_velocity_scale is working + CHECK(std::strcmp(mtgInfoSet[i].name.c_str(), + "AnkleFlexion_R_OmegaHalfScale")==0); + CHECK_THAT(mtgSet[iRef].getMaximumConcentricJointAngularVelocity(), + IsClose(mtgSet[i].getMaximumConcentricJointAngularVelocity()*2.0, + TEST_PREC, TEST_PREC) + ); + + i=9; + //UnitExtensor + CHECK(std::strcmp(mtgInfoSet[i].name.c_str(), + "UnitExtensor_R")==0); + mtgSet[i].calcTorqueMuscleInfo(0,0,1,tmi); + double angleSign = mtgInfoSet[i].angle_sign; + CHECK_THAT(tmi.fiberActiveTorqueAngleMultiplier, + IsClose(1.0, TEST_PREC, TEST_PREC) + ); + + //This extensor gets a Gaussian shaped active force length curve + //with a standard deviation of 1 radian. + double angle = 1; + double width = 1; + double faRef = exp(-angle*angle/(2*width*width)); + mtgSet[i].calcTorqueMuscleInfo(1*angleSign,0,1,tmi); + CHECK_THAT(tmi.fiberActiveTorqueAngleMultiplier, + IsClose(faRef, TEST_PREC, TEST_PREC) + ); + //The UnitExtensors passive curve reaches a unit torque at 1 radian of flexion + CHECK_THAT(tmi.fiberPassiveTorqueAngleMultiplier, + IsClose(1.0, TEST_PREC, TEST_PREC) + ); + + mtgSet[i].calcTorqueMuscleInfo(-1*angleSign,0,1,tmi); + CHECK_THAT(tmi.fiberActiveTorqueAngleMultiplier, + IsClose(faRef, TEST_PREC, TEST_PREC) + ); + //The UnitExtensor has a maximum isometric torque of 1 Nm + CHECK_THAT(mtgSet[i].getMaximumActiveIsometricTorque(), + IsClose(1., TEST_PREC, TEST_PREC) + ); + //The UnitExtensor has a maximum angular velocity of 1 rad/sec + CHECK_THAT(mtgSet[i].getMaximumConcentricJointAngularVelocity(), + IsClose(1., TEST_PREC, TEST_PREC) + ); + + i=10; + //UnitFlexor + CHECK(std::strcmp(mtgInfoSet[i].name.c_str(), + "UnitFlexor_R")==0); + + mtgSet[i].calcTorqueMuscleInfo(0,0,1,tmi); + CHECK_THAT(tmi.fiberActiveTorqueAngleMultiplier, + IsClose(1.0, TEST_PREC, TEST_PREC) + ); + + //This flexor gets a Gaussian shaped active force length curve + //with a standard deviation of 1 radian. + mtgSet[i].calcTorqueMuscleInfo(1*angleSign,0,1,tmi); + CHECK_THAT(tmi.fiberActiveTorqueAngleMultiplier, + IsClose(faRef, TEST_PREC, TEST_PREC) + ); + + mtgSet[i].calcTorqueMuscleInfo(-1*angleSign,0,1,tmi); + CHECK_THAT(tmi.fiberActiveTorqueAngleMultiplier, + IsClose(faRef, TEST_PREC, TEST_PREC) + ); + //The UnitFlexor's passive curve reaches a unit torque at 1 radian of extension + CHECK_THAT(tmi.fiberPassiveTorqueAngleMultiplier, + IsClose(1.0, TEST_PREC, TEST_PREC) + ); + + //The UnitExtensor has a maximum isometric torque of 1 Nm + CHECK_THAT(mtgSet[i].getMaximumActiveIsometricTorque(), + IsClose(1., TEST_PREC, TEST_PREC) + ); + //The UnitExtensor has a maximum angular velocity of 1 rad/sec + CHECK_THAT(mtgSet[i].getMaximumConcentricJointAngularVelocity(), + IsClose(1.*angleSign, TEST_PREC, TEST_PREC) + ); + + i=11; + CHECK(std::strcmp(mtgInfoSet[i].name.c_str(), + "KneeExtension_R_Anderson2007")==0); + CHECK(std::strcmp( mtgInfoSet[i].data_set.c_str(), + "Anderson2007") == 0); + CHECK(std::strcmp( mtgInfoSet[i].age_group.c_str(), + "SeniorOver65") == 0); + CHECK(std::strcmp( mtgInfoSet[i].gender.c_str(), + "female") == 0); + + CHECK(mtgSet[i].getDataSet() == Muscle::DataSet::Anderson2007); + CHECK(mtgSet[i].getAgeGroup() == Muscle::AgeGroupSet::SeniorOver65); + CHECK(mtgSet[i].getGender() == Muscle::GenderSet::Female); + CHECK_THAT(mtgSet[i].getSubjectMass(), + IsClose(81.68, TEST_PREC, TEST_PREC) + ); + CHECK_THAT(mtgSet[i].getSubjectHeight(), + IsClose(1.73, TEST_PREC, TEST_PREC) + ); + + CHECK_THAT(mtgSet[i].getActiveTorqueAngleCurveAngleScaling(), + IsClose(2.0,TEST_PREC, TEST_PREC) + ); + + mtgSet[i].calcTorqueMuscleInfo(0.,0.,0.,tmi); + mtgSet[i].setActiveTorqueAngleCurveAngleScaling(0.1); + mtgSet[i].calcTorqueMuscleInfo(0.,0.,0.,tmiRef); + + CHECK(std::fabs( tmi.fiberActiveTorqueAngleMultiplier + -tmiRef.fiberActiveTorqueAngleMultiplier) > TEST_PREC ); + +} + +#endif + +//At the present time this is not much of a test: all of the code is run and +//it is checked that each function returns true. The header has been manually +//inspected but is otherwise not checked in this test for correctness. It could +//be compared to a saved prototype header. This is a weak check, but better than +//nothing I suppose. +TEST_CASE(__FILE__"_ModelHeaderGeneration", "") +{ + RigidBodyDynamics::Model model; + std::string modelFile = rbdlSourcePath; + modelFile.append("/complexmodel.lua"); + + //Get the constraint set names + std::vector constraintSetNames = + LuaModelGetConstraintSetNames(modelFile.c_str()); + std::vector constraintSets; + + //Get the constrained model + std::vector< ConstraintSet > conSet; + conSet.resize(constraintSetNames.size()); + for(unsigned int i=0; i phasing; + bool constraintSetPhasingLoaded = + LuaModelGetConstraintSetPhases(modelFile.c_str(),constraintSetNames, + phasing); + CHECK(constraintSetPhasingLoaded); + + //Get the local points + std::vector< Point > pointSet; + bool pointsLoaded = + LuaModelReadPoints(modelFile.c_str(),&model,pointSet,false); + CHECK(pointsLoaded); + + //Get the local motion capture markers + std::vector< MotionCaptureMarker > markerSet; + bool markersLoaded = + LuaModelReadMotionCaptureMarkers(modelFile.c_str(),&model,markerSet,false); + CHECK(markersLoaded); + + //Get the local frames + std::vector< LocalFrame > localFrames; + bool localFramesLoaded = + LuaModelReadLocalFrames(modelFile.c_str(),&model,localFrames,false); + CHECK(localFramesLoaded); + + //-------------------------------------------- + #ifdef RBDL_BUILD_ADDON_MUSCLE + + HumanMetaData participant_data; + bool participantDataLoaded = LuaModelReadHumanMetaData(modelFile.c_str(), + participant_data,false); + CHECK(participantDataLoaded); + + std::vector< Addons::Muscle::Millard2016TorqueMuscle > mtgSet; + std::vector< Millard2016TorqueMuscleConfig > mtgSetInfo; + bool mtgSetLoaded = LuaModelReadMillard2016TorqueMuscleSets( + modelFile.c_str(), &model, participant_data, mtgSet, mtgSetInfo, false); + CHECK(mtgSetLoaded); + + + + #endif + //-------------------------------------------- + //std::string headerFile = rbdlSourcePath; + //headerFile.append("/complexmodel.h"); + std::string headerFile("complexmodel.h"); + + bool modelHeaderGenerated= + LuaModelWriteModelHeaderEntries(headerFile.c_str(),model,false); + CHECK(modelHeaderGenerated); + + bool pointsHeaderGenerated= + LuaModelWritePointsHeaderEntries(headerFile.c_str(),pointSet,true); + CHECK(pointsHeaderGenerated); + + bool markerHeaderGenerated= LuaModelWriteMotionCaptureMarkerHeaderEntries( + headerFile.c_str(),markerSet,true); + CHECK(markerHeaderGenerated); + + bool localFrameHeaderGenerated = LuaModelWriteLocalFrameHeaderEntries( + headerFile.c_str(),localFrames,true); + CHECK(localFrameHeaderGenerated); + + bool constraintSetHeaderGenerated = LuaModelWriteConstraintSetHeaderEntries( + headerFile.c_str(),constraintSetNames,conSet,true); + + bool phasingHeaderGenerated = LuaModelWriteConstraintSetPhaseHeaderEntries( + headerFile.c_str(), constraintSetNames, phasing,true); + CHECK(phasingHeaderGenerated); + + //-------------------------------------------- + #ifdef RBDL_BUILD_ADDON_MUSCLE + + bool mtgHeaderGenerated = LuaModelWriteMillard2016TorqueMuscleHeaderEntries( + headerFile.c_str(),mtgSet,mtgSetInfo,true); + + + #endif + //-------------------------------------------- + + bool headerGuardsAdded = LuaModelAddHeaderGuards(headerFile.c_str()); + CHECK(headerGuardsAdded); + +} diff --git a/addons/muscle/CMakeLists.txt b/addons/muscle/CMakeLists.txt index 02bd25e..3d5335b 100755 --- a/addons/muscle/CMakeLists.txt +++ b/addons/muscle/CMakeLists.txt @@ -1,86 +1,139 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.0) -SET ( RBDL_ADDON_MUSCLE_VERSION_MAJOR 1 ) +#CMAKE_POLICY(SET CMP0048 NEW) +#CMAKE_POLICY(SET CMP0040 NEW) + +SET ( RBDL_ADDON_MUSCLE_VERSION_MAJOR 2 ) SET ( RBDL_ADDON_MUSCLE_VERSION_MINOR 0 ) SET ( RBDL_ADDON_MUSCLE_VERSION_PATCH 0 ) SET ( RBDL_ADDON_MUSCLE_VERSION - ${RBDL_ADDON_MUSCLE_VERSION_MAJOR}.${RBDL_ADDON_MUSCLE_VERSION_MINOR}.${RBDL_ADDON_MUSCLE_VERSION_PATCH} + ${RBDL_ADDON_MUSCLE_VERSION_MAJOR}.${RBDL_ADDON_MUSCLE_VERSION_MINOR}.${RBDL_ADDON_MUSCLE_VERSION_PATCH} ) -PROJECT (RBDL_ADDON_MUSCLE VERSION ${RBDL_ADDON_MUSCLE_VERSION}) - +PROJECT (RBDL_ADDON_MUSCLE VERSION ${RBDL_ADDON_MUSCLE_VERSION}) LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake ) - SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES - LINKER_LANGUAGE CXX - ) - -INCLUDE_DIRECTORIES ( - ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl + LINKER_LANGUAGE CXX ) +IF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + FIND_PACKAGE (IPOPT REQUIRED) + INCLUDE_DIRECTORIES ( + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl + ${IPOPT_INCLUDE_DIR} + ) + SET(MUSCLE_SOURCES + muscle.h + Millard2016TorqueMuscle.h + Millard2016TorqueMuscle.cc + TorqueMuscleFittingToolkit.h + TorqueMuscleFittingToolkit.cc + MuscleFunctionFactory.h + MuscleFunctionFactory.cc + TorqueMuscleFunctionFactory.h + TorqueMuscleFunctionFactory.cc + csvtools.h + csvtools.cc + ) + + SET(MUSCLE_HEADERS + muscle.h + Millard2016TorqueMuscle.h + TorqueMuscleFittingToolkit.h + MuscleFunctionFactory.h + TorqueMuscleFunctionFactory.h + csvtools.h + ) + + +ELSE(RBDL_BUILD_ADDON_MUSCLE_FITTING) + INCLUDE_DIRECTORIES ( + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl + ) + + SET(MUSCLE_SOURCES + muscle.h + Millard2016TorqueMuscle.h + Millard2016TorqueMuscle.cc + MuscleFunctionFactory.h + MuscleFunctionFactory.cc + TorqueMuscleFunctionFactory.h + TorqueMuscleFunctionFactory.cc + csvtools.h + csvtools.cc + ) + + SET(MUSCLE_HEADERS + muscle.h + Millard2016TorqueMuscle.h + MuscleFunctionFactory.h + TorqueMuscleFunctionFactory.h + csvtools.h + ) + +ENDIF(RBDL_BUILD_ADDON_MUSCLE_FITTING) + -SET(MUSCLE_SOURCES - muscle.h - Millard2016TorqueMuscle.h - Millard2016TorqueMuscle.cc - MuscleFunctionFactory.h - MuscleFunctionFactory.cc - TorqueMuscleFunctionFactory.h - TorqueMuscleFunctionFactory.cc - csvtools.h - csvtools.cc -) - -SET(MUSCLE_HEADERS - muscle.h - Millard2016TorqueMuscle.h - MuscleFunctionFactory.h - TorqueMuscleFunctionFactory.h - csvtools.h -) IF (RBDL_BUILD_STATIC) - ADD_LIBRARY ( rbdl_muscle-static STATIC ${MUSCLE_SOURCES} ) - SET_TARGET_PROPERTIES ( rbdl_muscle-static PROPERTIES PREFIX "lib") - SET_TARGET_PROPERTIES ( rbdl_muscle-static PROPERTIES OUTPUT_NAME "rbdl_muscle") - TARGET_LINK_LIBRARIES (rbdl_muscle-static - rbdl_geometry-static - rbdl-static - ) - - - INSTALL (TARGETS rbdl_muscle-static - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - ) + + ADD_LIBRARY ( rbdl_muscle-static STATIC ${MUSCLE_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl_muscle-static PROPERTIES PREFIX "lib") + SET_TARGET_PROPERTIES ( rbdl_muscle-static PROPERTIES OUTPUT_NAME "rbdl_muscle") + + IF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + TARGET_LINK_LIBRARIES (rbdl_muscle-static + rbdl_geometry-static + rbdl-static + ${IPOPT_LIBRARY}) + ELSE(RBDL_BUILD_ADDON_MUSCLE_FITTING) + TARGET_LINK_LIBRARIES (rbdl_muscle-static + rbdl_geometry-static + rbdl-static) + ENDIF(RBDL_BUILD_ADDON_MUSCLE_FITTING) + + INSTALL (TARGETS rbdl_muscle-static + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) ELSE (RBDL_BUILD_STATIC) - ADD_LIBRARY ( rbdl_muscle SHARED ${MUSCLE_SOURCES} ) - SET_TARGET_PROPERTIES ( rbdl_muscle PROPERTIES - VERSION ${RBDL_VERSION} - SOVERSION ${RBDL_SO_VERSION} - ) - - TARGET_LINK_LIBRARIES (rbdl_muscle - rbdl_geometry - rbdl - ) - - - INSTALL (TARGETS rbdl_muscle - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ) + + ADD_LIBRARY ( rbdl_muscle SHARED ${MUSCLE_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl_muscle PROPERTIES + VERSION ${RBDL_VERSION} + SOVERSION ${RBDL_SO_VERSION} + ) + + IF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + TARGET_LINK_LIBRARIES ( + rbdl_muscle + rbdl_geometry + rbdl + ${IPOPT_LIBRARY} + ) + ELSE (RBDL_BUILD_ADDON_MUSCLE_FITTING) + TARGET_LINK_LIBRARIES ( + rbdl_muscle + rbdl_geometry + rbdl + ) + ENDIF(RBDL_BUILD_ADDON_MUSCLE_FITTING) + + INSTALL (TARGETS rbdl_muscle + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) + ENDIF (RBDL_BUILD_STATIC) FILE ( GLOB headers - "${CMAKE_CURRENT_SOURCE_DIR}/*.h" - ) + "${CMAKE_CURRENT_SOURCE_DIR}/*.h" +) INSTALL ( FILES ${MUSCLE_HEADERS} - DESTINATION - ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/muscle - ) + DESTINATION + ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/muscle +) diff --git a/addons/muscle/LICENSE b/addons/muscle/LICENSE index a25f3e9..0e31354 100755 --- a/addons/muscle/LICENSE +++ b/addons/muscle/LICENSE @@ -1,5 +1,5 @@ Rigid Body Dynamics Library Muscle Addon - -Copyright (c) 2016 Matthew Millard +Copyright (c) 2016 Matthew Millard (zlib license) diff --git a/addons/muscle/Millard2016TorqueMuscle.cc b/addons/muscle/Millard2016TorqueMuscle.cc index 62b05e7..4a574a2 100755 --- a/addons/muscle/Millard2016TorqueMuscle.cc +++ b/addons/muscle/Millard2016TorqueMuscle.cc @@ -1,7 +1,7 @@ //============================================================================== -/* +/* * RBDL - Rigid Body Dynamics Library: Addon : muscle - * Copyright (c) 2016 Matthew Millard + * Copyright (c) 2016 Matthew Millard * * Licensed under the zlib license. See LICENSE for more details. */ @@ -10,6 +10,9 @@ #include "TorqueMuscleFunctionFactory.h" #include "csvtools.h" +#include + + #include #include @@ -19,213 +22,237 @@ #include #include #include - + #include + static double EPSILON = std::numeric_limits::epsilon(); static double SQRTEPSILON = sqrt(EPSILON); + using namespace RigidBodyDynamics::Math; using namespace RigidBodyDynamics::Addons::Muscle; using namespace RigidBodyDynamics::Addons::Geometry; using namespace std; +const double Millard2016TorqueMuscle::mTaLambdaMax = 1.0; +const double Millard2016TorqueMuscle::mTpLambdaMax = 0.0; +//static double mTvLambdaMax = 1.0; + + /************************************************************* Table Access Structure Names *************************************************************/ -const double gravity = 9.81; //Needed for the strength scaling used - //by Anderson et al. +const double Gravity = 9.81; +//Needed for the strength scaling used +//by Anderson et al. And this value has to +//be equal to the gravity parameter used by +//Anderson et al.: it should not be changed if +//the force of gravity differs in the model! const char* DataSet::names[] = { "Anderson2007", - "Gymnast"}; + "Gymnast" + }; const char* GenderSet::names[] = {"Male", - "Female"}; + "Female" + }; const char* AgeGroupSet::names[] = {"Young18To25", "Middle55To65", - "SeniorOver65"}; - -const char* JointTorqueSet::names[] = { "HipExtension" , - "HipFlexion" , - "KneeExtension" , - "KneeFlexion" , - "AnkleExtension" , - "AnkleFlexion" , - "ElbowExtension" , - "ElbowFlexion" , - "ShoulderExtension" , - "ShoulderFlexion" , - "WristExtension" , - "WristFlexion" , + "SeniorOver65" + }; + +const char* JointTorqueSet::names[] = { "HipExtension", + "HipFlexion", + "KneeExtension", + "KneeFlexion", + "AnkleExtension", + "AnkleFlexion", + "ElbowExtension", + "ElbowFlexion", + "ShoulderExtension", + "ShoulderFlexion", + "WristExtension", + "WristFlexion", "ShoulderHorizontalAdduction", "ShoulderHorizontalAbduction", - "ShoulderInternalRotation" , - "ShoulderExternalRotation" , - "WristUlnarDeviation" , - "WristRadialDeviation" , - "WristPronation" , - "WristSupination" , - "LumbarExtension" , - "LumbarFlexion" }; + "ShoulderInternalRotation", + "ShoulderExternalRotation", + "WristUlnarDeviation", + "WristRadialDeviation", + "WristPronation", + "WristSupination", + "LumbarExtension", + "LumbarFlexion", + "UnitExtensor", + "UnitFlexor" + }; const char* Anderson2007::GenderNames[] = {"Male","Female"}; const char* Anderson2007::AgeGroupNames[] = { "Young18To25", "Middle55To65", - "SeniorOver65"}; + "SeniorOver65" + }; -const char* Anderson2007::JointTorqueNames[] = {"HipExtension" , - "HipFlexion" , - "KneeExtension" , - "KneeFlexion" , +const char* Anderson2007::JointTorqueNames[] = {"HipExtension", + "HipFlexion", + "KneeExtension", + "KneeFlexion", "AnkleExtension", - "AnkleFlexion" }; + "AnkleFlexion" + }; const char* Gymnast::GenderNames[] = {"Male"}; const char* Gymnast::AgeGroupNames[] = {"Young18To25"}; -const char* Gymnast::JointTorqueNames[] = - { "HipExtension" , - "HipFlexion" , - "KneeExtension" , - "KneeFlexion" , - "AnkleExtension" , - "AnkleFlexion" , - "ElbowExtension" , - "ElbowFlexion" , - "ShoulderExtension" , - "ShoulderFlexion" , - "WristExtension" , - "WristFlexion" , - "ShoulderHorizontalAdduction", - "ShoulderHorizontalAbduction", - "ShoulderInternalRotation" , - "ShoulderExternalRotation" , - "WristUlnarDeviation" , - "WristRadialDeviation" , - "WristPronation" , - "WristSupination" , - "LumbarExtension" , - "LumbarFlexion"}; +const char* Gymnast::JointTorqueNames[] = { + "HipExtension", + "HipFlexion", + "KneeExtension", + "KneeFlexion", + "AnkleExtension", + "AnkleFlexion", + "ElbowExtension", + "ElbowFlexion", + "ShoulderExtension", + "ShoulderFlexion", + "WristExtension", + "WristFlexion", + "ShoulderHorizontalAdduction", + "ShoulderHorizontalAbduction", + "ShoulderInternalRotation", + "ShoulderExternalRotation", + "WristUlnarDeviation", + "WristRadialDeviation", + "WristPronation", + "WristSupination", + "LumbarExtension", + "LumbarFlexion", + "UnitExtensor", + "UnitFlexor" +}; /************************************************************* Coefficient Tables *************************************************************/ /* -This data is taken from Table 3 of +This data is taken from Table 3 of -Anderson, D. E., Madigan, M. L., & Nussbaum, M. A. (2007). -Maximum voluntary joint torque as a function of joint angle -and angular velocity: model development and application to +Anderson, D. E., Madigan, M. L., & Nussbaum, M. A. (2007). +Maximum voluntary joint torque as a function of joint angle +and angular velocity: model development and application to the lower limb. Journal of biomechanics, 40(14), 3105-3113. -Each row contains the coefficients for the active and +Each row contains the coefficients for the active and passive torque characteristics for a specific joint, direction, gender and age group. Each row corresponds -to a single block taken from Table 3, as read from +to a single block taken from Table 3, as read from left to right top to bottom. The first 4 columns have been added to describe the joint, direction, gender and age group. Column labels: Parameter Set Meta Data - 0: joint: hip0_knee1_ankle2, - 1: direction: ext0_flex1, - 2: gender: male0_female1, - 3: age: age18to25_0_55to65_1_g65_2, + 0: joint: hip0_knee1_ankle2, + 1: direction: ext0_flex1, + 2: gender: male0_female1, + 3: age: age18to25_0_55to65_1_g65_2, Active Torque-Angle and Torque-Velocity Curves - 4: c1, - 5: c2, - 6: c3, - 7: c4, - 8: c5, - 9: c6, -Passive Torque-Angle Curves + 4: c1, + 5: c2, + 6: c3, + 7: c4, + 8: c5, + 9: c6, +Passive Torque-Angle Curves 10: b1, - 11: k1, + 11: k1, 12: b2, - 13: k2, + 13: k2, */ double const Millard2016TorqueMuscle::Anderson2007Table3Mean[36][14] = { -{0,0,0,0,0.161,0.958,0.932,1.578,3.19,0.242,-1.21,-6.351,0.476,5.91 }, -{0,0,1,0,0.181,0.697,1.242,1.567,3.164,0.164,-1.753,-6.358,0.239,3.872 }, -{0,0,0,1,0.171,0.922,1.176,1.601,3.236,0.32,-2.16,-8.073,0.108,4.593 }, -{0,0,1,1,0.14,0.83,1.241,1.444,2.919,0.317,-1.361,-7.128,0.013,6.479 }, -{0,0,0,2,0.144,0.896,1.125,1.561,3.152,0.477,-2.671,-7.85,0.092,5.192 }, -{0,0,1,2,0.138,0.707,1.542,1.613,3.256,0.36,-0.758,-7.545,0.018,6.061 }, -{0,1,0,0,0.113,0.738,-0.214,2.095,4.267,0.218,1.21,-6.351,-0.476,5.91 }, -{0,1,1,0,0.127,0.65,-0.35,2.136,4.349,0.156,1.753,-6.358,-0.239,3.872 }, -{0,1,0,1,0.107,0.712,-0.192,2.038,4.145,0.206,2.16,-8.073,-0.108,4.593 }, -{0,1,1,1,0.091,0.812,-0.196,2.145,4.366,0.186,1.361,-7.128,-0.013,6.479 }, -{0,1,0,2,0.101,0.762,-0.269,1.875,3.819,0.296,2.671,-7.85,-0.092,5.192 }, -{0,1,1,2,0.081,0.625,-0.422,2.084,4.245,0.196,0.758,-7.545,-0.018,6.061 }, -{1,0,0,0,0.163,1.258,1.133,1.517,3.952,0.095,0,0,-6.25,-4.521 }, -{1,0,1,0,0.159,1.187,1.274,1.393,3.623,0.173,0,0,-8.033,-5.25 }, -{1,0,0,1,0.156,1.225,1.173,1.518,3.954,0.266,0,0,-12.83,-5.127 }, -{1,0,1,1,0.128,1.286,1.141,1.332,3.469,0.233,0,0,-6.576,-4.466 }, -{1,0,0,2,0.137,1.31,1.067,1.141,3.152,0.386,0,0,-10.519,-5.662 }, -{1,0,1,2,0.124,1.347,1.14,1.066,2.855,0.464,0,0,-8.8,-6.763 }, -{1,1,0,0,0.087,0.869,0.522,2.008,5.233,0.304,0,0,6.25,-4.521 }, -{1,1,1,0,0.08,0.873,0.635,1.698,4.412,0.175,0,0,8.033,-5.25 }, -{1,1,0,1,0.081,0.986,0.523,1.83,4.777,0.23,0,0,12.83,-5.127 }, -{1,1,1,1,0.06,0.967,0.402,1.693,4.41,0.349,0,0,6.576,-4.466 }, -{1,1,0,2,0.069,0.838,0.437,1.718,4.476,0.414,0,0,10.519,-5.662 }, -{1,1,1,2,0.06,0.897,0.445,1.121,2.922,0.389,0,0,8.8,-6.763 }, -{2,0,0,0,0.095,1.391,0.408,0.987,3.558,0.295,-0.0005781,-5.819,0.967,6.09 }, -{2,0,1,0,0.104,1.399,0.424,0.862,3.109,0.189,-0.005218,-4.875,0.47,6.425 }, -{2,0,0,1,0.114,1.444,0.551,0.593,2.128,0.35,-0.001311,-10.943,0.377,8.916 }, -{2,0,1,1,0.093,1.504,0.381,0.86,3.126,0.349,-2.888e-05,-17.189,0.523,7.888 }, -{2,0,0,2,0.106,1.465,0.498,0.49,1.767,0.571,-5.693e-05,-21.088,0.488,7.309 }, -{2,0,1,2,0.125,1.299,0.58,0.587,1.819,0.348,-2.35e-05,-12.567,0.331,6.629 }, -{2,1,0,0,0.033,1.51,-0.187,0.699,1.94,0.828,0.0005781,-5.819,-0.967,6.09 }, -{2,1,1,0,0.027,1.079,-0.302,0.864,2.399,0.771,0.005218,-4.875,-0.47,6.425 }, -{2,1,0,1,0.028,1.293,-0.284,0.634,1.759,0.999,0.001311,-10.943,-0.377,8.916 }, -{2,1,1,1,0.024,1.308,-0.254,0.596,1.654,1.006,2.888e-05,-17.189,-0.523,7.888}, -{2,1,0,2,0.029,1.419,-0.174,0.561,1.558,1.198,5.693e-05,-21.088,-0.488,7.309}, -{2,1,1,2,0.022,1.096,-0.369,0.458,1.242,1.401,2.35e-05,-12.567,-0.331,6.629 }}; + {0,0,0,0,0.161,0.958,0.932,1.578,3.19,0.242,-1.21,-6.351,0.476,5.91 }, + {0,0,1,0,0.181,0.697,1.242,1.567,3.164,0.164,-1.753,-6.358,0.239,3.872 }, + {0,0,0,1,0.171,0.922,1.176,1.601,3.236,0.32,-2.16,-8.073,0.108,4.593 }, + {0,0,1,1,0.14,0.83,1.241,1.444,2.919,0.317,-1.361,-7.128,0.013,6.479 }, + {0,0,0,2,0.144,0.896,1.125,1.561,3.152,0.477,-2.671,-7.85,0.092,5.192 }, + {0,0,1,2,0.138,0.707,1.542,1.613,3.256,0.36,-0.758,-7.545,0.018,6.061 }, + {0,1,0,0,0.113,0.738,-0.214,2.095,4.267,0.218,1.21,-6.351,-0.476,5.91 }, + {0,1,1,0,0.127,0.65,-0.35,2.136,4.349,0.156,1.753,-6.358,-0.239,3.872 }, + {0,1,0,1,0.107,0.712,-0.192,2.038,4.145,0.206,2.16,-8.073,-0.108,4.593 }, + {0,1,1,1,0.091,0.812,-0.196,2.145,4.366,0.186,1.361,-7.128,-0.013,6.479 }, + {0,1,0,2,0.101,0.762,-0.269,1.875,3.819,0.296,2.671,-7.85,-0.092,5.192 }, + {0,1,1,2,0.081,0.625,-0.422,2.084,4.245,0.196,0.758,-7.545,-0.018,6.061 }, + {1,0,0,0,0.163,1.258,1.133,1.517,3.952,0.095,0,0,-6.25,-4.521 }, + {1,0,1,0,0.159,1.187,1.274,1.393,3.623,0.173,0,0,-8.033,-5.25 }, + {1,0,0,1,0.156,1.225,1.173,1.518,3.954,0.266,0,0,-12.83,-5.127 }, + {1,0,1,1,0.128,1.286,1.141,1.332,3.469,0.233,0,0,-6.576,-4.466 }, + {1,0,0,2,0.137,1.31,1.067,1.141,3.152,0.386,0,0,-10.519,-5.662 }, + {1,0,1,2,0.124,1.347,1.14,1.066,2.855,0.464,0,0,-8.8,-6.763 }, + {1,1,0,0,0.087,0.869,0.522,2.008,5.233,0.304,0,0,6.25,-4.521 }, + {1,1,1,0,0.08,0.873,0.635,1.698,4.412,0.175,0,0,8.033,-5.25 }, + {1,1,0,1,0.081,0.986,0.523,1.83,4.777,0.23,0,0,12.83,-5.127 }, + {1,1,1,1,0.06,0.967,0.402,1.693,4.41,0.349,0,0,6.576,-4.466 }, + {1,1,0,2,0.069,0.838,0.437,1.718,4.476,0.414,0,0,10.519,-5.662 }, + {1,1,1,2,0.06,0.897,0.445,1.121,2.922,0.389,0,0,8.8,-6.763 }, + {2,0,0,0,0.095,1.391,0.408,0.987,3.558,0.295,-0.0005781,-5.819,0.967,6.09 }, + {2,0,1,0,0.104,1.399,0.424,0.862,3.109,0.189,-0.005218,-4.875,0.47,6.425 }, + {2,0,0,1,0.114,1.444,0.551,0.593,2.128,0.35,-0.001311,-10.943,0.377,8.916 }, + {2,0,1,1,0.093,1.504,0.381,0.86,3.126,0.349,-2.888e-05,-17.189,0.523,7.888 }, + {2,0,0,2,0.106,1.465,0.498,0.49,1.767,0.571,-5.693e-05,-21.088,0.488,7.309 }, + {2,0,1,2,0.125,1.299,0.58,0.587,1.819,0.348,-2.35e-05,-12.567,0.331,6.629 }, + {2,1,0,0,0.033,1.51,-0.187,0.699,1.94,0.828,0.0005781,-5.819,-0.967,6.09 }, + {2,1,1,0,0.027,1.079,-0.302,0.864,2.399,0.771,0.005218,-4.875,-0.47,6.425 }, + {2,1,0,1,0.028,1.293,-0.284,0.634,1.759,0.999,0.001311,-10.943,-0.377,8.916 }, + {2,1,1,1,0.024,1.308,-0.254,0.596,1.654,1.006,2.888e-05,-17.189,-0.523,7.888}, + {2,1,0,2,0.029,1.419,-0.174,0.561,1.558,1.198,5.693e-05,-21.088,-0.488,7.309}, + {2,1,1,2,0.022,1.096,-0.369,0.458,1.242,1.401,2.35e-05,-12.567,-0.331,6.629 } +}; //See the description for the mean data. This table constains the //parameter standard deviations double const Millard2016TorqueMuscle::Anderson2007Table3Std[36][14] = { -{0,0,0,0,0.049,0.201,0.358,0.286,0.586,0.272,0.66,0.97,0.547,4.955 }, -{0,0,1,0,0.047,0.13,0.418,0.268,0.542,0.175,1.93,2.828,0.292,1.895 }, -{0,0,0,1,0.043,0.155,0.195,0.306,0.622,0.189,1.297,2.701,0.091,0.854 }, -{0,0,1,1,0.032,0.246,0.365,0.223,0.45,0.14,1.294,2.541,0.02,2.924 }, -{0,0,0,2,0.039,0.124,0.077,0.184,0.372,0.368,0.271,3.402,0.111,1.691 }, -{0,0,1,2,0.003,0.173,0.279,0.135,0.273,0.237,0.613,0.741,0.031,2.265 }, -{0,1,0,0,0.025,0.217,0.245,0.489,0.995,0.225,0.66,0.97,0.547,4.955 }, -{0,1,1,0,0.033,0.178,0.232,0.345,0.702,0.179,1.93,2.828,0.292,1.895 }, -{0,1,0,1,0.02,0.248,0.274,0.318,0.652,0.088,1.297,2.701,0.091,0.854 }, -{0,1,1,1,0.016,0.244,0.209,0.375,0.765,0.262,1.294,2.541,0.02,2.924 }, -{0,1,0,2,0.025,0.151,0.234,0.164,0.335,0.102,0.271,3.402,0.111,1.691 }, -{0,1,1,2,0.008,0.062,0.214,0.321,0.654,0.28,0.613,0.741,0.031,2.265 }, -{1,0,0,0,0.04,0.073,0.073,0.593,1.546,0.171,0,0,2.617,0.553 }, -{1,0,1,0,0.028,0.084,0.181,0.38,0.989,0.27,0,0,3.696,1.512 }, -{1,0,0,1,0.031,0.063,0.048,0.363,0.947,0.06,0,0,2.541,2.148 }, -{1,0,1,1,0.016,0.094,0.077,0.319,0.832,0.133,0,0,1.958,1.63 }, -{1,0,0,2,0.017,0.127,0.024,0.046,0.04,0.124,0,0,1.896,1.517 }, -{1,0,1,2,0.018,0.044,0.124,0.128,0.221,0.129,0,0,6.141,0.742 }, -{1,1,0,0,0.015,0.163,0.317,1.364,3.554,0.598,0,0,2.617,0.553 }, -{1,1,1,0,0.015,0.191,0.287,0.825,2.139,0.319,0,0,3.696,1.512 }, -{1,1,0,1,0.017,0.138,0.212,0.795,2.067,0.094,0,0,2.541,2.148 }, -{1,1,1,1,0.015,0.21,0.273,0.718,1.871,0.143,0,0,1.958,1.63 }, -{1,1,0,2,0.022,0.084,0.357,0.716,1.866,0.201,0,0,1.896,1.517 }, -{1,1,1,2,0.005,0.145,0.21,0.052,0.135,0.078,0,0,6.141,0.742 }, -{2,0,0,0,0.022,0.089,0.083,0.595,2.144,0.214,0.001193,7.384,0.323,1.196 }, -{2,0,1,0,0.034,0.19,0.186,0.487,1.76,0.213,0.01135,6.77,0.328,1.177 }, -{2,0,0,1,0.029,0.136,0.103,0.165,0.578,0.133,0.003331,10.291,0.403,3.119 }, -{2,0,1,1,0.026,0.235,0.143,0.448,1.613,0.27,3.562e-05,7.848,0.394,1.141 }, -{2,0,0,2,0.035,0.136,0.132,0.262,0.944,0.313,3.164e-05,1.786,0.258,0.902 }, -{2,0,1,2,0.006,0.095,0.115,0.258,0.423,0.158,2.535e-05,10.885,0.247,2.186}, -{2,1,0,0,0.005,0.19,0.067,0.108,0.301,0.134,0.001193,7.384,0.323,1.196 }, -{2,1,1,0,0.006,0.271,0.171,0.446,1.236,0.206,0.01135,6.77,0.328,1.177 }, -{2,1,0,1,0.005,0.479,0.178,0.216,0.601,0.214,0.003331,10.291,0.403,3.119 }, -{2,1,1,1,0.002,0.339,0.133,0.148,0.41,0.284,3.562e-05,7.848,0.394,1.141 }, -{2,1,0,2,0.002,0.195,0.056,0.188,0.521,0.29,3.164e-05,1.786,0.258,0.902 }, -{2,1,1,2,0.003,0.297,0.109,0.089,0.213,0.427,2.535e-05,10.885,0.247,2.186}}; + {0,0,0,0,0.049,0.201,0.358,0.286,0.586,0.272,0.66,0.97,0.547,4.955 }, + {0,0,1,0,0.047,0.13,0.418,0.268,0.542,0.175,1.93,2.828,0.292,1.895 }, + {0,0,0,1,0.043,0.155,0.195,0.306,0.622,0.189,1.297,2.701,0.091,0.854 }, + {0,0,1,1,0.032,0.246,0.365,0.223,0.45,0.14,1.294,2.541,0.02,2.924 }, + {0,0,0,2,0.039,0.124,0.077,0.184,0.372,0.368,0.271,3.402,0.111,1.691 }, + {0,0,1,2,0.003,0.173,0.279,0.135,0.273,0.237,0.613,0.741,0.031,2.265 }, + {0,1,0,0,0.025,0.217,0.245,0.489,0.995,0.225,0.66,0.97,0.547,4.955 }, + {0,1,1,0,0.033,0.178,0.232,0.345,0.702,0.179,1.93,2.828,0.292,1.895 }, + {0,1,0,1,0.02,0.248,0.274,0.318,0.652,0.088,1.297,2.701,0.091,0.854 }, + {0,1,1,1,0.016,0.244,0.209,0.375,0.765,0.262,1.294,2.541,0.02,2.924 }, + {0,1,0,2,0.025,0.151,0.234,0.164,0.335,0.102,0.271,3.402,0.111,1.691 }, + {0,1,1,2,0.008,0.062,0.214,0.321,0.654,0.28,0.613,0.741,0.031,2.265 }, + {1,0,0,0,0.04,0.073,0.073,0.593,1.546,0.171,0,0,2.617,0.553 }, + {1,0,1,0,0.028,0.084,0.181,0.38,0.989,0.27,0,0,3.696,1.512 }, + {1,0,0,1,0.031,0.063,0.048,0.363,0.947,0.06,0,0,2.541,2.148 }, + {1,0,1,1,0.016,0.094,0.077,0.319,0.832,0.133,0,0,1.958,1.63 }, + {1,0,0,2,0.017,0.127,0.024,0.046,0.04,0.124,0,0,1.896,1.517 }, + {1,0,1,2,0.018,0.044,0.124,0.128,0.221,0.129,0,0,6.141,0.742 }, + {1,1,0,0,0.015,0.163,0.317,1.364,3.554,0.598,0,0,2.617,0.553 }, + {1,1,1,0,0.015,0.191,0.287,0.825,2.139,0.319,0,0,3.696,1.512 }, + {1,1,0,1,0.017,0.138,0.212,0.795,2.067,0.094,0,0,2.541,2.148 }, + {1,1,1,1,0.015,0.21,0.273,0.718,1.871,0.143,0,0,1.958,1.63 }, + {1,1,0,2,0.022,0.084,0.357,0.716,1.866,0.201,0,0,1.896,1.517 }, + {1,1,1,2,0.005,0.145,0.21,0.052,0.135,0.078,0,0,6.141,0.742 }, + {2,0,0,0,0.022,0.089,0.083,0.595,2.144,0.214,0.001193,7.384,0.323,1.196 }, + {2,0,1,0,0.034,0.19,0.186,0.487,1.76,0.213,0.01135,6.77,0.328,1.177 }, + {2,0,0,1,0.029,0.136,0.103,0.165,0.578,0.133,0.003331,10.291,0.403,3.119 }, + {2,0,1,1,0.026,0.235,0.143,0.448,1.613,0.27,3.562e-05,7.848,0.394,1.141 }, + {2,0,0,2,0.035,0.136,0.132,0.262,0.944,0.313,3.164e-05,1.786,0.258,0.902 }, + {2,0,1,2,0.006,0.095,0.115,0.258,0.423,0.158,2.535e-05,10.885,0.247,2.186}, + {2,1,0,0,0.005,0.19,0.067,0.108,0.301,0.134,0.001193,7.384,0.323,1.196 }, + {2,1,1,0,0.006,0.271,0.171,0.446,1.236,0.206,0.01135,6.77,0.328,1.177 }, + {2,1,0,1,0.005,0.479,0.178,0.216,0.601,0.214,0.003331,10.291,0.403,3.119 }, + {2,1,1,1,0.002,0.339,0.133,0.148,0.41,0.284,3.562e-05,7.848,0.394,1.141 }, + {2,1,0,2,0.002,0.195,0.056,0.188,0.521,0.29,3.164e-05,1.786,0.258,0.902 }, + {2,1,1,2,0.003,0.297,0.109,0.089,0.213,0.427,2.535e-05,10.885,0.247,2.186} +}; /* This table contains parameters for the newly made torque muscle curves: @@ -237,14 +264,14 @@ double const Millard2016TorqueMuscle::Anderson2007Table3Std[36][14] = { 5. tvAtMaxEccentricVelocity Nm/Nm 6. tvAtHalfMaxConcentricVelocity Nm/Nm 7. angleAtZeroPassiveTorque rad - 8. angleAtOneNormPassiveTorque rad + 8. mAngleAtOneNormPassiveTorque rad */ -double const Millard2016TorqueMuscle::GymnastWholeBody[22][12] = -{ {0,0,0,0,175.746, 9.02335, 1.06465, 1.05941, 1.1, 0.163849, 0.79158, 1.5708 }, +double const Millard2016TorqueMuscle::GymnastWholeBody[24][12] = { + {0,0,0,0,175.746, 9.02335, 1.06465, 1.05941, 1.1, 0.163849, 0.79158, 1.5708 }, {0,1,0,0,157.293, 9.18043, 0.733038, 1.21999, 1.11905, 0.25, -0.0888019,-0.515207 }, {1,0,0,0,285.619, 19.2161, 0.942478, 0.509636, 1.13292, 0.115304, 2.00713, 2.70526 }, {1,1,0,0,98.7579, 16.633, 1.02974, 1.11003, 1.12, 0.19746, 0, -0.174533 }, @@ -255,17 +282,20 @@ double const Millard2016TorqueMuscle::GymnastWholeBody[22][12] = {5,0,0,0,15.5653, 36.5472, 1.55334, 1.38928, 1.16875, 0.115356, 1.0472, 1.5708 }, {5,1,0,0,39.2252, 36.3901, 0.663225, 1.71042, 1.14, 0.115456, -0.793739, -1.49714 }, {3,2,0,0,128.496, 18.05, 0.839204, 1.28041, 1.25856, 0.214179, 1.5708, 2.26893 }, - {3,3,0,0,94.6839, 18 , -0.277611, 2.37086, 1.23042, 0.224227, -0.523599, -1.0472 }, - {3,5,0,0,50.522 , 19.47, -1.18761, 2.80524, 1.27634, 0.285399, 1.39626, 1.91986 }, + {3,3,0,0,94.6839, 18, -0.277611, 2.37086, 1.23042, 0.224227, -0.523599, -1.0472 }, + {3,5,0,0,50.522, 19.47, -1.18761, 2.80524, 1.27634, 0.285399, 1.39626, 1.91986 }, {3,4,0,0,43.5837, 18, -0.670796, 1.98361, 1.35664, 0.229104, -1.0472, -1.5708 }, {4,1,0,0,101.384, 18.1, 0.33, 3.62155, 1.37223, 0.189909, 0, -0.174533 }, {4,0,0,0,69.8728, 18.45, 1.64319, 1.30795, 1.31709, 0.189676, 2.0944, 2.61799 }, {5,6,0,0,13.5361, 35.45, -0.209204, 1.33735, 1.23945, 0.250544, -0.785398, -1.5708 }, - {5,7,0,0,12.976 , 27.88, -0.212389, 0.991803, 1.3877, 0.207506, 0.785398, 1.5708 }, + {5,7,0,0,12.976, 27.88, -0.212389, 0.991803, 1.3877, 0.207506, 0.785398, 1.5708 }, {5,9,0,0,31.4217, 18.02, 0.43, 1.47849, 1.34817, 0.196913, 0, -0.523599}, {5,8,0,0,23.8345, 21.77, -1.14319, 2.56082, 1.31466, 0.2092, 0.349066, 0.872665 }, {6,0,0,0,687.864, 7.98695, 1.5506, 1.14543, 1.1, 0.150907, 0.306223, 1.35342 }, - {6,1,0,0,211.65 , 19.2310, 0, 6.28319, 1.1, 0.150907, 0, -0.785398 }}; + {6,1,0,0,211.65, 19.2310, 0, 6.28319, 1.1, 0.150907, 0, -0.785398 }, + {7,0,0,0, 1., 1., 0., 1., 1.1, 0.25, 0., 1. }, + {7,1,0,0, 1., 1., 0., 1., 1.1, 0.25, 0., -1. }, +}; /* @@ -279,7 +309,7 @@ double const Millard2016TorqueMuscle::GymnastWholeBody[22][12] = the pair of joint and direction indicies used in the tables *************************************************************/ -const static struct JointSet{ +const static struct JointSet { enum item { Hip = 0, Knee, Ankle, @@ -287,52 +317,57 @@ const static struct JointSet{ Elbow, Wrist, Lumbar, - Last}; - JointSet(){} + Generic, + Last + }; + JointSet() {} } JointSet; -struct DirectionSet{ +struct DirectionSet { enum item { - Extension = 0, - Flexion, - HorizontalAdduction, - HorizontalAbduction, - ExternalRotation, - InternalRotation, - Supination, - Pronation, - RadialDeviation, - UlnarDeviation, - Last + Extension = 0, + Flexion, + HorizontalAdduction, + HorizontalAbduction, + ExternalRotation, + InternalRotation, + Supination, + Pronation, + RadialDeviation, + UlnarDeviation, + Last }; - DirectionSet(){} + DirectionSet() {} } DirectionSet; -const static int JointTorqueMap[22][3] = { - {(int)JointTorqueSet::HipExtension , (int)JointSet::Hip , (int)DirectionSet::Extension }, - {(int)JointTorqueSet::HipFlexion , (int)JointSet::Hip , (int)DirectionSet::Flexion }, - {(int)JointTorqueSet::KneeExtension , (int)JointSet::Knee , (int)DirectionSet::Extension }, - {(int)JointTorqueSet::KneeFlexion , (int)JointSet::Knee , (int)DirectionSet::Flexion }, - {(int)JointTorqueSet::AnkleExtension , (int)JointSet::Ankle , (int)DirectionSet::Extension }, - {(int)JointTorqueSet::AnkleFlexion , (int)JointSet::Ankle , (int)DirectionSet::Flexion }, - {(int)JointTorqueSet::ElbowExtension , (int)JointSet::Elbow , (int)DirectionSet::Extension }, - {(int)JointTorqueSet::ElbowFlexion , (int)JointSet::Elbow , (int)DirectionSet::Flexion }, - {(int)JointTorqueSet::ShoulderExtension , (int)JointSet::Shoulder , (int)DirectionSet::Extension }, - {(int)JointTorqueSet::ShoulderFlexion , (int)JointSet::Shoulder , (int)DirectionSet::Flexion }, - {(int)JointTorqueSet::WristExtension , (int)JointSet::Wrist , (int)DirectionSet::Extension }, - {(int)JointTorqueSet::WristFlexion , (int)JointSet::Wrist , (int)DirectionSet::Flexion }, - {(int)JointTorqueSet::ShoulderHorizontalAdduction , (int)JointSet::Shoulder , (int)DirectionSet::HorizontalAdduction}, - {(int)JointTorqueSet::ShoulderHorizontalAbduction , (int)JointSet::Shoulder , (int)DirectionSet::HorizontalAbduction}, - {(int)JointTorqueSet::ShoulderInternalRotation , (int)JointSet::Shoulder , (int)DirectionSet::InternalRotation }, - {(int)JointTorqueSet::ShoulderExternalRotation , (int)JointSet::Shoulder , (int)DirectionSet::ExternalRotation }, - {(int)JointTorqueSet::WristUlnarDeviation , (int)JointSet::Wrist , (int)DirectionSet::UlnarDeviation }, - {(int)JointTorqueSet::WristRadialDeviation , (int)JointSet::Wrist , (int)DirectionSet::RadialDeviation }, - {(int)JointTorqueSet::WristPronation , (int)JointSet::Wrist , (int)DirectionSet::Pronation }, - {(int)JointTorqueSet::WristSupination , (int)JointSet::Wrist , (int)DirectionSet::Supination }, - {(int)JointTorqueSet::LumbarExtension , (int)JointSet::Lumbar , (int)DirectionSet::Extension }, - {(int)JointTorqueSet::LumbarFlexion , (int)JointSet::Lumbar , (int)DirectionSet::Flexion }}; +const static int JointTorqueMap[24][3] = { + {(int)JointTorqueSet::HipExtension, (int)JointSet::Hip, (int)DirectionSet::Extension }, + {(int)JointTorqueSet::HipFlexion, (int)JointSet::Hip, (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::KneeExtension, (int)JointSet::Knee, (int)DirectionSet::Extension }, + {(int)JointTorqueSet::KneeFlexion, (int)JointSet::Knee, (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::AnkleExtension, (int)JointSet::Ankle, (int)DirectionSet::Extension }, + {(int)JointTorqueSet::AnkleFlexion, (int)JointSet::Ankle, (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::ElbowExtension, (int)JointSet::Elbow, (int)DirectionSet::Extension }, + {(int)JointTorqueSet::ElbowFlexion, (int)JointSet::Elbow, (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::ShoulderExtension, (int)JointSet::Shoulder, (int)DirectionSet::Extension }, + {(int)JointTorqueSet::ShoulderFlexion, (int)JointSet::Shoulder, (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::WristExtension, (int)JointSet::Wrist, (int)DirectionSet::Extension }, + {(int)JointTorqueSet::WristFlexion, (int)JointSet::Wrist, (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::ShoulderHorizontalAdduction, (int)JointSet::Shoulder, (int)DirectionSet::HorizontalAdduction}, + {(int)JointTorqueSet::ShoulderHorizontalAbduction, (int)JointSet::Shoulder, (int)DirectionSet::HorizontalAbduction}, + {(int)JointTorqueSet::ShoulderInternalRotation, (int)JointSet::Shoulder, (int)DirectionSet::InternalRotation }, + {(int)JointTorqueSet::ShoulderExternalRotation, (int)JointSet::Shoulder, (int)DirectionSet::ExternalRotation }, + {(int)JointTorqueSet::WristUlnarDeviation, (int)JointSet::Wrist, (int)DirectionSet::UlnarDeviation }, + {(int)JointTorqueSet::WristRadialDeviation, (int)JointSet::Wrist, (int)DirectionSet::RadialDeviation }, + {(int)JointTorqueSet::WristPronation, (int)JointSet::Wrist, (int)DirectionSet::Pronation }, + {(int)JointTorqueSet::WristSupination, (int)JointSet::Wrist, (int)DirectionSet::Supination }, + {(int)JointTorqueSet::LumbarExtension, (int)JointSet::Lumbar, (int)DirectionSet::Extension }, + {(int)JointTorqueSet::LumbarFlexion, (int)JointSet::Lumbar, (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::UnitExtensor, (int)JointSet::Generic, (int)DirectionSet::Extension }, + {(int)JointTorqueSet::UnitFlexor, (int)JointSet::Generic, (int)DirectionSet::Flexion } +}; /************************************************************* @@ -340,216 +375,238 @@ const static int JointTorqueMap[22][3] = { *************************************************************/ Millard2016TorqueMuscle:: - Millard2016TorqueMuscle( ) - :angleOffset(1.0), - signOfJointAngle(1.0), - signOfJointTorque(1.0), - signOfConcentricAnglularVelocity(1.0), - muscleName("empty") +Millard2016TorqueMuscle( ) + :mAngleOffset(1.0), + mSignOfJointAngle(1.0), + mSignOfJointTorque(1.0), + mSignOfConcentricAnglularVelocity(1.0), + mMuscleName("empty") { - muscleCurvesAreDirty = true; - useTabularMaxActiveIsometricTorque = true; - useTabularOmegaMax = true; - passiveTorqueScale = 1.0; - + mMuscleCurvesAreDirty = true; + mUseTabularMaxActiveIsometricTorque = true; + mUseTabularOmegaMax = true; + mPassiveTorqueScale = 1.0; + + mTaLambda = 0.0; + mTpLambda = 0.0; + mTvLambda = 0.0; + mTvLambdaMax = 1.0; + mTaAngleScaling = 1.0; } Millard2016TorqueMuscle::Millard2016TorqueMuscle( - DataSet::item dataSet, - const SubjectInformation &subjectInfo, - int jointTorque, - double jointAngleOffsetRelativeToDoxygenFigures, - double signOfJointAngleRelativeToDoxygenFigures, - double signOfJointTorque, - const std::string& name - ):angleOffset(jointAngleOffsetRelativeToDoxygenFigures), - signOfJointAngle(signOfJointAngleRelativeToDoxygenFigures), - signOfJointTorque(signOfJointTorque), - signOfConcentricAnglularVelocity(signOfJointTorque), - muscleName(name), - dataSet(dataSet) + DataSet::item dataSet, + const SubjectInformation &subjectInfo, + int jointTorque, + double jointAngleOffsetRelativeToDoxygenFigures, + double signOfJointAngleRelativeToDoxygenFigures, + double mSignOfJointTorque, + const std::string& name +):mAngleOffset(jointAngleOffsetRelativeToDoxygenFigures), + mSignOfJointAngle(signOfJointAngleRelativeToDoxygenFigures), + mSignOfJointTorque(mSignOfJointTorque), + mSignOfConcentricAnglularVelocity(mSignOfJointTorque), + mMuscleName(name), + mDataSet(dataSet) { - subjectHeightInMeters = subjectInfo.heightInMeters; - subjectMassInKg = subjectInfo.massInKg; - passiveCurveAngleOffset = 0.; - passiveTorqueScale = 1.0; - betaMax = 0.1; + mSubjectHeightInMeters = subjectInfo.heightInMeters; + mSubjectMassInKg = subjectInfo.massInKg; + mPassiveCurveAngleOffset = 0.; + mPassiveTorqueScale = 1.0; + mBetaMax = 0.1; - int gender = (int) subjectInfo.gender; - int ageGroup = (int) subjectInfo.ageGroup; + int gender = (int) subjectInfo.gender; + int ageGroup = (int) subjectInfo.ageGroup; - int joint = -1; - int jointDirection = -1; + mGender = subjectInfo.gender; + mAgeGroup = subjectInfo.ageGroup; - for(int i=0; i < JointTorqueSet::Last; ++i){ - if(JointTorqueMap[i][0] == jointTorque){ - joint = JointTorqueMap[i][1]; - jointDirection = JointTorqueMap[i][2]; - } - } + int joint = -1; + int jointDirection = -1; - if(joint == -1 || jointDirection == -1){ - cerr << "Millard2016TorqueMuscle::" - << "Millard2016TorqueMuscle:" - << muscleName - << ": A jointTorqueDirection of " << jointTorque - << " does not exist."; - assert(0); - abort(); + for(int i=0; i < JointTorqueSet::Last; ++i) { + if(JointTorqueMap[i][0] == jointTorque) { + mJointTorque = JointTorqueSet::item(i); + joint = JointTorqueMap[i][1]; + jointDirection = JointTorqueMap[i][2]; } + } - if( abs(abs(signOfJointAngle)-1) > EPSILON){ + if(joint == -1 || jointDirection == -1) { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << ": A jointTorqueDirection of " << jointTorque + << " does not exist."; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); + } - cerr << "Millard2016TorqueMuscle::" - << "Millard2016TorqueMuscle:" - << muscleName - << ": signOfJointAngleRelativeToAnderson2007 must be [-1,1] not " - << signOfJointAngle; - assert(0); - abort(); - } + if( abs(abs(mSignOfJointAngle)-1) > EPSILON) { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << ": signOfJointAngleRelativeToAnderson2007 must be [-1,1] not " + << mSignOfJointAngle; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); + } - if( abs(abs(signOfConcentricAnglularVelocity)-1) > EPSILON){ - cerr << "Millard2016TorqueMuscle::" - << "Millard2016TorqueMuscle:" - << muscleName - << ": signOfJointAngularVelocityDuringConcentricContraction " - << "must be [-1,1] not " - << signOfConcentricAnglularVelocity; - assert(0); - abort(); - } + if( abs(abs(mSignOfConcentricAnglularVelocity)-1) > EPSILON) { + ostringstream errormsg; + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << ": signOfJointAngularVelocityDuringConcentricContraction " + << "must be [-1,1] not " + << mSignOfConcentricAnglularVelocity; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); + } - if( abs(abs(signOfJointTorque)-1) > EPSILON){ - cerr << "Millard2016TorqueMuscle::" - << "Millard2016TorqueMuscle:" - << muscleName - << ": signOfJointTorque must be [-1,1] not " - << signOfJointTorque; - assert(0); - abort(); - } + if( abs(abs(mSignOfJointTorque)-1) > EPSILON) { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << ": mSignOfJointTorque must be [-1,1] not " + << mSignOfJointTorque; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); + } - if(subjectHeightInMeters <= 0){ - cerr << "Millard2016TorqueMuscle::" - << "Millard2016TorqueMuscle:" - << muscleName - << ": subjectHeightInMeters > 0, but it's " - << subjectHeightInMeters; - assert(0); - abort(); + if(mSubjectHeightInMeters <= 0) { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << ": mSubjectHeightInMeters > 0, but it's " + << mSubjectHeightInMeters; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); + } + + if(mSubjectMassInKg <= 0) { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << ": mSubjectMassInKg > 0, but it's " + << mSubjectMassInKg; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); + } + + + + int idx = -1; + int jointIdx = 0; + int dirIdx = 1; + int genderIdx = 2; + int ageIdx = 3; + + switch(mDataSet) { + case DataSet::Anderson2007: { + mAnderson2007c1c2c3c4c5c6.resize(6); + mAnderson2007b1k1b2k2.resize(4); + + for(int i=0; i<36; ++i) { + + if( abs(Anderson2007Table3Mean[i][jointIdx] + -(double)joint) < EPSILON + && abs(Anderson2007Table3Mean[i][dirIdx] + -(double)jointDirection) < EPSILON + && abs(Anderson2007Table3Mean[i][genderIdx] + -(double)gender) < EPSILON + && abs(Anderson2007Table3Mean[i][ageIdx] + -(double)ageGroup) < EPSILON) { + idx = i; + } } - if(subjectMassInKg <= 0){ - cerr << "Millard2016TorqueMuscle::" - << "Millard2016TorqueMuscle:" - << muscleName - << ": subjectMassInKg > 0, but it's " - << subjectMassInKg; - assert(0); - abort(); + if(idx != -1) { + for(int i=0; i<6; ++i) { + mAnderson2007c1c2c3c4c5c6[i] = Anderson2007Table3Mean[idx][i+4]; + } + for(int i=0; i<4; ++i) { + mAnderson2007b1k1b2k2[i] = Anderson2007Table3Mean[idx][i+10]; + } } + } + break; + + case DataSet::Gymnast: { + mGymnastParams.resize(8); + for(int i=0; i(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + updTorqueMuscleSummary(activation, + jointAngle,jointAngularVelocity, + mTaLambda,mTpLambda,mTvLambda, + mTaAngleScaling, mAngleAtOneNormActiveTorque, + mPassiveCurveAngleOffset, + mOmegaMax, + mMaxActiveIsometricTorque, + mutableThis->mTmSummary); + + return mTmSummary.jointTorque; +} - double fiberAngle = calcFiberAngle(jointAngle); - double fiberVelocity = calcFiberAngularVelocity( - jointAngularVelocity); - double ta = taCurve.calcValue(fiberAngle); - double tp = tpCurve.calcValue(fiberAngle); - double fiberVelocityNorm = fiberVelocity/omegaMax; - double tv = tvCurve.calcValue(fiberVelocityNorm); - //double beta = betaMax*betaCurve.calcValue(fiberVelocityNorm); - double jointTorque = maxActiveIsometricTorque*( - activation*ta*tv - + tp*(1 - betaMax*fiberVelocityNorm)); - - return jointTorque*signOfJointTorque; -} void Millard2016TorqueMuscle:: - calcActivation(double jointAngle, - double jointAngularVelocity, - double jointTorque, - TorqueMuscleSummary &tms) const +calcActivation(double jointAngle, + double jointAngularVelocity, + double jointTorque, + TorqueMuscleSummary &tms) const { - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - double activation = 0.; - - tms.activation = activation; - tms.fiberActiveTorqueAngleMultiplier = 0.; - tms.fiberActiveTorqueAngularVelocityMultiplier = 0.; - tms.fiberNormalizedDampingTorque = 0.; - tms.fiberPassiveTorqueAngleMultiplier = 0.; - tms.fiberTorque = 0.; - tms.jointTorque = 0.; - - if(jointTorque*signOfJointTorque > SQRTEPSILON){ - double fiberAngle = calcFiberAngle(jointAngle); - double fiberVelocity = calcFiberAngularVelocity( - jointAngularVelocity); - double ta = taCurve.calcValue(fiberAngle); - double tp = tpCurve.calcValue(fiberAngle); - double fiberVelocityNorm = fiberVelocity/omegaMax; - double tv = tvCurve.calcValue(fiberVelocityNorm); - //double beta = betaMax*betaCurve.calcValue(fiberVelocityNorm); - - double fiberTorque = jointTorque*signOfJointTorque; - - /* - double jointTorque = maxActiveIsometricTorque*( - activation*ta*tv - + tp - - beta*fiberVelocityNorm); - */ - activation = ((fiberTorque/maxActiveIsometricTorque) - - tp*(1-betaMax*fiberVelocityNorm)) - /(ta*tv); - - tms.activation = activation; - tms.fiberActiveTorqueAngleMultiplier = ta; - tms.fiberActiveTorqueAngularVelocityMultiplier = tv; - tms.fiberNormalizedDampingTorque = -tp*betaMax*fiberVelocityNorm; - tms.fiberPassiveTorqueAngleMultiplier = tp; - tms.fiberTorque = fiberTorque; - tms.jointTorque = jointTorque; - - } - - + updInvertTorqueMuscleSummary(jointTorque,jointAngle,jointAngularVelocity, + mTaLambda,mTpLambda,mTvLambda, + mTaAngleScaling, + mAngleAtOneNormActiveTorque, + mPassiveCurveAngleOffset, + mOmegaMax, + mMaxActiveIsometricTorque, + tms); } double Millard2016TorqueMuscle:: - calcMaximumActiveIsometricTorqueScalingFactor( - double jointAngle, - double jointAngularVelocity, - double activation, - double jointTorque) const +calcMaximumActiveIsometricTorqueScalingFactor( + double jointAngle, + double jointAngularVelocity, + double activation, + double jointTorque) const { - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } double scaleFactor = 0.; - if(jointTorque*signOfJointTorque > SQRTEPSILON){ - double fiberAngle = calcFiberAngle(jointAngle); - double fiberVelocity = calcFiberAngularVelocity( - jointAngularVelocity); - double ta = taCurve.calcValue(fiberAngle); - double tp = tpCurve.calcValue(fiberAngle); - double fiberVelocityNorm = fiberVelocity/omegaMax; - double tv = tvCurve.calcValue(fiberVelocityNorm); - //double beta = betaMax*betaCurve.calcValue(fiberVelocityNorm); + Millard2016TorqueMuscle* mutableThis = + const_cast(this); - double fiberTorqueUpd = jointTorque*signOfJointTorque; + updTorqueMuscleSummary(activation, + jointAngle,jointAngularVelocity, + mTaLambda,mTpLambda,mTvLambda, + mTaAngleScaling, + mAngleAtOneNormActiveTorque, + mPassiveCurveAngleOffset, + mOmegaMax, + mMaxActiveIsometricTorque, + mutableThis->mTmSummary); - double maxActiveIsometricTorqueUpd = - fiberTorqueUpd/( activation*ta*tv - + tp*(1-betaMax*fiberVelocityNorm)); + scaleFactor = jointTorque/mTmSummary.jointTorque; - - scaleFactor = maxActiveIsometricTorqueUpd - /maxActiveIsometricTorque; - } return scaleFactor; } void Millard2016TorqueMuscle:: - calcTorqueMuscleInfo(double jointAngle, - double jointAngularVelocity, - double activation, - TorqueMuscleInfo& tmi) const +calcTorqueMuscleInfo(double jointAngle, + double jointAngularVelocity, + double activation, + TorqueMuscleInfo& tmi) const { - - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - double fiberAngle = calcFiberAngle(jointAngle); - double fiberAngularVelocity = calcFiberAngularVelocity(jointAngularVelocity); - double ta = taCurve.calcValue(fiberAngle); - double tp = tpCurve.calcValue(fiberAngle); - - double omegaNorm = fiberAngularVelocity/omegaMax; - double D_wn_w = 1.0/omegaMax; - double tv = tvCurve.calcValue(omegaNorm); - - double beta = betaMax;//betaMax*betaCurve.calcValue(omegaNorm); - double D_beta_D_wn = 0; - - double tb = -tp*beta*omegaNorm; - double D_tb_Dwn = -tp*beta;//-beta - D_beta_D_wn*omegaNorm; - - double D_ta_DfiberAngle = taCurve.calcDerivative(fiberAngle,1); - double D_tp_DfiberAngle = tpCurve.calcDerivative(fiberAngle,1); - double D_tv_DfiberAngularVelocity - = tvCurve.calcDerivative(omegaNorm,1)*D_wn_w; - double D_tb_DfiberAngularVelocity = D_tb_Dwn*D_wn_w; - double D_tb_DfiberAngle = -D_tp_DfiberAngle*beta*omegaNorm; - - double D_fiberAngle_D_jointAngle = signOfJointAngle; - double D_tv_DfiberAngularVelocity_D_jointAngularVelocity = - signOfConcentricAnglularVelocity; - - - tmi.jointAngle = jointAngle; - tmi.jointAngularVelocity = jointAngularVelocity; - tmi.fiberAngle = fiberAngle; - //tmi.tendonAngle = 0.; - tmi.fiberAngularVelocity = fiberAngularVelocity; - //tmi.tendonAngularVelocity = 0.; - - tmi.fiberPassiveTorqueAngleMultiplier = tp; - tmi.fiberActiveTorqueAngleMultiplier = ta; - tmi.fiberActiveTorqueAngularVelocityMultiplier = tv; - //tmi.tendonTorqueAngleMultiplier = activation*ta*tv + tp; - - tmi.activation = activation; - tmi.fiberActiveTorque = maxActiveIsometricTorque*(activation*ta*tv); - tmi.fiberPassiveTorque = maxActiveIsometricTorque*(tb+tp); - tmi.fiberDampingTorque = maxActiveIsometricTorque*(tb); - tmi.fiberNormDampingTorque = tb; - tmi.fiberTorque = tmi.fiberActiveTorque - + tmi.fiberPassiveTorque; - //tmi.tendonTorque = tmi.fiberActiveTorque - // + tmi.fiberPassiveTorque; - - tmi.jointTorque = signOfJointTorque*tmi.fiberTorque; - - tmi.fiberStiffness = maxActiveIsometricTorque*( - activation*D_ta_DfiberAngle*tv - + D_tp_DfiberAngle); - - //tmi.tendonStiffness = std::numeric_limits::infinity(); - tmi.jointStiffness = signOfJointTorque - *tmi.fiberStiffness - *D_fiberAngle_D_jointAngle; - - tmi.fiberActivePower = tmi.fiberActiveTorque - * tmi.fiberAngularVelocity; - tmi.fiberPassivePower = tmi.fiberPassiveTorque - * tmi.fiberAngularVelocity; - tmi.fiberPower = tmi.fiberActivePower - + tmi.fiberPassivePower; - //tmi.tendonPower = 0.; - tmi.jointPower = tmi.jointTorque * jointAngularVelocity; - - - //tau = signTq*tauIso*(a * ta(theta) * tv(thetaDot) + tp(theta) ) - // Dtau_Da = tauMaxIso*(ta(theta) * tv(thetaDot) ) - tmi.DjointTorqueDactivation = - signOfJointTorque - *maxActiveIsometricTorque - *(ta * tv); - - //Dtau_Domega = signTq*tauIso*(a * ta(theta) * Dtv_DthetaDot(thetaDot)* ) - tmi.DjointTorqueDjointAngularVelocity = - signOfJointTorque - * maxActiveIsometricTorque - * ( activation - * ta - * D_tv_DfiberAngularVelocity - * D_tv_DfiberAngularVelocity_D_jointAngularVelocity - + D_tb_DfiberAngularVelocity); - - tmi.DjointTorqueDjointAngle = - signOfJointTorque - * maxActiveIsometricTorque - * ( activation - *D_ta_DfiberAngle - * tv - + D_tp_DfiberAngle - + D_tb_DfiberAngle - )* D_fiberAngle_D_jointAngle; + updTorqueMuscleInfo(activation, jointAngle, jointAngularVelocity, + mTaLambda,mTpLambda,mTvLambda, + mTaAngleScaling, mAngleAtOneNormActiveTorque, + mPassiveCurveAngleOffset, + mOmegaMax, + mMaxActiveIsometricTorque, + tmi); } @@ -804,425 +745,556 @@ void Millard2016TorqueMuscle:: *************************************************************/ double Millard2016TorqueMuscle:: - getJointTorqueSign() const +getJointTorqueSign() const { - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - return signOfJointTorque; + return mSignOfJointTorque; } double Millard2016TorqueMuscle:: - getJointAngleSign() const +getJointAngleSign() const { - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - return signOfJointAngle; + return mSignOfJointAngle; } double Millard2016TorqueMuscle:: - getJointAngleOffset() const +getJointAngleOffset() const { - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - return angleOffset; + return mAngleOffset; } double Millard2016TorqueMuscle:: - getNormalizedDampingCoefficient() const +getNormalizedDampingCoefficient() const { - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - return betaMax; + return mBetaMax; } void Millard2016TorqueMuscle:: - setNormalizedDampingCoefficient(double betaUpd) +setNormalizedDampingCoefficient(double betaUpd) { - if(betaUpd < 0){ - cerr << "Millard2016TorqueMuscle::" + if(betaUpd < 0) { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" << "setNormalizedDampingCoefficient:" - << muscleName - << "betaMax is " << betaUpd - << " but betaMax must be > 0 " + << mMuscleName + << "mBetaMax is " << betaUpd + << " but mBetaMax must be > 0 " << endl; - assert(0); - abort(); + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - betaMax = betaUpd; + mBetaMax = betaUpd; } double Millard2016TorqueMuscle:: - getMaximumActiveIsometricTorque() const -{ - if(muscleCurvesAreDirty){ +getMaximumActiveIsometricTorque() const +{ + if(mMuscleCurvesAreDirty) { + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + + /* + if(mUseTabularMaxActiveIsometricTorque){ + return mMaxActiveIsometricTorque; + }else{ + return mMaxActiveIsometricTorque*mMaxActiveIsometricMultiplerProduct; + } + */ + return mMaxActiveIsometricTorque; + +} + +double Millard2016TorqueMuscle:: +getMaximumConcentricJointAngularVelocity() const +{ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - return maxActiveIsometricTorque; + return calcJointAngularVelocity( mOmegaMax ); } double Millard2016TorqueMuscle:: - getMaximumConcentricJointAngularVelocity() const +getTorqueVelocityMultiplierAtHalfOmegaMax() const { - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - return calcJointAngularVelocity( omegaMax ); + return mTorqueVelocityMultiplierAtHalfOmegaMax; } + void Millard2016TorqueMuscle:: - setMaximumActiveIsometricTorque(double maxIsoTorque) +setTorqueVelocityMultiplierAtHalfOmegaMax(double tvAtHalfOmegaMax) { - muscleCurvesAreDirty = true; - useTabularMaxActiveIsometricTorque = false; - maxActiveIsometricTorque = maxIsoTorque; -} + if(mDataSet == DataSet::Anderson2007) { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" + << "setTorqueVelocityMultiplierAtHalfOmegaMax:" + << mMuscleName + << " This function is only compatible with the Gymnast dataset" + << " but this muscle is from the Anderson2007 dataset. Switch" + << " data sets or stop using this function."; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); + } + mMuscleCurvesAreDirty = true; + mUseTabularTorqueVelocityMultiplierAtHalfOmegaMax = false; + mTorqueVelocityMultiplierAtHalfOmegaMax = tvAtHalfOmegaMax; +} + void Millard2016TorqueMuscle:: - setMaximumConcentricJointAngularVelocity(double maxAngularVelocity){ - if(fabs(maxAngularVelocity) < SQRTEPSILON){ - cerr << "Millard2016TorqueMuscle::" - << "setMaximumJointAngularVelocity:" - << muscleName - << " The value of maxJointAngularVelocity needs to be greater " - " than sqrt(epsilon), but it is " - << maxAngularVelocity; - assert(0); - abort(); +setMaximumActiveIsometricTorque(double maxIsoTorque) +{ + mMuscleCurvesAreDirty = true; + mUseTabularMaxActiveIsometricTorque = false; + //mMaxActiveIsometricTorqueUserDefined = maxIsoTorque; + mMaxActiveIsometricTorque = maxIsoTorque; +} + +void Millard2016TorqueMuscle:: +setMaximumConcentricJointAngularVelocity(double maxAngularVelocity) +{ + if(fabs(maxAngularVelocity) < SQRTEPSILON) { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" + << "setMaximumJointAngularVelocity:" + << mMuscleName + << " The value of maxJointAngularVelocity needs to be greater " + << " than sqrt(epsilon), but it is " + << maxAngularVelocity; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + mMuscleCurvesAreDirty = true; + mUseTabularOmegaMax = false; + mOmegaMax = fabs(maxAngularVelocity); +} + +double Millard2016TorqueMuscle:: +getJointAngleAtMaximumActiveIsometricTorque() const +{ + if(mMuscleCurvesAreDirty) { + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return calcJointAngle(mAngleAtOneNormActiveTorque); +} + +double Millard2016TorqueMuscle:: +getActiveTorqueAngleCurveWidth() const +{ + if(mMuscleCurvesAreDirty) { + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); } + VectorNd domain = mTaCurve.getCurveDomain(); + double activeTorqueAngleAngleScaling + = getActiveTorqueAngleCurveAngleScaling(); + double width = fabs(domain[1]-domain[0])/activeTorqueAngleAngleScaling; + + return width; - muscleCurvesAreDirty = true; - useTabularOmegaMax = false; - omegaMax = fabs(maxAngularVelocity); } + double Millard2016TorqueMuscle:: - getJointAngleAtMaximumActiveIsometricTorque() const +getJointAngleAtOneNormalizedPassiveIsometricTorque() const { - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - return calcJointAngle(angleAtOneNormActiveTorque); + return calcJointAngle(mAngleAtOneNormPassiveTorque); } double Millard2016TorqueMuscle:: - getJointAngleAtOneNormalizedPassiveIsometricTorque() const +getJointAngleAtSmallestNormalizedPassiveIsometricTorque() const { - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - return calcJointAngle(angleAtOneNormPassiveTorque); + return calcJointAngle(mAngleAtSmallestNormPassiveTorque); } + double Millard2016TorqueMuscle:: - getPassiveTorqueScale() const +getPassiveTorqueScale() const { - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - return passiveTorqueScale; + return mPassiveTorqueScale; } void Millard2016TorqueMuscle:: - setPassiveTorqueScale(double passiveTorqueScaling) +setPassiveTorqueScale(double passiveTorqueScaling) { - muscleCurvesAreDirty = true; - passiveTorqueScale = passiveTorqueScaling; + mMuscleCurvesAreDirty = true; + mPassiveTorqueScale = passiveTorqueScaling; } double Millard2016TorqueMuscle:: - getPassiveCurveAngleOffset() const +getPassiveCurveAngleOffset() const { - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - return passiveCurveAngleOffset; + return mPassiveCurveAngleOffset; } void Millard2016TorqueMuscle:: - setPassiveCurveAngleOffset(double passiveCurveAngleOffsetVal) +setPassiveCurveAngleOffset(double passiveCurveAngleOffsetVal) { - muscleCurvesAreDirty = true; - passiveCurveAngleOffset = passiveCurveAngleOffsetVal; + mMuscleCurvesAreDirty = true; + mPassiveCurveAngleOffset = passiveCurveAngleOffsetVal; } void Millard2016TorqueMuscle:: - fitPassiveCurveAngleOffset(double jointAngleTarget, - double passiveTorqueTarget) +fitPassiveCurveAngleOffset(double jointAngleTarget, + double passiveFiberTorqueTarget) { - muscleCurvesAreDirty = true; - setPassiveCurveAngleOffset(0.0); + mMuscleCurvesAreDirty = true; + setPassiveCurveAngleOffset(0.0); + updateTorqueMuscleCurves(); - if(passiveTorqueTarget < SQRTEPSILON){ - cerr << "Millard2016TorqueMuscle::" + if(passiveFiberTorqueTarget < SQRTEPSILON) { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" << "fitPassiveTorqueScale:" - << muscleName - << ": passiveTorque " << passiveTorqueTarget + << mMuscleName + << ": passiveTorque " << passiveFiberTorqueTarget << " but it should be greater than sqrt(eps)" << endl; - assert(0); - abort(); - } - - //Solve for the fiber angle at which the curve develops - //the desired passiveTorque - double normPassiveTorque = passiveTorqueTarget - /maxActiveIsometricTorque; - //Ge a good initial guess - - VectorNd curveDomain = tpCurve.getCurveDomain(); - double angleRange = abs( curveDomain[1]-curveDomain[0]); - double fiberAngle = 0.5*(curveDomain[0]+curveDomain[1]); - double jointAngleCurr = calcJointAngle(fiberAngle); - - TorqueMuscleInfo tqInfo = TorqueMuscleInfo(); - - calcTorqueMuscleInfo(jointAngleCurr,0.,0.,tqInfo); - double err = tqInfo.fiberPassiveTorqueAngleMultiplier-normPassiveTorque; - double jointAngleLeft = 0; - double jointAngleRight = 0; - double errLeft = 0; - double errRight = 0; - - double h = 0.25*angleRange; - - //Get a good initial guess - necessary because these curves - //can be *very* nonlinear. - int iter = 0; - int iterMax = 10; - int tol = sqrt(SQRTEPSILON); - - while(iter < iterMax && abs(err) > tol){ - jointAngleLeft = jointAngleCurr-h; - jointAngleRight = jointAngleCurr+h; - - calcTorqueMuscleInfo(jointAngleLeft,0.,0.,tqInfo); - errLeft = tqInfo.fiberPassiveTorqueAngleMultiplier - - normPassiveTorque; - - calcTorqueMuscleInfo(jointAngleRight,0.,0.,tqInfo); - errRight = tqInfo.fiberPassiveTorqueAngleMultiplier - - normPassiveTorque; - - if(abs(errLeft) SQRTEPSILON && iter < iterMax){ - calcTorqueMuscleInfo(jointAngleCurr,0.,0.,tqInfo); - err = tqInfo.fiberPassiveTorqueAngleMultiplier - -normPassiveTorque; - derr = signOfJointTorque*tqInfo.DjointTorqueDjointAngle - / maxActiveIsometricTorque ; - delta = -err/derr; - jointAngleCurr += delta; - ++iter; - } + //Solve for the fiber angle at which the curve develops + //the desired passiveTorque + double normPassiveFiberTorque = passiveFiberTorqueTarget + /mMaxActiveIsometricTorque; + double currentFiberAngle = calcFiberAngleGivenNormalizedPassiveTorque( + normPassiveFiberTorque, + mTpLambda, + mPassiveCurveAngleOffset); //0.); - if(abs(err)>SQRTEPSILON){ - cerr << "Millard2016TorqueMuscle::" - << "fitPassiveCurveAngleOffset:" - << muscleName - << ": failed to fit the passive curve offset " - << " such that the curve develops the desired " - << " passive torque at the given joint angle. This" - << " should not be possible - contact the maintainer " - << " of this addon." - << endl; - assert(0); - abort(); - } - double fiberAngleOffset = calcFiberAngle(jointAngleTarget) - - calcFiberAngle(jointAngleCurr); + //double fiberAngleOffset = mPassiveCurveAngleOffset + // +calcFiberAngle(jointAngleTarget) + // -currentFiberAngle; + double fiberAngleOffset = calcFiberAngle(jointAngleTarget) + -currentFiberAngle; - setPassiveCurveAngleOffset(fiberAngleOffset); + setPassiveCurveAngleOffset(fiberAngleOffset); + updateTorqueMuscleCurves(); } void Millard2016TorqueMuscle:: - fitPassiveTorqueScale(double jointAngleTarget, - double passiveTorqueTarget) +fitPassiveTorqueScale(double jointAngleTarget, + double passiveFiberTorqueTarget) { - muscleCurvesAreDirty = true; - setPassiveTorqueScale(1.0); - - VectorNd curveDomain = tpCurve.getCurveDomain(); - VectorNd curveRange = VectorNd(2); - curveRange[0] = tpCurve.calcValue(curveDomain[0]); - curveRange[1] = tpCurve.calcValue(curveDomain[1]); - double fiberAngle = calcFiberAngle(jointAngleTarget); - - if( (fiberAngle < curveDomain[0] && (curveRange[0] < curveRange[1])) - || (fiberAngle > curveDomain[1] && (curveRange[1] < curveRange[0])) ){ - cerr << "Millard2016TorqueMuscle::" - << "fitPassiveTorqueScale:" - << muscleName - << ": joint angle is " << jointAngleTarget - << " but it should be between " - << calcJointAngle(curveDomain[0]) << " and " - << calcJointAngle(curveDomain[1]) - << endl; - assert(0); - abort(); - } + mMuscleCurvesAreDirty = true; + setPassiveTorqueScale(1.0); + updateTorqueMuscleCurves(); - if(passiveTorqueTarget < SQRTEPSILON){ - cerr << "Millard2016TorqueMuscle::" - << "fitPassiveTorqueScale:" - << muscleName - << ": passiveTorque " << passiveTorqueTarget - << " but it should be greater than sqrt(eps)" - << endl; - assert(0); - abort(); - } + double normPassiveFiberTorqueTarget = passiveFiberTorqueTarget + /mMaxActiveIsometricTorque; + double fiberAngle = calcFiberAngle(jointAngleTarget); + double normPassiveFiberTorqueCurrent= + calcBlendedCurveDerivative(fiberAngle-mPassiveCurveAngleOffset, + mTpLambda, mTpLambdaMax, + 0,0, + mTpCurve); + + double passiveTorqueScale = normPassiveFiberTorqueTarget + /normPassiveFiberTorqueCurrent; + + + setPassiveTorqueScale(passiveTorqueScale); + updateTorqueMuscleCurves(); +} - double normPassiveTorque = passiveTorqueTarget/maxActiveIsometricTorque; - int iter = 0; - int iterMax = 10; - double err = SQRTEPSILON*2; - double derr = 0; - double delta= 0; - double scale = 1.0; - setPassiveTorqueScale(scale); - TorqueMuscleInfo tqInfo = TorqueMuscleInfo(); - - while(abs(err) > SQRTEPSILON && iter < iterMax){ - setPassiveTorqueScale(scale); - calcTorqueMuscleInfo(jointAngleTarget,0,0,tqInfo); - err = tqInfo.fiberPassiveTorqueAngleMultiplier - normPassiveTorque; - derr= tqInfo.fiberPassiveTorqueAngleMultiplier/scale; - delta = -err/derr; - scale += delta; - - if(scale < SQRTEPSILON){ - scale = SQRTEPSILON; +void Millard2016TorqueMuscle::calcTorqueMuscleDataFeatures( + RigidBodyDynamics::Math::VectorNd const &jointTorque, + RigidBodyDynamics::Math::VectorNd const &jointAngle, + RigidBodyDynamics::Math::VectorNd const &jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double torqueVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActiveIsometricTorque, + TorqueMuscleDataFeatures &tmf) const +{ + TorqueMuscleSummary tmsCache; + double activation; + double tp; + double minActivation = 1.0; + double maxActivation = 0.; + double maxTp = 0.; + + tmf.isInactive = true; + + for(unsigned int i=0; i 0) { + tmf.isInactive=false; + if(activation <= minActivation) { + minActivation = activation; + tmf.indexOfMinActivation = i; + tmf.summaryAtMinActivation = tmsCache; + } + if(activation >= maxActivation) { + tmf.indexOfMaxActivation = i; + tmf.summaryAtMaxActivation = tmsCache; + maxActivation = activation; } - iter++; } - - if(abs(err) > SQRTEPSILON){ - cerr << "Millard2016TorqueMuscle::" - << "fitPassiveTorqueScale:" - << muscleName - << ": passiveTorqueScale could not be fit to" - << " the data given. See the maintainer of this" - << " addon for help." - << endl; - assert(0); - abort(); + if(tmsCache.fiberPassiveTorqueAngleMultiplier >= maxTp) { + maxTp = tmsCache.fiberPassiveTorqueAngleMultiplier; + tmf.summaryAtMaxPassiveTorqueAngleMultiplier = tmsCache; + tmf.indexOfMaxPassiveTorqueAngleMultiplier = i; } + } } -/* -const RigidBodyDynamics::Math::VectorNd& - Millard2016TorqueMuscle::getParametersc1c2c3c4c5c6() -{ - return c1c2c3c4c5c6Anderson2007; -} -const RigidBodyDynamics::Math::VectorNd& - Millard2016TorqueMuscle::getParametersb1k1b2k2() -{ - return b1k1b2k2Anderson2007; -} -*/ const SmoothSegmentedFunction& Millard2016TorqueMuscle:: - getActiveTorqueAngleCurve() const +getActiveTorqueAngleCurve() const { //This must be updated if the parameters have changed - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - return taCurve; + return mTaCurve; } const SmoothSegmentedFunction& Millard2016TorqueMuscle:: getPassiveTorqueAngleCurve() const { //This must be updated if the parameters have changed - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - return tpCurve; + return mTpCurve; } const SmoothSegmentedFunction& Millard2016TorqueMuscle:: getTorqueAngularVelocityCurve() const { //This must be updated if the parameters have changed - if(muscleCurvesAreDirty){ + if(mMuscleCurvesAreDirty) { Millard2016TorqueMuscle* mutableThis = - const_cast(this); + const_cast(this); mutableThis->updateTorqueMuscleCurves(); } - return tvCurve; + return mTvCurve; } -string Millard2016TorqueMuscle::getName(){ - return muscleName; + +string Millard2016TorqueMuscle::getName() +{ + return mMuscleName; } -void Millard2016TorqueMuscle::setName(string &name){ - muscleCurvesAreDirty = true; - muscleName = name; +void Millard2016TorqueMuscle::setName(string &name) +{ + mMuscleCurvesAreDirty = true; + mMuscleName = name; } +double Millard2016TorqueMuscle:: +getActiveTorqueAngleCurveBlendingVariable() const +{ + return mTaLambda; +} + +double Millard2016TorqueMuscle:: +getPassiveTorqueAngleCurveBlendingVariable() const +{ + return mTpLambda; +} + +double Millard2016TorqueMuscle:: +getTorqueAngularVelocityCurveBlendingVariable() const +{ + return mTvLambda; +} + +double Millard2016TorqueMuscle::getActiveTorqueAngleCurveAngleScaling() const +{ + return mTaAngleScaling; +} + +void Millard2016TorqueMuscle:: +setActiveTorqueAngleCurveAngleScaling(double angleScaling) +{ + if(angleScaling < SQRTEPSILON) { + ostringstream errormsg; + cerr << "Millard2016TorqueMuscle::" + << "setActiveTorqueAngleCurveAngleScaling:" + << mMuscleName + << ": angleScaling must be > sqrt(eps), this " + << angleScaling + <<" is outside the acceptable range." + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + mMuscleCurvesAreDirty = true; + mTaAngleScaling = angleScaling; +} + +void Millard2016TorqueMuscle:: +setActiveTorqueAngleCurveBlendingVariable(double blendingVariable) +{ + if(blendingVariable < 0. || blendingVariable > 1.0) { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" + << "setActiveTorqueAngleCurveBlendingVariable:" + << mMuscleName + << ": blending variable must be [0,1] and this " + << blendingVariable + << " is outside the acceptable range." + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + mMuscleCurvesAreDirty = true; + mTaLambda = blendingVariable; +} + +void Millard2016TorqueMuscle:: +setPassiveTorqueAngleCurveBlendingVariable(double blendingVariable) +{ + if(blendingVariable < 0. || blendingVariable > 1.0) { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" + << "setPassiveTorqueAngleCurveBlendingVariable:" + << mMuscleName + << ": blending variable must be [0,1] and this " + << blendingVariable + << " is outside the acceptable range." + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + mMuscleCurvesAreDirty = true; + mTpLambda = blendingVariable; +} + +void Millard2016TorqueMuscle:: +setTorqueAngularVelocityCurveBlendingVariable(double blendingVariable) +{ + + if(blendingVariable < 0. || blendingVariable > 1.0) { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" + << "setTorqueAngularVelocityCurveBlendingVariable:" + << mMuscleName + << ": blending variable must be [0,1] and this " + << blendingVariable + << " is outside the acceptable range." + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + mMuscleCurvesAreDirty = true; + mTvLambda = blendingVariable; +} + +void Millard2016TorqueMuscle::setFittedParameters( + const TorqueMuscleParameterFittingData &fittedParameters) +{ + + if(!fittedParameters.fittingConverged) { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" + << "setTorqueMuscleParameters:" + << mMuscleName + << ": The fittingConverged field of fittedParameters is false! " + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + setPassiveTorqueAngleCurveBlendingVariable( + fittedParameters.passiveTorqueAngleBlendingVariable); + setActiveTorqueAngleCurveBlendingVariable( + fittedParameters.activeTorqueAngleBlendingVariable); + setTorqueAngularVelocityCurveBlendingVariable( + fittedParameters.torqueVelocityBlendingVariable); + setPassiveCurveAngleOffset( + fittedParameters.passiveTorqueAngleCurveOffset); + setMaximumActiveIsometricTorque( + fittedParameters.maximumActiveIsometricTorque); + setMaximumConcentricJointAngularVelocity( + fittedParameters.maximumAngularVelocity); + setActiveTorqueAngleCurveAngleScaling( + fittedParameters.activeTorqueAngleAngleScaling); + +} /************************************************************* Utilities @@ -1232,404 +1304,1234 @@ void Millard2016TorqueMuscle::setName(string &name){ //dja = signJointAngle*dfa double Millard2016TorqueMuscle:: - calcFiberAngle(double jointAngle) const +calcFiberAngle(double jointAngle) const { - return signOfJointAngle*(jointAngle-angleOffset); + return mSignOfJointAngle*(jointAngle-mAngleOffset); } double Millard2016TorqueMuscle:: - calcJointAngle(double fiberAngle) const +calcJointAngle(double fiberAngle) const { - return fiberAngle*signOfJointAngle + angleOffset; + return fiberAngle*mSignOfJointAngle + mAngleOffset; } double Millard2016TorqueMuscle:: - calcFiberAngularVelocity(double jointAngularVelocity) const +calcFiberAngularVelocity(double jointAngularVelocity) const { - return signOfConcentricAnglularVelocity*jointAngularVelocity; + return mSignOfConcentricAnglularVelocity*jointAngularVelocity; } double Millard2016TorqueMuscle:: - calcJointAngularVelocity(double fiberAngularVelocity) const +calcJointAngularVelocity(double fiberAngularVelocity) const { - return signOfConcentricAnglularVelocity*fiberAngularVelocity; + return mSignOfConcentricAnglularVelocity*fiberAngularVelocity; } void Millard2016TorqueMuscle::updateTorqueMuscleCurves() { - std::string tempName = muscleName; + std::string tempName = mMuscleName; - switch(dataSet){ - case DataSet::Anderson2007: - { - double c4 = c1c2c3c4c5c6Anderson2007[3]; - double c5 = c1c2c3c4c5c6Anderson2007[4]; + switch(mDataSet) { + case DataSet::Anderson2007: { + double c4 = mAnderson2007c1c2c3c4c5c6[3]; + double c5 = mAnderson2007c1c2c3c4c5c6[4]; - if(useTabularOmegaMax){ - omegaMax = abs( 2.0*c4*c5/(c5-3.0*c4) ); - } + if(mUseTabularOmegaMax) { + mOmegaMax = abs( 2.0*c4*c5/(c5-3.0*c4) ); + } - scaleFactorAnderson2007 = subjectHeightInMeters - *subjectMassInKg - *gravity; + mScaleFactorAnderson2007 = mSubjectHeightInMeters + *mSubjectMassInKg + *Gravity; - if(useTabularMaxActiveIsometricTorque){ - maxActiveIsometricTorque = scaleFactorAnderson2007 - *c1c2c3c4c5c6Anderson2007[0]; - } + if(mUseTabularMaxActiveIsometricTorque) { + mMaxActiveIsometricTorque = mScaleFactorAnderson2007 + *mAnderson2007c1c2c3c4c5c6[0]; + } - angleAtOneNormActiveTorque = c1c2c3c4c5c6Anderson2007[2]; - - TorqueMuscleFunctionFactory:: - createAnderson2007ActiveTorqueAngleCurve( - c1c2c3c4c5c6Anderson2007[1], - c1c2c3c4c5c6Anderson2007[2], - tempName.append("_taCurve"), - taCurve); - - tempName = muscleName; - - TorqueMuscleFunctionFactory:: - createAnderson2007ActiveTorqueVelocityCurve( - c1c2c3c4c5c6Anderson2007[3], - c1c2c3c4c5c6Anderson2007[4], - c1c2c3c4c5c6Anderson2007[5], - 1.1, - 1.4, - tempName.append("_tvCurve"), - tvCurve); - - tempName = muscleName; - - double normMaxActiveIsometricTorque = maxActiveIsometricTorque - /scaleFactorAnderson2007; - - TorqueMuscleFunctionFactory:: - createAnderson2007PassiveTorqueAngleCurve( - scaleFactorAnderson2007, - normMaxActiveIsometricTorque, - b1k1b2k2Anderson2007[0], - b1k1b2k2Anderson2007[1], - b1k1b2k2Anderson2007[2], - b1k1b2k2Anderson2007[3], - tempName.append("_tpCurve"), - tpCurve); - - tpCurve.shift(passiveCurveAngleOffset,0); - tpCurve.scale(1.0,passiveTorqueScale); - - double k = 0; - double b = 0; - - if(b1k1b2k2Anderson2007[0] > 0){ - b = b1k1b2k2Anderson2007[0]; - k = b1k1b2k2Anderson2007[1]; - }else if(b1k1b2k2Anderson2007[2] > 0){ - b = b1k1b2k2Anderson2007[2]; - k = b1k1b2k2Anderson2007[3]; - } + mAngleAtOneNormActiveTorque = mAnderson2007c1c2c3c4c5c6[2]; + + TorqueMuscleFunctionFactory:: + createAnderson2007ActiveTorqueAngleCurve( + mAnderson2007c1c2c3c4c5c6[1], + mAnderson2007c1c2c3c4c5c6[2], + tempName.append("_taCurve"), + mTaCurve); + + tempName = mMuscleName; + + TorqueMuscleFunctionFactory:: + createAnderson2007ActiveTorqueVelocityCurve( + mAnderson2007c1c2c3c4c5c6[3], + mAnderson2007c1c2c3c4c5c6[4], + mAnderson2007c1c2c3c4c5c6[5], + 1.1, + 1.4, + tempName.append("_tvCurve"), + mTvCurve); + + mTorqueVelocityMultiplierAtHalfOmegaMax = mTvCurve.calcValue(0.5); + + tempName = mMuscleName; + + double normMaxActiveIsometricTorque = mMaxActiveIsometricTorque + /mScaleFactorAnderson2007; + + TorqueMuscleFunctionFactory:: + createAnderson2007PassiveTorqueAngleCurve( + mScaleFactorAnderson2007, + normMaxActiveIsometricTorque, + mAnderson2007b1k1b2k2[0], + mAnderson2007b1k1b2k2[1], + mAnderson2007b1k1b2k2[2], + mAnderson2007b1k1b2k2[3], + tempName.append("_tpCurve"), + mTpCurve); + + //mTpCurve.shift(mPassiveCurveAngleOffset,0); + mTpCurve.scale(1.0,mPassiveTorqueScale); + + double k = 0; + double b = 0; + + if(mAnderson2007b1k1b2k2[0] > 0) { + b = mAnderson2007b1k1b2k2[0]; + k = mAnderson2007b1k1b2k2[1]; + } else if(mAnderson2007b1k1b2k2[2] > 0) { + b = mAnderson2007b1k1b2k2[2]; + k = mAnderson2007b1k1b2k2[3]; + } - if(abs(b) > 0 && passiveTorqueScale > SQRTEPSILON){ - angleAtOneNormPassiveTorque = - (1/k)*log(abs(maxActiveIsometricTorque/b)) - + passiveCurveAngleOffset; - }else{ - angleAtOneNormPassiveTorque = - std::numeric_limits::signaling_NaN(); - } + VectorNd xDomain = mTpCurve.getCurveDomain(); + double tpAtX0 = mTpCurve.calcValue(xDomain[0]); + double tpAtX1 = mTpCurve.calcValue(xDomain[1]); - //angleAtOneNormPassiveTorque - //gymnastParams[Gymnast::PassiveAngleAtOneNormTorque] - - - } break; - case DataSet::Gymnast: - { - if(useTabularOmegaMax){ - omegaMax = gymnastParams[ - Gymnast::OmegaMax]; - } - if(useTabularMaxActiveIsometricTorque){ - maxActiveIsometricTorque = gymnastParams[ - Gymnast::TauMax]; - } - angleAtOneNormActiveTorque = gymnastParams[ - Gymnast::ActiveAngleAtOneNormTorque]; - - TorqueMuscleFunctionFactory:: - createGaussianShapedActiveTorqueAngleCurve( - gymnastParams[Gymnast::ActiveAngleAtOneNormTorque], - gymnastParams[Gymnast::ActiveAngularStandardDeviation], - tempName.append("_taCurve"), - taCurve); - - TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( - gymnastParams[Gymnast::PassiveAngleAtZeroTorque], - gymnastParams[Gymnast::PassiveAngleAtOneNormTorque], - tempName.append("_tpCurve"), - tpCurve); - - tpCurve.shift(passiveCurveAngleOffset,0); - tpCurve.scale(1.0,passiveTorqueScale); - - TorqueMuscleFunctionFactory::createTorqueVelocityCurve( - gymnastParams[Gymnast::TvAtMaxEccentricVelocity], - gymnastParams[Gymnast::TvAtHalfMaxConcentricVelocity], - tempName.append("_tvCurve"), - tvCurve); - - if(passiveTorqueScale > 0){ - angleAtOneNormPassiveTorque = - gymnastParams[Gymnast::PassiveAngleAtOneNormTorque] - + passiveCurveAngleOffset; - }else{ - angleAtOneNormPassiveTorque = - std::numeric_limits::signaling_NaN(); - } - - } break; - default: - { - cerr << "Millard2016TorqueMuscle::" - << "Millard2016TorqueMuscle:" - << muscleName - << "dataSet has a value of " << dataSet - << " which is not a valid choice"; - assert(0); - abort(); + if(fabs(tpAtX0) < SQRTEPSILON) { + mAngleAtSmallestNormPassiveTorque = xDomain[0]; + } else if(fabs(tpAtX1) < SQRTEPSILON) { + mAngleAtSmallestNormPassiveTorque = xDomain[1]; + } else { + mAngleAtSmallestNormPassiveTorque = + std::numeric_limits::signaling_NaN(); } - }; + if( fabs(tpAtX0) > SQRTEPSILON + || fabs(tpAtX1) > SQRTEPSILON) { + double argGuess = (1/k)*log(abs(mMaxActiveIsometricTorque/b)); + // + mPassiveCurveAngleOffset; + mAngleAtOneNormPassiveTorque = calcInverseBlendedCurveValue(1.0, + argGuess, + mTpLambda, + mTpLambdaMax, + mTpCurve); + mAngleAtOneNormPassiveTorque += mPassiveCurveAngleOffset; + } else { + mAngleAtOneNormPassiveTorque = + std::numeric_limits::signaling_NaN(); + } + + + //mAngleAtOneNormPassiveTorque + //mGymnastParams[Gymnast::PassiveAngleAtOneNormTorque] + + + } + break; + case DataSet::Gymnast: { + if(mUseTabularOmegaMax) { + mOmegaMax = mGymnastParams[ + Gymnast::OmegaMax]; + } + if(mUseTabularMaxActiveIsometricTorque) { + mMaxActiveIsometricTorque = mGymnastParams[ + Gymnast::TauMax]; + } + mAngleAtOneNormActiveTorque = mGymnastParams[ + Gymnast::ActiveAngleAtOneNormTorque]; + TorqueMuscleFunctionFactory:: + createGaussianShapedActiveTorqueAngleCurve( + mGymnastParams[Gymnast::ActiveAngleAtOneNormTorque], + mGymnastParams[Gymnast::ActiveAngularStandardDeviation], + tempName.append("_taCurve"), + mTaCurve); - //If the passiveScale is < 1 and > 0, then we must iterate to - //find the true value of angleAtOneNormPassiveTorque; + TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( + mGymnastParams[Gymnast::PassiveAngleAtZeroTorque], + mGymnastParams[Gymnast::PassiveAngleAtOneNormTorque], + tempName.append("_tpCurve"), + mTpCurve); - if(isfinite(angleAtOneNormPassiveTorque) - && (passiveTorqueScale > 0 && passiveTorqueScale != 1.0) ){ - int iter = 1; - int iterMax =100; - double err = 10*SQRTEPSILON; - double derr = 0; - double delta = 0; + //mTpCurve.shift(mPassiveCurveAngleOffset,0); + mTpCurve.scale(1.0,mPassiveTorqueScale); - while(abs(err) > SQRTEPSILON && iter < iterMax){ - err = tpCurve.calcValue(angleAtOneNormPassiveTorque) - - 1.0; - derr = tpCurve.calcDerivative(angleAtOneNormPassiveTorque,1); - delta = -err/derr; - angleAtOneNormPassiveTorque += delta; - ++iterMax; + if(mUseTabularTorqueVelocityMultiplierAtHalfOmegaMax) { + mTorqueVelocityMultiplierAtHalfOmegaMax + = mGymnastParams[Gymnast::TvAtHalfMaxConcentricVelocity]; } - if(abs(err) > SQRTEPSILON){ - cerr << "Millard2016TorqueMuscle::" - << "Millard2016TorqueMuscle:" - << muscleName - << "Internal Error: failed to solve for " - <<"angleAtOneNormPassiveTorque. Consult the maintainer of this " - <<"addon"; - assert(0); - abort(); + TorqueMuscleFunctionFactory::createTorqueVelocityCurve( + mGymnastParams[Gymnast::TvAtMaxEccentricVelocity], + mTorqueVelocityMultiplierAtHalfOmegaMax, + tempName.append("_tvCurve"), + mTvCurve); + + + //If the passive curve is non-zero, recalculate the angle at which + //it hits one normalized force. + mAngleAtSmallestNormPassiveTorque = + mGymnastParams[Gymnast::PassiveAngleAtZeroTorque] + +mPassiveCurveAngleOffset; + + //Verify that the curve hits zero at this angle + if(fabs(mTpCurve.calcValue(mAngleAtSmallestNormPassiveTorque)) + >SQRTEPSILON) { + mAngleAtSmallestNormPassiveTorque = + numeric_limits::signaling_NaN(); } + VectorNd xDomain = mTpCurve.getCurveDomain(); + double tpAtX0 = mTpCurve.calcValue(xDomain[0]); + double tpAtX1 = mTpCurve.calcValue(xDomain[1]); + + if( fabs(tpAtX0) > SQRTEPSILON + || fabs(tpAtX1) > SQRTEPSILON) { + double argGuess =mGymnastParams[Gymnast::PassiveAngleAtOneNormTorque]; + //+ mPassiveCurveAngleOffset; + mAngleAtOneNormPassiveTorque = + calcInverseBlendedCurveValue(1.0,argGuess,mTpLambda,mTpLambdaMax, + mTpCurve); + mAngleAtOneNormPassiveTorque += mPassiveCurveAngleOffset; + } else { + mAngleAtOneNormPassiveTorque = + std::numeric_limits::signaling_NaN(); + } } + break; + default: { + ostringstream errormsg; + errormsg << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << "mDataSet has a value of " << mDataSet + << " which is not a valid choice"; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + }; - //Update the damping curve. - tempName = muscleName; - TorqueMuscleFunctionFactory::createDampingBlendingCurve( - -1.0, - tempName.append("_dampingCurve"), - betaCurve); - - muscleCurvesAreDirty = false; + VectorNd tvDomain = mTvCurve.getCurveDomain(); + mTvLambdaMax = mTvCurve.calcValue(tvDomain[0]); + assert(mTvLambdaMax >= 1.0); //If this fails you've gotten the incorrect + //domain end. + + /* + double taIsoMax = calcBlendedCurveDerivative(mAngleAtOneNormActiveTorque, + mTaLambda, mTaLambdaMax, + 0,0, mTaCurve); + double tvIsoMax = calcBlendedCurveDerivative(0, + mTvLambda, mTvLambdaMax, + 0,0, mTvCurve); + mMaxActiveIsometricMultiplerProduct = taIsoMax*tvIsoMax; + + if(mUseTabularMaxActiveIsometricTorque == false){ + mMaxActiveIsometricTorque = mMaxActiveIsometricTorqueUserDefined + /mMaxActiveIsometricMultiplerProduct; + } + */ + mMuscleCurvesAreDirty = false; } void Millard2016TorqueMuscle::printJointTorqueProfileToFile( - const std::string& path, - const std::string& fileNameWithoutExtension, - int numberOfSamplePoints) + const std::string& path, + const std::string& fileNameWithoutExtension, + int numberOfSamplePoints) { - if(muscleCurvesAreDirty){ - updateTorqueMuscleCurves(); - } + if(mMuscleCurvesAreDirty) { + updateTorqueMuscleCurves(); + } - VectorNd activeDomain = taCurve.getCurveDomain(); - VectorNd passiveDomain = tpCurve.getCurveDomain(); - VectorNd velocityDomain= tvCurve.getCurveDomain(); + VectorNd activeDomain = mTaCurve.getCurveDomain(); + VectorNd passiveDomain = mTpCurve.getCurveDomain(); + VectorNd velocityDomain= mTvCurve.getCurveDomain(); - double angleMin = activeDomain[0]; - double angleMax = activeDomain[1]; + double jointMin = calcJointAngle( activeDomain[0] ); + double jointMax = calcJointAngle( activeDomain[1] ); - if(tpCurve.calcValue(passiveDomain[0]) >= 0.99){ - angleMin = passiveDomain[0]; - } + if(mTpCurve.calcValue( activeDomain[0] ) >= 0.99) { + jointMin = calcJointAngle( passiveDomain[0] ); + } + + if(mTpCurve.calcValue(activeDomain[1]) >= 0.99) { + jointMax = calcJointAngle( passiveDomain[1] ); + } - if(tpCurve.calcValue(passiveDomain[1]) >= 0.99){ - angleMax = passiveDomain[1]; - } - double jointMin = signOfJointAngle*angleMin + angleOffset; - double jointMax = signOfJointAngle*angleMax + angleOffset; - if(jointMin > jointMax){ - double tmp = jointMin; - jointMin=jointMax; - jointMax=tmp; + if(jointMin > jointMax) { + double tmp = jointMin; + jointMin=jointMax; + jointMax=tmp; + } + double range = jointMax-jointMin; + jointMin = jointMin -range*0.5; + jointMax = jointMax +range*0.5; + double jointDelta = (jointMax-jointMin) + /((double)numberOfSamplePoints-1.); + + double velMin = calcJointAngularVelocity( -mOmegaMax ); + double velMax = calcJointAngularVelocity( mOmegaMax ); + + if(velMin > velMax) { + double tmp = velMin; + velMin = velMax; + velMax = tmp; + } + double velRange = velMax-velMin; + velMin = velMin-0.5*velRange; + velMax = velMax+0.5*velRange; + double velDelta = (velMax-velMin)/((double)numberOfSamplePoints-1.0); + + double angleAtMaxIsoTorque = mAngleAtOneNormActiveTorque; + + std::vector< std::vector < double > > matrix; + std::vector < double > row(21); + std::string header("jointAngle," + "jointVelocity," + "activation," + "fiberAngle," + "fiberAngularVelocity," + "passiveTorqueAngleMultiplier," + "activeTorqueAngleMultiplier," + "torqueVelocityMultiplier," + "activeTorque," + "passiveTorque," + "fiberTorque," + "jointTorque," + "fiberStiffness," + "jointStiffness," + "fiberActivePower," + "fiberPassivePower," + "fiberPower," + "jointPower," + "DjointTorqueDactivation," + "DjointTorqueDjointAngularVelocity," + "DjointTorqueDjointAngle"); + + double activation =1.0; + double jointAngle = 0.; + double jointVelocity = 0.; + + + for(int i=0; i velMax){ - double tmp = velMin; - velMin = velMax; - velMax = tmp; + break; + case 1: { + output = maximumBlendingValue - curve.calcValue(curveArgument); } - double velRange = velMax-velMin; - velMin = velMin-0.1*velRange; - velMax = velMax+0.1*velRange; - double velDelta = (velMax-velMin)/((double)numberOfSamplePoints-1.0); - - double angleAtMaxIsoTorque = angleAtOneNormActiveTorque; - - std::vector< std::vector < double > > matrix; - std::vector < double > row(21); - std::string header("jointAngle," - "jointVelocity," - "activation," - "fiberAngle," - "fiberAngularVelocity," - "passiveTorqueAngleMultiplier," - "activeTorqueAngleMultiplier," - "torqueVelocityMultiplier," - "activeTorque," - "passiveTorque," - "fiberTorque," - "jointTorque," - "fiberStiffness," - "jointStiffness," - "fiberActivePower," - "fiberPassivePower," - "fiberPower," - "jointPower," - "DjointTorqueDactivation," - "DjointTorqueDjointAngularVelocity," - "DjointTorqueDjointAngle"); - - double activation =1.0; - double jointAngle = 0.; - double jointVelocity = 0.; - - - for(int i=0; i::signaling_NaN(); + VectorNd domain = mTpCurve.getCurveDomain(); + double y0 = mTpCurve.calcValue(domain[0]); + double y1 = mTpCurve.calcValue(domain[1]); + + if( (blendingVariable < 1) && (y0 > SQRTEPSILON || y1 > SQRTEPSILON) ) { + double tp = normPassiveFiberTorque/(1-blendingVariable); + angle = mTpCurve.calcInverseValue(tp,0.)-passiveTorqueAngleCurveOffset; + } + + return angle; } +//============================================================================= +//============================================================================= +void Millard2016TorqueMuscle:: +updTorqueMuscleSummaryCurveValues(double fiberAngle, + double normFiberAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double torqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + TorqueMuscleSummary &updTms) const +{ + + double taAngle = (fiberAngle-activeTorqueAngleAtOneNormTorque) + /activeTorqueAngleAngleScaling + + activeTorqueAngleAtOneNormTorque; + + updTms.fiberActiveTorqueAngleMultiplier = + calcBlendedCurveDerivative(taAngle, + activeTorqueAngleBlendingVariable, + mTaLambdaMax, + 0,0, + mTaCurve); + + updTms.fiberPassiveTorqueAngleMultiplier = + calcBlendedCurveDerivative(fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 0,0, + mTpCurve); + + updTms.fiberTorqueAngularVelocityMultiplier = + calcBlendedCurveDerivative( + normFiberAngularVelocity, + torqueAngularVelocityBlendingVariable, + mTvLambdaMax, + 0,0, + mTvCurve); + + updTms.fiberNormalizedDampingTorque = updTms.fiberPassiveTorqueAngleMultiplier + * (-normFiberAngularVelocity*mBetaMax); + +} + +void Millard2016TorqueMuscle:: +updTorqueMuscleInfo( + double activation, + double jointAngle, + double jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double torqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActIsoTorque, + TorqueMuscleInfo &updTmi) const +{ + + //Update state quantities + updTmi.activation = activation; + updTmi.jointAngle = jointAngle; + updTmi.jointAngularVelocity = jointAngularVelocity; + + double fiberAngle = calcFiberAngle(jointAngle); + double fiberAngularVelocity = calcFiberAngularVelocity(jointAngularVelocity); + double omegaNorm = fiberAngularVelocity/maxAngularVelocity; + double D_wn_D_w = 1.0/maxAngularVelocity; + double D_wn_D_wmax = -fiberAngularVelocity + /(maxAngularVelocity*maxAngularVelocity); + + double D2_wn_D_wmax2 = -2.0*D_wn_D_wmax/maxAngularVelocity; + + double taAngle = (fiberAngle-activeTorqueAngleAtOneNormTorque) + /activeTorqueAngleAngleScaling + + activeTorqueAngleAtOneNormTorque; + + double D_taAngle_D_fiberAngle = 1.0/activeTorqueAngleAngleScaling; + + double D_taAngle_D_angleScaling = + -(fiberAngle-activeTorqueAngleAtOneNormTorque) + /(activeTorqueAngleAngleScaling*activeTorqueAngleAngleScaling); + + double D2_taAngle_D_angleScaling2 = -2.0*D_taAngle_D_angleScaling + /activeTorqueAngleAngleScaling; + + //Update force component values + double ta = + calcBlendedCurveDerivative(taAngle, + activeTorqueAngleBlendingVariable, + mTaLambdaMax, + 0,0, + mTaCurve); + + double tp = + calcBlendedCurveDerivative(fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 0,0, + mTpCurve); + + double tv = calcBlendedCurveDerivative( + omegaNorm, + torqueAngularVelocityBlendingVariable, + mTvLambdaMax, + 0,0, + mTvCurve); + + double tb = tp * (-omegaNorm*mBetaMax); + + double D_tb_D_omegaMax = tp*(-D_wn_D_wmax*mBetaMax); + + + //Update force component derivative values; + //1st derivatives w.r.t fiber angle/velocity + double D_ta_D_taAngle = calcBlendedCurveDerivative( + taAngle, + activeTorqueAngleBlendingVariable, + mTaLambdaMax, + 1,0, + mTaCurve); + + double D_ta_D_angleScaling = D_ta_D_taAngle*D_taAngle_D_angleScaling; + + double D_ta_D_fiberAngle = D_ta_D_taAngle*D_taAngle_D_fiberAngle; + + double D_tp_D_fiberAngle = calcBlendedCurveDerivative( + fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 1,0, + mTpCurve); + + double D_tp_D_fiberAngleOffset= -D_tp_D_fiberAngle; + + double D_tv_D_wn = calcBlendedCurveDerivative( + omegaNorm, + torqueAngularVelocityBlendingVariable, + mTvLambdaMax, + 1,0, + mTvCurve); + + + + double D_tv_D_fiberAngularVelocity = D_tv_D_wn*D_wn_D_w; + double D_tv_D_omegaMax = D_tv_D_wn*D_wn_D_wmax; + + + //1st derivatives w.r.t fitting-related variables + double D_ta_D_taLambda = calcBlendedCurveDerivative( + taAngle, + activeTorqueAngleBlendingVariable, + mTaLambdaMax, + 0,1, + mTaCurve); + + double D_tp_D_tpLambda = calcBlendedCurveDerivative( + fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 0,1, + mTpCurve); + double D_tv_D_tvLambda = calcBlendedCurveDerivative( + omegaNorm, + torqueAngularVelocityBlendingVariable, + mTvLambdaMax, + 0,1, + mTvCurve); + + //2nd derivatives w.r.t fitting-related variables. + double D2_ta_D_taLambda2 = calcBlendedCurveDerivative( + taAngle, + activeTorqueAngleBlendingVariable, + mTaLambdaMax, + 0,2, + mTaCurve); + + double D2_tv_D_tvLambda2 = calcBlendedCurveDerivative( + omegaNorm, + torqueAngularVelocityBlendingVariable, + mTvLambdaMax, + 0,2, + mTvCurve); + double D2_tp_D_tpLambda2 = calcBlendedCurveDerivative( + fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 0,2, + mTpCurve); + + + double D2_tp_D_fiberAngle2 =calcBlendedCurveDerivative( + fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 2,0, + mTpCurve); + + double D2_ta_D_taAngle2 = calcBlendedCurveDerivative( + taAngle, + activeTorqueAngleBlendingVariable, + mTaLambdaMax, + 2,0, + mTaCurve); + + double D2_ta_D_angleScaling2 = + D2_ta_D_taAngle2*D_taAngle_D_angleScaling*D_taAngle_D_angleScaling + + D_ta_D_taAngle*D2_taAngle_D_angleScaling2; + + double D2_tv_D_wn2 = calcBlendedCurveDerivative( + omegaNorm, + torqueAngularVelocityBlendingVariable, + mTvLambdaMax, + 2,0, + mTvCurve); + + double D2_tb_D_omegaMax2 = tp*(-D2_wn_D_wmax2*mBetaMax); + double D2_tb_D_omegaMax_D_tpLambda = D_tp_D_tpLambda*(-D_wn_D_wmax*mBetaMax); + double D2_tb_D_omegaMax_D_tpOffset = D_tp_D_fiberAngleOffset + *(-D_wn_D_wmax*mBetaMax); + double D2_tv_D_omegaMax2 = D2_tv_D_wn2*D_wn_D_wmax*D_wn_D_wmax + + D_tv_D_wn*D2_wn_D_wmax2; + + + + //Note that d/d_tpOffsetAngle kicks out another -1, so this 2nd derivative has + //a positive sign. + double D2_tp_D_fiberAngleOffset2= D2_tp_D_fiberAngle2; + + double D2_tp_D_tpLambda_D_fiberAngle = + calcBlendedCurveDerivative( fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 1,1, + mTpCurve); + double D2_tp_D_tpLambda_D_fiberAngleOffset = -D2_tp_D_tpLambda_D_fiberAngle; + + //Damping derivatives + double D_tb_D_wn = tp * (-1.0*mBetaMax); + + double D_tb_D_tpLambda = -D_tp_D_tpLambda*mBetaMax*omegaNorm; + + + double D_tb_D_fiberAngularVelocity = D_tb_D_wn*D_wn_D_w; + + double D_tb_D_fiberAngle = -D_tp_D_fiberAngle*mBetaMax*omegaNorm; + + double D_tb_D_fiberAngleOffset= -D_tp_D_fiberAngleOffset*mBetaMax*omegaNorm; + + //Damping second derivatives + double D2_tb_D_tpLambda_D_fiberAngleOffset = + -D2_tp_D_tpLambda_D_fiberAngleOffset*mBetaMax*omegaNorm; + double D2_tb_D_tpLambda2 = + -D2_tp_D_tpLambda2*mBetaMax*omegaNorm; + double D2_tb_D_fiberAngleOffset2 = + -D2_tp_D_fiberAngleOffset2*mBetaMax*omegaNorm; + + //Sign conventions + double D_fiberAngle_D_jointAngle = mSignOfJointAngle; + double D_fiberAngularVelocity_D_jointAngularVelocity = + mSignOfConcentricAnglularVelocity; + + + updTmi.jointAngle = jointAngle; + updTmi.jointAngularVelocity = jointAngularVelocity; + updTmi.fiberAngle = fiberAngle; + updTmi.fiberAngularVelocity = fiberAngularVelocity; + + updTmi.fiberPassiveTorqueAngleMultiplier = tp; + updTmi.fiberActiveTorqueAngleMultiplier = ta; + updTmi.fiberTorqueAngularVelocityMultiplier = tv; + + updTmi.activation = activation; + updTmi.fiberActiveTorque = maxActIsoTorque*(activation*ta*tv); + updTmi.fiberPassiveTorque = maxActIsoTorque*(tp+tb); + updTmi.fiberPassiveElasticTorque = maxActIsoTorque*tp; + updTmi.fiberDampingTorque = maxActIsoTorque*tb; + updTmi.fiberNormDampingTorque = tb; + + updTmi.fiberTorque = updTmi.fiberActiveTorque + updTmi.fiberPassiveTorque; + updTmi.jointTorque = mSignOfJointTorque*updTmi.fiberTorque; + + updTmi.fiberStiffness = maxActIsoTorque*( + activation*D_ta_D_fiberAngle*tv + + D_tp_D_fiberAngle + + D_tb_D_fiberAngle); + + updTmi.jointStiffness = mSignOfJointTorque + *updTmi.fiberStiffness + *D_fiberAngle_D_jointAngle; + + updTmi.fiberActivePower = updTmi.fiberActiveTorque + * updTmi.fiberAngularVelocity; + + updTmi.fiberPassivePower = updTmi.fiberPassiveTorque + * updTmi.fiberAngularVelocity; + + updTmi.fiberPower = updTmi.fiberActivePower + + updTmi.fiberPassivePower; + + updTmi.jointPower = updTmi.jointTorque * jointAngularVelocity; + + + updTmi.DfiberPassiveTorqueAngleMultiplier_DblendingVariable + = D_tp_D_tpLambda; + updTmi.DfiberActiveTorqueAngleMultiplier_DblendingVariable + = D_ta_D_taLambda; + updTmi.DfiberTorqueAngularVelocityMultiplier_DblendingVariable + = D_tv_D_tvLambda; + updTmi.DfiberPassiveTorqueAngleMultiplier_DangleOffset + = D_tp_D_fiberAngleOffset; + + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm) + // Dtau_Da = tauMaxIso*(ta(theta) * tv(thetaDot) ) + updTmi.DjointTorque_Dactivation = + mSignOfJointTorque + *maxActIsoTorque + *(ta * tv); + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm) + //Dtau_Dtheta = signTq*tauIso*(a*Dta_Dtheta(theta)*tv(thetaDot) + // + Dtp_Dtheta(theta)*(1-beta*omegaNorm) + updTmi.DjointTorque_DjointAngle = + mSignOfJointTorque + * maxActIsoTorque + * ( activation + *D_ta_D_fiberAngle + * tv + + ( D_tp_D_fiberAngle + + D_tb_D_fiberAngle) + )* D_fiberAngle_D_jointAngle; + + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm) + //Dtau_Domega = signTq*tauIso*(a * ta(theta) * Dtv_DthetaDot(thetaDot) + // - tp(theta)*beta*DomegaNorm_thetaDot + updTmi.DjointTorque_DjointAngularVelocity = + mSignOfJointTorque + * maxActIsoTorque + * ( activation + * ta + * ( D_tv_D_fiberAngularVelocity + *D_fiberAngularVelocity_D_jointAngularVelocity) + + ( D_tb_D_fiberAngularVelocity + *D_fiberAngularVelocity_D_jointAngularVelocity) + ); + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm) + //Dtau_DtaLambda = signTq*tauIso*(a * Dta(theta)_Dlambda * tv(thetaDot) + updTmi.DjointTorque_DactiveTorqueAngleBlendingVariable = + mSignOfJointTorque + * maxActIsoTorque + *(activation*D_ta_D_taLambda*tv); + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm) + //Dtau_DtpLambda = signTq*tauIso*( D_tp(theta)_D_lambda(1-beta*omegaNorm) + updTmi.DjointTorque_DpassiveTorqueAngleBlendingVariable = + mSignOfJointTorque + * maxActIsoTorque + *(D_tp_D_tpLambda + D_tb_D_tpLambda); + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm) + //Dtau_tvLambda = signTq*tauIso*(a*ta(theta)*Dtv_Dlambda(thetaDot) + updTmi.DjointTorque_DtorqueAngularVelocityBlendingVariable = + mSignOfJointTorque + * maxActIsoTorque + *(activation*ta*D_tv_D_tvLambda); + + // mSignOfJointTorque*( (maxActIsoTorque*(activation*ta*tv) + // + maxActIsoTorque*(tp+tb))) + updTmi.DjointTorque_DmaximumIsometricTorque = + mSignOfJointTorque + *((activation*ta*tv) + (tp+tb)); + + updTmi.DjointTorque_DpassiveTorqueAngleCurveAngleOffset = + mSignOfJointTorque + *maxActIsoTorque + *(D_tp_D_fiberAngleOffset + D_tb_D_fiberAngleOffset); + //* D_fiberAngle_D_jointAngle; + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)+tb(theta,omega) + //Dtau_DomegaMax = signTq*tauIso*(a*ta(theta)*Dtv_DomegaMax(thetaDot) + // + tp(theta)*(-beta*D_omegaNorm_DomegaMax) + updTmi.DjointTorque_DmaximumAngularVelocity = + mSignOfJointTorque + *maxActIsoTorque + *(activation*ta*D_tv_D_omegaMax + D_tb_D_omegaMax); + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)+tb(theta,omega) + //Dtau_DtaAngleScaling = signTq*tauIso*(a*Dta(theta)_DangleScaling*tv(thetaDot) + + updTmi.DjointTorque_DactiveTorqueAngleAngleScaling = + mSignOfJointTorque + *maxActIsoTorque + *(activation*D_ta_D_angleScaling*tv); + + + /* + Second derivatives of the exposed variables, x, for fitting: + + Option 1: + x = [taLambda,tvLambda,tpLambda,tpOffset] + For brevity, the short forms will be used just in the comments below + x = [a, v, p, o] + For reference: + t(a,v,p,o) = mSignOfJointTorque*( + (maxActIsoTorque*( + activation*ta(a)*tv(v)) + (tp(p,o)+tb(p,o)))) + Where 't' is the shortform used to represent joint-torque. + + The lower right triangle for the Hessian of tau(a,v,p,o) w.r.t. x is + stored, row-wise, in the vector fittingInfo. Thus we have + + a v p o + H(tau(a,v,p,o),x) = a [ 0: d2t/da2 + v [ 1: d2t/dadv 2: d2t/dv2 + p [ 3: d2t/dadp 4: d2t/dvdp 5: d2t/dp2 + o [ 6: d2t/dado 7: d2t/dvdo 8: d2t/dpdo 9: d2t/do2 + The numbers indicate the index of the quantity in fittingInfo. + + Option 2: + x = [taAngleScaling, tvOmegaMax, tpLambda,tpOffset] + for brevity + x = [s, m, p, o] + + For reference: + t(s,m,p,o) = mSignOfJointTorque*( + (maxActIsoTorque*( + activation*ta(s)*tv(m)) + (tp(p,o)+tb(p,o)))) + Where 't' is the shortform used to represent joint-torque. + + The lower right triangle for the Hessian of tau(s,m,p,o) w.r.t. x is + stored, row-wise, in the vector fittingInfo. Thus we have + + s m p o + H(tau(s,m,p,o),x) = s [ 0: d2t/ds2 + m [ 1: d2t/dsdm 2: d2t/dm2 + p [ 3: d2t/dsdp 4: d2t/dmdp 5: d2t/dp2 + o [ 6: d2t/dsdo 7: d2t/dmdo 8: d2t/dpdo 9: d2t/do2 + The numbers indicate the index of the quantity in fittingInfo. + + So that both fitting options are possible, the extra entries for the + above Hessian will just be concatentated to the existing vector. Three + of the entries are duplicates, but for now they are being included + just to make this slightly easier to use for now. Thus the vector of + 2nd derivatives will look like: + + For joint-torque related constraints that use x = [a,v,p,o] + 0: d2t/da2 + 1: d2t/dadv + 2: d2t/dv2 + 3: d2t/dadp + 4: d2t/dvdp + 5: d2t/dp2 + 6: d2t/dado + 7: d2t/dvdo + 8: d2t/dpdo + 9: d2t/do2 + For constraints on the value of the passive element + 10: d2p/dp2 + 11: d2p/dpdo + 12: d2p/do2 + For joint-torque related constraints that use x = [s,m,p,o] + 13: d2t/ds2 + 14: d2t/dsdm + 15: d2t/dm2 + 16: d2t/dsdp + 17: d2t/dmdp + 18: d2t/dp2 + 19: d2t/dsdo + 20: d2t/dmdo + 21: d2t/dpdo + 22: d2t/do2 + + */ + + #ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING + updTmi.fittingInfo.conservativeResize(23); + + //0: d2t/da2 + updTmi.fittingInfo[0] = mSignOfJointTorque*(maxActIsoTorque*( + activation*D2_ta_D_taLambda2*tv)); + + //1: d2t/dadv + updTmi.fittingInfo[1] = mSignOfJointTorque*(maxActIsoTorque* + (activation*D_ta_D_taLambda*D_tv_D_tvLambda)); + + //2: d2t/dv2 + updTmi.fittingInfo[2] = mSignOfJointTorque*(maxActIsoTorque* + (activation*ta*D2_tv_D_tvLambda2)); + + //3: d2t/dadp + updTmi.fittingInfo[3] = 0.; + + //4: d2t/dvdp + updTmi.fittingInfo[4] = 0.; + + //5: d2t/dp2 + updTmi.fittingInfo[5] = mSignOfJointTorque*(maxActIsoTorque*( + D2_tp_D_tpLambda2+D2_tb_D_tpLambda2)); + //6: d2t/dado + updTmi.fittingInfo[6] = 0.; + //7: d2t/dvdo + updTmi.fittingInfo[7] = 0.; + //8: d2t/dpdo + updTmi.fittingInfo[8] = mSignOfJointTorque*(maxActIsoTorque*( + D2_tp_D_tpLambda_D_fiberAngleOffset + +D2_tb_D_tpLambda_D_fiberAngleOffset + ));//*D_fiberAngle_D_jointAngle); + //9: d2t/do2 + updTmi.fittingInfo[9] = mSignOfJointTorque*(maxActIsoTorque*( + D2_tp_D_fiberAngleOffset2 + +D2_tb_D_fiberAngleOffset2 + ));//*D_fiberAngle_D_jointAngle); + //10: d2tp/dp2 + updTmi.fittingInfo[10] = D2_tp_D_tpLambda2; + //11: d2tp/dpdo + updTmi.fittingInfo[11] = D2_tp_D_tpLambda_D_fiberAngleOffset; + //12: d2tp/do2 + updTmi.fittingInfo[12] = D2_tp_D_fiberAngleOffset2; + + //13: d2t/ds2 + updTmi.fittingInfo[13] = mSignOfJointTorque + *maxActIsoTorque + *(activation*D2_ta_D_angleScaling2*tv); + //14: d2t/dsdm + updTmi.fittingInfo[14] = mSignOfJointTorque + *maxActIsoTorque + *(activation*D_ta_D_angleScaling*D_tv_D_omegaMax); + + //15: d2t/dm2 + updTmi.fittingInfo[15] = mSignOfJointTorque + *maxActIsoTorque + *(activation*ta*D2_tv_D_omegaMax2 + D2_tb_D_omegaMax2); + //16: d2t/dsdp + updTmi.fittingInfo[16] = 0.; + + //17: d2t/dmdp + updTmi.fittingInfo[17] = mSignOfJointTorque + *maxActIsoTorque + *(D2_tb_D_omegaMax_D_tpLambda); + //18: d2t/dp2 + updTmi.fittingInfo[18] = mSignOfJointTorque*( + maxActIsoTorque*( + D2_tp_D_tpLambda2+D2_tb_D_tpLambda2)); + //19: d2t/dsdo + updTmi.fittingInfo[19] = 0.; + + //20: d2t/dmdo + updTmi.fittingInfo[20] = mSignOfJointTorque + *maxActIsoTorque + *(D2_tb_D_omegaMax_D_tpOffset); + + //21: d2t/dpdo + updTmi.fittingInfo[21] = mSignOfJointTorque + *(maxActIsoTorque + *( D2_tp_D_tpLambda_D_fiberAngleOffset + +D2_tb_D_tpLambda_D_fiberAngleOffset)); + + //22: d2t/do2 + updTmi.fittingInfo[22] = mSignOfJointTorque + *(maxActIsoTorque + *( D2_tp_D_fiberAngleOffset2 + +D2_tb_D_fiberAngleOffset2)); + + #endif + +} + +//============================================================================= +void Millard2016TorqueMuscle:: +updTorqueMuscleSummary( double activation, + double jointAngle, + double jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double torqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActIsoTorque, + TorqueMuscleSummary &updTms) const +{ + double fiberAngle = calcFiberAngle(jointAngle); + double fiberVelocity = calcFiberAngularVelocity(jointAngularVelocity); + double fiberVelocityNorm = fiberVelocity/maxAngularVelocity; + + + updTorqueMuscleSummaryCurveValues(fiberAngle, + fiberVelocityNorm, + activeTorqueAngleBlendingVariable, + passiveTorqueAngleBlendingVariable, + torqueAngularVelocityBlendingVariable, + activeTorqueAngleAngleScaling, + activeTorqueAngleAtOneNormTorque, + passiveTorqueAngleCurveOffset, + updTms); + updTms.fiberAngle = fiberAngle; + updTms.fiberAngularVelocity = fiberVelocity; + updTms.activation = activation; + updTms.fiberTorque = + maxActIsoTorque + *( activation*( updTms.fiberActiveTorqueAngleMultiplier + *updTms.fiberTorqueAngularVelocityMultiplier + ) + + ( updTms.fiberPassiveTorqueAngleMultiplier + + updTms.fiberNormalizedDampingTorque + ) + ); + + updTms.jointTorque = updTms.fiberTorque*mSignOfJointTorque; +} + +void Millard2016TorqueMuscle:: +updInvertTorqueMuscleSummary(double jointTorque, + double jointAngle, + double jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double torqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActIsoTorque, + TorqueMuscleSummary &updTms) const +{ + + double fiberAngle = calcFiberAngle(jointAngle); + double fiberVelocity = calcFiberAngularVelocity(jointAngularVelocity); + double fiberVelocityNorm = fiberVelocity/maxAngularVelocity; + + updTorqueMuscleSummaryCurveValues(fiberAngle, + fiberVelocityNorm, + activeTorqueAngleBlendingVariable, + passiveTorqueAngleBlendingVariable, + torqueAngularVelocityBlendingVariable, + activeTorqueAngleAngleScaling, + activeTorqueAngleAtOneNormTorque, + passiveTorqueAngleCurveOffset, + updTms); + + updTms.fiberAngle = fiberAngle; + updTms.fiberAngularVelocity = fiberVelocity; + + updTms.jointTorque = jointTorque; + updTms.fiberTorque = jointTorque*mSignOfJointTorque; + + updTms.activation = + ( (updTms.fiberTorque/maxActIsoTorque) + - (updTms.fiberPassiveTorqueAngleMultiplier + + updTms.fiberNormalizedDampingTorque) + )/( updTms.fiberActiveTorqueAngleMultiplier + *updTms.fiberTorqueAngularVelocityMultiplier); +} + +//============================================================================== +DataSet::item Millard2016TorqueMuscle::getDataSet(){ + return mDataSet; +} +GenderSet::item Millard2016TorqueMuscle::getGender(){ + return mGender; +} +AgeGroupSet::item Millard2016TorqueMuscle::getAgeGroup(){ + return mAgeGroup; +} +JointTorqueSet::item Millard2016TorqueMuscle::getJointTorque(){ + return mJointTorque; +} +double Millard2016TorqueMuscle::getSubjectMass(){ + return mSubjectMassInKg; +} +double Millard2016TorqueMuscle::getSubjectHeight(){ + return mSubjectHeightInMeters; +} diff --git a/addons/muscle/Millard2016TorqueMuscle.h b/addons/muscle/Millard2016TorqueMuscle.h index 648710d..880e310 100755 --- a/addons/muscle/Millard2016TorqueMuscle.h +++ b/addons/muscle/Millard2016TorqueMuscle.h @@ -3,7 +3,7 @@ /* * RBDL - Rigid Body Dynamics Library: Addon : muscle - * Copyright (c) 2016 Matthew Millard + * Copyright (c) 2016 Matthew Millard * * Licensed under the zlib license. See LICENSE for more details. */ @@ -13,9 +13,14 @@ #include "../geometry/SmoothSegmentedFunction.h" + namespace RigidBodyDynamics { namespace Addons { namespace Muscle{ + #ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING + class TorqueMuscleFittingToolkit; + class FitTorqueMuscleParameters; + #endif /** This struct contains an enumerated list of the data sets which @@ -96,7 +101,9 @@ namespace RigidBodyDynamics { WristSupination = 19, LumbarExtension = 20, LumbarFlexion = 21, - Last = 22 + UnitExtensor = 22, + UnitFlexor = 23, + Last = 24 }; const static char* names[]; JointTorqueSet(){} @@ -195,6 +202,8 @@ namespace RigidBodyDynamics { WristSupination = JointTorqueSet::WristSupination , LumbarExtension = JointTorqueSet::LumbarExtension, LumbarFlexion = JointTorqueSet::LumbarFlexion, + UnitExtensor = JointTorqueSet::UnitExtensor, + UnitFlexor = JointTorqueSet::UnitFlexor, LastJointTorque }; const static char* GenderNames[]; @@ -222,12 +231,12 @@ namespace RigidBodyDynamics { in the program aborting and an error message being printed to the terminal. - @param subjectHeightInMeters + @param mSubjectHeightInMeters This parameter is used to scale from the normalized curves reported by Anderson et al. See the class description for details. - @param subjectMassInKg + @param mSubjectMassInKg This parameter is used to scale from the normalized curves reported by Anderson et al. See the class description for details. @@ -240,7 +249,15 @@ namespace RigidBodyDynamics { }; + struct TorqueMuscleSummary{ + /**The angle of the fiber (rad)*/ + double fiberAngle; + + /**The angular velocity of the fiber (rad/s)*/ + double fiberAngularVelocity; + + /**The activation of the muscle*/ double activation; @@ -254,7 +271,7 @@ namespace RigidBodyDynamics { /**The normalized value of the torque-angular-velocity curve. Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ - double fiberActiveTorqueAngularVelocityMultiplier; + double fiberTorqueAngularVelocityMultiplier; /**The torque generated by the damping element (Nm/Nm)*/ double fiberNormalizedDampingTorque; @@ -267,6 +284,18 @@ namespace RigidBodyDynamics { chosen by the user. (Nm)*/ double jointTorque; + + TorqueMuscleSummary(): + fiberAngle(nan("1")), + fiberAngularVelocity(nan("1")), + activation(nan("1")), + fiberPassiveTorqueAngleMultiplier(nan("1")), + fiberActiveTorqueAngleMultiplier(nan("1")), + fiberTorqueAngularVelocityMultiplier(nan("1")), + fiberNormalizedDampingTorque(nan("1")), + fiberTorque(nan("1")), + jointTorque(nan("1")){} + }; struct TorqueMuscleInfo{ @@ -298,15 +327,16 @@ namespace RigidBodyDynamics { /** The normalized value of the passive-torque-angle curve. Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ double fiberPassiveTorqueAngleMultiplier; - + double DfiberPassiveTorqueAngleMultiplier_DblendingVariable; + double DfiberPassiveTorqueAngleMultiplier_DangleOffset; /**The normalized value of the active-torque-angle curve. Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ double fiberActiveTorqueAngleMultiplier; - + double DfiberActiveTorqueAngleMultiplier_DblendingVariable; /**The normalized value of the torque-angular-velocity curve. Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ - double fiberActiveTorqueAngularVelocityMultiplier; - + double fiberTorqueAngularVelocityMultiplier; + double DfiberTorqueAngularVelocityMultiplier_DblendingVariable; /*The normalized value of the tendon-torque-angle curve. Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ //double tendonTorqueAngleMultiplier; @@ -318,10 +348,15 @@ namespace RigidBodyDynamics { fiber (Nm)*/ double fiberActiveTorque; - /**The torque generated by the passive element of the muscle + /**The total torque generated by the passive elements of the muscle fiber (Nm)*/ double fiberPassiveTorque; + /**The torque generated by the passive elastic element of the + muscle fiber (Nm) + */ + double fiberPassiveElasticTorque; + /**The torque generated by the damping element (Nm)*/ double fiberDampingTorque; @@ -362,59 +397,166 @@ namespace RigidBodyDynamics { /**The total power output of the fiber element.(Watts - Nm/s)*/ double fiberPower; + + /** The power output by this muscle at the joint. This is + signed so that it is consistent with the sign convention of the + joint chosen by the user. (Nm/rad)*/ + double jointPower; + + /**The partial derivative of joint torque w.r.t activation*/ + double DjointTorque_Dactivation; /**The partial derivative of joint torque w.r.t the joint angle*/ - double DjointTorqueDjointAngle; + double DjointTorque_DjointAngle; /**The partial derivative of joint torque w.r.t the joint angular velocity*/ - double DjointTorqueDjointAngularVelocity; + double DjointTorque_DjointAngularVelocity; - /**The partial derivative of joint torque w.r.t activation*/ - double DjointTorqueDactivation; + ///First derivatives for the fitting algorithm + double DjointTorque_DactiveTorqueAngleBlendingVariable; + double DjointTorque_DpassiveTorqueAngleBlendingVariable; + double DjointTorque_DtorqueAngularVelocityBlendingVariable; + double DjointTorque_DmaximumIsometricTorque; + double DjointTorque_DpassiveTorqueAngleCurveAngleOffset; - /*The power output of the tendon. A positive power means that - the tendon is physically shortening. (Watts - Nm/s)*/ - //double tendonPower; + double DjointTorque_DactiveTorqueAngleAngleScaling; + double DjointTorque_DmaximumAngularVelocity; - /** The power output by this muscle at the joint. This is - signed so that it is consistent with the sign convention of the - joint chosen by the user. (Nm/rad)*/ - double jointPower; + + ///This vector contains additional quantities that are needed by + /// the fitting functions, and is not for external use. Use this + /// vector at your own risk, as its definition/sizing etc may + /// change. + RigidBodyDynamics::Math::VectorNd fittingInfo; TorqueMuscleInfo(): - jointAngle(nan("1")), - jointAngularVelocity(nan("1")), - fiberAngle(nan("1")), - fiberAngularVelocity(nan("1")), - fiberPassiveTorqueAngleMultiplier(nan("1")), - fiberActiveTorqueAngleMultiplier(nan("1")), - fiberActiveTorqueAngularVelocityMultiplier(nan("1")), - activation(nan("1")), - fiberActiveTorque(nan("1")), - fiberPassiveTorque(nan("1")), - fiberTorque(nan("1")), - jointTorque(nan("1")), - fiberStiffness(nan("1")), - jointStiffness(nan("1")), - fiberActivePower(nan("1")), - fiberPassivePower(nan("1")), - fiberPower(nan("1")), - jointPower(nan("1")){} + jointAngle(nan("1")), + jointAngularVelocity(nan("1")), + fiberAngle(nan("1")), + fiberAngularVelocity(nan("1")), + fiberPassiveTorqueAngleMultiplier(nan("1")), + DfiberPassiveTorqueAngleMultiplier_DblendingVariable(nan("1")), + DfiberPassiveTorqueAngleMultiplier_DangleOffset(nan("1")), + fiberActiveTorqueAngleMultiplier(nan("1")), + DfiberActiveTorqueAngleMultiplier_DblendingVariable(nan("1")), + fiberTorqueAngularVelocityMultiplier(nan("1")), + DfiberTorqueAngularVelocityMultiplier_DblendingVariable(nan("1")), + activation(nan("1")), + fiberActiveTorque(nan("1")), + fiberPassiveTorque(nan("1")), + fiberPassiveElasticTorque(nan("1")), + fiberDampingTorque(nan("1")), + fiberNormDampingTorque(nan("1")), + fiberTorque(nan("1")), + jointTorque(nan("1")), + fiberStiffness(nan("1")), + jointStiffness(nan("1")), + fiberActivePower(nan("1")), + fiberPassivePower(nan("1")), + fiberPower(nan("1")), + jointPower(nan("1")), + DjointTorque_Dactivation(nan("1")), + DjointTorque_DjointAngle(nan("1")), + DjointTorque_DjointAngularVelocity(nan("1")), + DjointTorque_DactiveTorqueAngleBlendingVariable(nan("1")), + DjointTorque_DpassiveTorqueAngleBlendingVariable(nan("1")), + DjointTorque_DtorqueAngularVelocityBlendingVariable(nan("1")), + DjointTorque_DmaximumIsometricTorque(nan("1")), + DjointTorque_DpassiveTorqueAngleCurveAngleOffset(nan("1")), + DjointTorque_DactiveTorqueAngleAngleScaling(nan("1")), + DjointTorque_DmaximumAngularVelocity(nan("1")) + {} + + }; + + struct TorqueMuscleParameterFittingData { + + unsigned int indexAtMaximumActivation; + unsigned int indexAtMinimumActivation; + unsigned int indexAtMaxPassiveTorqueAngleMultiplier; + + bool isTorqueMuscleActive; + + double activeTorqueAngleBlendingVariable; + double passiveTorqueAngleBlendingVariable; + double torqueVelocityBlendingVariable; + double passiveTorqueAngleCurveOffset; + double maximumActiveIsometricTorque; + + double activeTorqueAngleAngleScaling; + double maximumAngularVelocity; + + //double objectiveValue; + //double constraintError; + bool fittingConverged; + + TorqueMuscleSummary summaryDataAtMinimumActivation; + TorqueMuscleSummary summaryDataAtMaximumActivation; + TorqueMuscleSummary summaryDataAtMaximumPassiveTorqueAngleMultiplier; + + + TorqueMuscleParameterFittingData(): + indexAtMaximumActivation(std::numeric_limits::infinity()), + indexAtMinimumActivation(std::numeric_limits::infinity()), + indexAtMaxPassiveTorqueAngleMultiplier(std::numeric_limits::infinity()), + isTorqueMuscleActive(true), + activeTorqueAngleBlendingVariable(nan("1")), + passiveTorqueAngleBlendingVariable(nan("1")), + torqueVelocityBlendingVariable(nan("1")), + passiveTorqueAngleCurveOffset(nan("1")), + maximumActiveIsometricTorque(nan("1")), + activeTorqueAngleAngleScaling(nan("1")), + maximumAngularVelocity(nan("1")), + fittingConverged(false), + summaryDataAtMinimumActivation(), + summaryDataAtMaximumActivation(), + summaryDataAtMaximumPassiveTorqueAngleMultiplier(){} + }; + + struct TorqueMuscleDataFeatures{ + unsigned int indexOfMaxActivation; + unsigned int indexOfMinActivation; + unsigned int indexOfMaxPassiveTorqueAngleMultiplier; + + bool isInactive; + + TorqueMuscleSummary summaryAtMaxActivation; + TorqueMuscleSummary summaryAtMinActivation; + TorqueMuscleSummary summaryAtMaxPassiveTorqueAngleMultiplier; }; /** - This class implements a rigid-tendon torque muscle for a growing - list of joints and torque-directions. This rigid-tendon torque - muscle model provides modeling support for 3 phenomena + \brief This class implements a rigid-tendon muscle-torque-generator + (MTG) for a growing list of joints and torque-directions. For a + detailed description of the MTGs available and the automatic + fitting routine (implemented in TorqueMuscleFittingToolkit) please + see the publication: M.Millard, A.L.Kleesattel, M.Harant, & + K.Mombaur. A reduced muscle model and planar musculoskeletal model + fit for the synthesis of whole body movements. Journal of + Biomechanics. (under review as of August 2018) + + This rigid-tendon torque muscle model provides modeling support + for 3 phenomena - torque-angle curve (\f$\mathbf{t}_A(\theta)\f$): the variation of active isometric torque in one direction as a function of joint angle - torque-velocity curve (\f$\mathbf{t}_V(\dot{\theta})\f$): the variation of torque as a function of angular velocity - passive-torque-angle curve (\f$\mathbf{t}_P(\theta-\theta_S)\f$): the variation of passive torque as a function of joint angle. Here \f$s_P\f$ and \f$\theta_S\f$ are user-defined scaling and shift parameters. each of which are represented as smooth normalized curves that - vary between 0 and 1. These three phenomena are used to compute the + vary between 0 and 1. A series of scaling + (\f$s^A, s^V, s^\tau\f$), shifting (\f$\Delta^P\f$), and blending + (\f$\lambda^A,\lambda^V,\lambda^P\f$) variables have been introduced + to these curves to make it possible to easily fit these curves to + a specific subject. These fitting variables can be set/fitted by + making use of the functions in this class. Alternatively these + fitting variables can be automatically adjusted (using IPOPT) by + making use of the TorqueMuscleFittingToolkit. + + \image html fig_MuscleAddon_BlendableTorqueMuscle.png "Examples of the adjustable characteristic curves" + + These three curves are used to compute the torque developed \f$\tau\f$ given the angle of the joint \f$\theta\f$, the angular-velocity of the joint \f$\dot{\theta}\f$, and the activation of the muscle \f$\mathbf{a}\f$ (a 0-1 quantity @@ -428,11 +570,11 @@ namespace RigidBodyDynamics { The damping term \f$\beta\f$ is necessary to supress vibration that will occur as the passive element \f$\mathbf{t}_P\f$ is streched, its stiffness increases, and the natural frequency of the overall - system rises. By default \f$\beta\f$ is set to 0.1 which has proven effective - for supressing vibration in the trunk segments during a stoop lift in which the - stiffness of the lumbar back muscles grows appreciably. - This model does not yet provide support for the following phenomena - but will in the future. + system rises. By default \f$\beta\f$ is set to 0.1 which has proven + effective for supressing vibration in the trunk segments during a + stoop lift in which the stiffness of the lumbar back muscles grows + appreciably. This model does not yet provide support for the + following phenomena but will in the future. - activation dynamics: currently is left to the user - tendon-elasticity @@ -484,9 +626,9 @@ namespace RigidBodyDynamics { students, Dolan et al from 126 women and 23 men, and Raschke from 5 male subjects) scaling has been used to make the strength of the subject consistent. Scaling coefficients for the lower body, shoulders and - elbow, and forearm/wrist using measurements that overlapped between - datasets. Presently this data set includes curves for 22 joint and - direction specific torque muscles. + elbow, and forearm/wrist have been calculated using measurements + that overlapped between datasets. Presently this data set includes + curves for 22 torque muscles. - Number of subjects: 1 elite gymnast (69.6 kg, 1.732 m) - Gender: male @@ -567,17 +709,17 @@ namespace RigidBodyDynamics { This error has been corrected by fitting a Bezier curve to a Hill-type curve that passes through the point where \f$\dot{\theta}= \frac{1}{2} \dot{\theta}_{MAX}\f$ - -# Anderson et al.'s, Jackson, and Kentel et al. had discintinuities + -# Anderson et al.'s, Jackson, and Kentel et al. had discontinuities in the first derivative of the force velocity curve at \f$\dot{\theta}=0\f$. While this follows Huxley's famous observations - that the slope does discontinuously change at at \f$\dot{\theta}=0\f$. - This is not a phenomena that is not compatible with most optimal + that the slope does discontinuously change at at \f$\dot{\theta}=0\f$, + this is not a phenomena that is not compatible with most optimal control formulations and thus this discontinuity is not present in the force velocity curves used in this model. -# Anderson et al. and Kentel et al.'s active-torque-angle curves can achieve negative values - this is obviously undesirable as it will allow a muscle to push. - -# Kentel et al.'s activation inhibiition function does not always + -# Kentel et al.'s activation inhibition function does not always cross 1.0 for \f$\dot{\theta}=0\f$, which means that \f$\tau_{ISO}\f$ is not reached. This makes for a confusing model to use. @@ -612,7 +754,7 @@ namespace RigidBodyDynamics { Internally the sign of the fiber velocity follows signOfJointTorque so that the signs of joint power and muscle power are consistent. - Strength Scaling + Strength Scaling: Anderson2007 Data Set The leg strength (here we mean \f$\tau_{ISO}\f$) predicted by Anderson et al.'s curves should be taken @@ -621,12 +763,15 @@ namespace RigidBodyDynamics { from active people: they did not include people at the extremes (both very weak, and very strong), nor did they include children. Finally, the torques produced by each subject were normalized by - subjectMassInKg*subjectHeightInM*accelerationDueToGravity. Strength + mSubjectMassInKg*subjectHeightInM*accelerationDueToGravity. Strength is a strange phenomena which is not nicely normalized by just these quantites, and so the strength predicted by Anderson et al.'s curves might not fit your subject even if they are represented in Anderson et al.'s data set. + + Strength Scaling: Gymnast Data Set + The strength used in the Gymnast data set is fitted to an elite male gymnast. It goes without saying that an elite gymnast has strength proportions, and an absolute strength that are not typical. @@ -642,10 +787,20 @@ namespace RigidBodyDynamics { If you happen to know the maximum-isometric-active-torque (note this does not include the passive component) that your subject can - produce,you can update the strength of the torque-muscle using the + produce, you can update the strength of the torque-muscle using the functions getMaximumActiveIsometricTorque(), and setMaximumActiveIsometricTorque(). + Fitting to a Specific Subject + + If you have recorded the motions and external forces acting on a + subject during a movement of interest, you can make use of the + TorqueMuscleFittingToolkit to adjust the MTG so that it is strong + enough and flexible enough to allow a model of the subject to + reproduce the experimental data. Please see the + TorqueMuscleFittingToolkit class for details. + + Limitations This rigid-tendon torque muscle has some limitations that you should @@ -655,14 +810,15 @@ namespace RigidBodyDynamics { the mechanical work done by this torque actuator will greatly differ from the positive mechanical work done by a torque actuator that includes an elastic tendon. This difference is greatest for those - muscles with long tendons - namely the Achilles tendon. If you are + muscles with long tendons - namely the Achilles tendon in humans. + If you are interested in fiber kinematics, fiber work, or metabolic energy consumption you cannot use this model especially for muscles that have long tendons. -# This model formulation predicts torque well, but does a poor job of predicting joint stiffness. In this model stiffness is given by the partial derivative of torque w.r.t. joint angle. Since the - active-torque-angle curve fits a cosine function, it is possible + active-torque-angle curve fits a bell-shaped curve, it is possible to construct a torque muscle that has a region of physically impossible negative stiffness. Real muscle, in constrast, always has a positive stiffness even on the descending limb of the @@ -698,6 +854,16 @@ namespace RigidBodyDynamics { -Time: second -Power: Nm/second + Developer Notes + + All of the numerical evaluations of this model take place + in a few private stateless functions: + -updateTorqueMuscleSummary + -updTorqueMuscleSummaryCurveValues + -updTorqueMuscleInfo + -updInvertTorqueMuscleSummary + If you want to change something fundamental about the + mathematics of the model, then you have to update these functions. References @@ -754,6 +920,10 @@ namespace RigidBodyDynamics { */ class Millard2016TorqueMuscle { +#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING + friend class TorqueMuscleFittingToolkit; + friend class FitTorqueMuscleParameters; +#endif public: /** @@ -791,6 +961,11 @@ namespace RigidBodyDynamics { deviation at the wrist, pronation/supination of the wrist, and others as the list of directions grows. + Note: blending variables + + When this constructor is called the blending variables for the + active-torque-angle-curve, torque-angular-velocity-curve, and + passive-torque-angle-curve are set to 0. @param dataSet The desired source of joint torque @@ -830,8 +1005,8 @@ namespace RigidBodyDynamics { @throws abort() when -# The combination of dataSet, gender, joint, and jointDirection does not correspond to a valid entry - -# subjectHeightInMeters <= 0 - -# subjectMassInKg <= 0 + -# mSubjectHeightInMeters <= 0 + -# mSubjectMassInKg <= 0 -# abs(signOfJointTorque)-1 > epsilon */ @@ -903,16 +1078,19 @@ namespace RigidBodyDynamics { @param jointTorque (Nm). - @param tms - a TorqueMuscleSummary struct which contains the - calculated activation along with all of the - internal parameters of the muscle so that you can - understand why the activation value takes the - value that it does. + @param updTorqueMuscleSummaryStruct + TorqueMuscleSummary struct which contains the + calculated activation along with all of the + internal parameters of the muscle so that you can + understand why the activation value takes the + value that it does. See the TorqueMuscleSummary + (in this file) for details. */ void calcActivation(double jointAngle, double jointAngularVelocity, double jointTorque, - TorqueMuscleSummary &tms) const; + TorqueMuscleSummary + &updTorqueMuscleSummaryStruct) const; /** @@ -956,6 +1134,7 @@ namespace RigidBodyDynamics { Calculates a large number of internal quantities of the torque muscle ranging from the values of the muscle's components, the stiffness of the muscle, and its power output. + See the struct TorqueMuscleInfo (in this file) for details. @param jointAngle (radians) @@ -968,13 +1147,13 @@ namespace RigidBodyDynamics { you must ensure after the fact that your activations fall within a bound of [0,1]. - @param torqueMuscleInfoStruct: A torque muscle struct + @param updTorqueMuscleInfoStruct: A torque muscle struct */ void calcTorqueMuscleInfo( double jointAngle, double jointAngularVelocity, double activation, - TorqueMuscleInfo& torqueMuscleInfoStruct) const; + TorqueMuscleInfo& updTorqueMuscleInfoStruct) const; /** @return the sign of the joint torque (+/- 1) @@ -1007,12 +1186,21 @@ namespace RigidBodyDynamics { */ double getJointAngleAtMaximumActiveIsometricTorque() const; + /** + @return the width of the active torque angle curve. + Note that this will return the angular width of + the non-blended curve. + Angle is in radians. + + */ + double getActiveTorqueAngleCurveWidth() const; + /** @return the joint angle at which the normalized passive-torque-angle curve reaches a value of 1.0. If this curve never reaches a value of 1.0 (because it is flat, or the - passiveTorqueScale has been set to 0) a value + mPassiveTorqueScale has been set to 0) a value of std::numeric_limits::signaling_NaN() is returned. Use the std function isfinite to test if a signaling_NaN has been returned. @@ -1020,6 +1208,15 @@ namespace RigidBodyDynamics { */ double getJointAngleAtOneNormalizedPassiveIsometricTorque() const; + /** + @return the joint angle at which the normalized + passive-torque-angle curve reaches its + minimum value + Angle is in radians + */ + double getJointAngleAtSmallestNormalizedPassiveIsometricTorque() const; + + /** @return the maximum concentric angular velocity in radians/sec. This scalar positive quantity @@ -1036,7 +1233,9 @@ namespace RigidBodyDynamics { /** @return the angle \f$\theta_S\f$ that the passive curve has - been shifted (radians). + been shifted. Note that the sign convention is in the sense + of the fiber angle, not in the sense of the joint angle. + (radians). */ double getPassiveCurveAngleOffset() const; @@ -1066,25 +1265,30 @@ namespace RigidBodyDynamics { void setPassiveTorqueScale(double passiveTorqueScale); /** - @param passiveCurveAngleOffsetVal the angle \f$\theta_S\f$ - that the passive curve should be shifted. Angles in radians + @param passiveCurveAngleOffsetVal the angle \f$\Delta_p\f$ + that the passive curve should be shifted. Note that the + sign convention is in the sense of the fiber angle, not + in the sense of the joint angle. Angles in radians */ void setPassiveCurveAngleOffset( double passiveCurveAngleOffsetVal); + double getTorqueVelocityMultiplierAtHalfOmegaMax() const; + void setTorqueVelocityMultiplierAtHalfOmegaMax(double tvAtHalfOmegaMax); + /** - This function iteratively solves for the passiveTorqueScale + This function iteratively solves for the scaling so that at the specified jointAngle the passive curve develops the specified passiveTorque @param jointAngle the target joint angle in radians - @param passiveTorque the target passive joint torque in Nm. + @param passiveFiberTorque the target passive joint torque in Nm. @throws abort if jointAngle is not in the domain of the curve - @throws abort if passiveTorque < sqrt(eps) + @throws abort if passiveFiberTorque < sqrt(eps) */ void fitPassiveTorqueScale(double jointAngle, - double passiveTorque); + double passiveFiberTorque); /** This function solves for the passive curve angle offset @@ -1092,11 +1296,11 @@ namespace RigidBodyDynamics { develops the specified passiveTorque @param jointAngle the target joint angle in radians - @param passiveTorque the target passive joint torque in Nm. + @param passiveFiberTorque the target passive joint torque in Nm. @throws abort if passiveTorque < sqrt(eps) */ void fitPassiveCurveAngleOffset(double jointAngle, - double passiveTorque); + double passiveFiberTorque); /** Sets the strength of the muscle to match a desired value. @@ -1106,7 +1310,7 @@ namespace RigidBodyDynamics { muscle (Nm) */ - void setMaximumActiveIsometricTorque( + void setMaximumActiveIsometricTorque( double maxIsometricTorque); /** @@ -1114,89 +1318,104 @@ namespace RigidBodyDynamics { angular velocity magnitude in radians/sec. This scalar quantity is not signed and must be greater than 0. */ - void setMaximumConcentricJointAngularVelocity(double maxAngularVelocity); + void setMaximumConcentricJointAngularVelocity( + double maxAngularVelocity); /** - @return the SmoothSegmentedFunction the has been fitted to - Anderson et al.'s passive torque angle curve. + @return the SmoothSegmentedFunction represents the + active-torque-angle curve profile. */ + const RigidBodyDynamics::Addons::Geometry:: SmoothSegmentedFunction& getActiveTorqueAngleCurve() const; - + /** - @return the SmoothSegmentedFunction the has been fitted to - Anderson et al.'s active torque angle curve. + @return the SmoothSegmentedFunction that represents the + passive-torque-angle curve. */ + const RigidBodyDynamics::Addons::Geometry:: SmoothSegmentedFunction& getPassiveTorqueAngleCurve() const; - + /** - @return the SmoothSegmentedFunction the has been fitted to - Anderson et al.'s torque velocity curve. + @return the SmoothSegmentedFunction the represents the + torque-angular-velocity curve. */ const RigidBodyDynamics::Addons::Geometry:: SmoothSegmentedFunction& getTorqueAngularVelocityCurve() const; + + /** + @returns Value of \f$\lambda^A\f$ that is currently in use. + */ + double getActiveTorqueAngleCurveBlendingVariable() const; + + /** + @returns Value of \f$\lambda^P\f$ that is currently in use. + */ + double getPassiveTorqueAngleCurveBlendingVariable() const; + + /** + @returns Value of \f$\lambda^V\f$ that is currently in use. + */ + double getTorqueAngularVelocityCurveBlendingVariable() const; + /** - To set the active-torque-angle curve to a custom made - Bezier curve use this function to get a writeable reference - to the object that is used to store the Bezier curve - parameters for the active-torque-angle curve. To return back - to using the default curve simply call the function - useDefaultActiveTorqueAngleCurve(). - - @return a writeable reference to the - SmoothSegmentedFunction for the active torque angle curve. + @param blendingVariable [0-1] + The desired value of \f$\lambda^A\f$. Note at + \f$\lambda^A=1\f$ the curve is a horizontal line + with a value of 1. */ - //RigidBodyDynamics::Addons::Geometry:: - //SmoothSegmentedFunction& updActiveTorqueAngleCurve() ; + void setActiveTorqueAngleCurveBlendingVariable( + double blendingVariable); /** - Calling this function will make the muscle model use the - default active-torque-angle curve. + @param blendingVariable [0-1] + The desired value of \f$\lambda^P\f$. Note at + \f$\lambda^P=1\f$ the curve is a horizontal line + with a value of 0. */ - //bool useDefaultActiveTorqueAngleCurve(); + void setPassiveTorqueAngleCurveBlendingVariable( + double blendingVariable); /** - To set the passive-torque-angle curve to a custom made - Bezier curve use this function to get a writeable reference - to the object that is used to store the Bezier curve - parameters for the passive-torque-angle curve. To return back - to using the default curve simply call the function - useDefaultPassiveTorqueAngleCurve(). Note that the offset angle - and scale parameters are not applied to the custom curve. - - @return a writeable reference to the - SmoothSegmentedFunction for the passive torque angle curve. + @param blendingVariable [0-1] + The desired value of \f$\lambda^V\f$. Note at + \f$\lambda^V=1\f$ the curve is a horizontal line + with a value of 1. */ - //const RigidBodyDynamics::Addons::Geometry:: - //SmoothSegmentedFunction& updPassiveTorqueAngleCurve(); + void setTorqueAngularVelocityCurveBlendingVariable( + double blendingVariable); /** - Calling this function will make the muscle model use the - default passive-torque-angle curve. + @returns the scaling parameter that is applied to the + domain of the active-torque-angle curve. Note that + a value greater than 1 means that the curve has + been stretched. */ - //bool useDefaultPassiveTorqueAngleCurve(); + double getActiveTorqueAngleCurveAngleScaling() const; /** - To set the torque-angular-velicity curve to a custom made - Bezier curve use this function to get a writeable reference - to the object that is used to store the Bezier curve - parameters for the active-torque-angle curve. To return back - to using the default curve simply call the function - useDefaultTorqueAngularVelocityCurve(). - - @return a writeable reference to the - SmoothSegmentedFunction for the torque angular velocity curve. + @param angleScaling + Scale the domain of the active torque angle curve. + This scaling is applied so that the angle at which + the curve peaks remains unchanged. Note that an + angleScaling > 1 means that the typically bell-shaped + curve will be wider. */ - //const RigidBodyDynamics::Addons::Geometry:: - //SmoothSegmentedFunction& updTorqueAngularVelocityCurve(); + void setActiveTorqueAngleCurveAngleScaling(double angleScaling); + /** - Calling this function will make the muscle model use the - default passive-torque-angle curve. + @param fittedParameters: the structure returned after + one of the optimization-based fitting functions from + TorqueMuscleFittingToolkit has been called. If the + fitting was successful, calling this function will + update all of the adjusted parameters. */ - //bool useDefaultTorqueAngularVelocityCurve(); + void setFittedParameters ( + const TorqueMuscleParameterFittingData &fittedParameters); + /** Prints 2 csv files: @@ -1225,66 +1444,76 @@ namespace RigidBodyDynamics { std::string getName(); void setName(std::string& name); - private: - /** - @return the parameters c1,...,c6 that desribe the - active-torque-angle and torque-velocity curves of this - torque muscle model. See the Anderson et al. paper metioned - in the class description for detail. - */ - //const RigidBodyDynamics::Math::VectorNd& - //getParametersC1C2C3C4C5C6(); + DataSet::item getDataSet(); + GenderSet::item getGender(); + AgeGroupSet::item getAgeGroup(); + JointTorqueSet::item getJointTorque(); + double getSubjectMass(); + double getSubjectHeight(); - /** - @return the parameters b1,k1,b2,k2 that desribe the - passive-torque-angle curves of this model. See the Anderson - et al. paper metioned in the class description for detail. - */ - //const RigidBodyDynamics::Math::VectorNd& - //getParametersB1K1B2K2(); + private: - bool muscleCurvesAreDirty; + bool mMuscleCurvesAreDirty; void updateTorqueMuscleCurves(); - TorqueMuscleInfo tmInfo; + TorqueMuscleInfo mTmInfo; + TorqueMuscleSummary mTmSummary; RigidBodyDynamics::Addons::Geometry:: - SmoothSegmentedFunction taCurve; - RigidBodyDynamics::Addons::Geometry:: - SmoothSegmentedFunction tpCurve; + SmoothSegmentedFunction mTaCurve; + double mTaLambda; + static const double mTaLambdaMax;// = 1.0 -> defined in the cc file RigidBodyDynamics::Addons::Geometry:: - SmoothSegmentedFunction tvCurve; + SmoothSegmentedFunction mTpCurve; + double mTpLambda; + static const double mTpLambdaMax;// = 0.0 -> defined in the cc file RigidBodyDynamics::Addons::Geometry:: - SmoothSegmentedFunction betaCurve; + SmoothSegmentedFunction mTvCurve; + double mTvLambda; + + ///This is set to the maximum eccentric value of the + /// torque-velocity curve so that the optimization problem + /// of solving for mTaLambda, mTvLambda & + /// mMaxActiveIsometricTorque is convex. This parameter is + /// updated in updateTorqueMuscleCurves + double mTvLambdaMax; - RigidBodyDynamics::Math::VectorNd c1c2c3c4c5c6Anderson2007; - RigidBodyDynamics::Math::VectorNd b1k1b2k2Anderson2007; - RigidBodyDynamics::Math::VectorNd gymnastParams; - DataSet::item dataSet; + RigidBodyDynamics::Math::VectorNd mAnderson2007c1c2c3c4c5c6; + RigidBodyDynamics::Math::VectorNd mAnderson2007b1k1b2k2; + RigidBodyDynamics::Math::VectorNd mGymnastParams; - bool useTabularOmegaMax; - bool useTabularMaxActiveIsometricTorque; + DataSet::item mDataSet; + GenderSet::item mGender; + AgeGroupSet::item mAgeGroup; + JointTorqueSet::item mJointTorque; + bool mUseTabularOmegaMax; + bool mUseTabularMaxActiveIsometricTorque; + bool mUseTabularTorqueVelocityMultiplierAtHalfOmegaMax; - double maxActiveIsometricTorque; - double angleAtOneNormActiveTorque; - double omegaMax; - double angleAtOneNormPassiveTorque; - double passiveTorqueScale; - double passiveCurveAngleOffset; + double mTorqueVelocityMultiplierAtHalfOmegaMax; - double betaMax; //passive damping coefficient + double mMaxActiveIsometricTorque; + double mAngleAtOneNormActiveTorque; + double mTaAngleScaling; + double mOmegaMax; + double mAngleAtOneNormPassiveTorque; + double mAngleAtSmallestNormPassiveTorque; + double mPassiveTorqueScale; + double mPassiveCurveAngleOffset; - double subjectHeightInMeters; - double subjectMassInKg; - double scaleFactorAnderson2007; + double mBetaMax; //passive damping coefficient - double signOfJointAngle; - double signOfConcentricAnglularVelocity; - double signOfJointTorque; - double angleOffset; + double mSubjectHeightInMeters; + double mSubjectMassInKg; + double mScaleFactorAnderson2007; - std::string muscleName; + double mSignOfJointAngle; + double mSignOfConcentricAnglularVelocity; + double mSignOfJointTorque; + double mAngleOffset; + + std::string mMuscleName; double calcJointAngle(double fiberAngle) const; double calcFiberAngle(double jointAngle) const; @@ -1297,9 +1526,326 @@ namespace RigidBodyDynamics { //getAnderson2007ParameterMatrix(); static double const Anderson2007Table3Mean[36][14]; static double const Anderson2007Table3Std[36][14]; - static double const GymnastWholeBody[22][12]; + static double const GymnastWholeBody[24][12]; + + + void calcTorqueMuscleDataFeatures( + RigidBodyDynamics::Math::VectorNd const &jointTorque, + RigidBodyDynamics::Math::VectorNd const &jointAngle, + RigidBodyDynamics::Math::VectorNd const &jointAangularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double torqueVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActiveIsometricTorque, + TorqueMuscleDataFeatures &tmf) const; + + /** + This function will calculate the angle at which the blendable + passive torque angle curve develops a desired torque. Note + that if the blendingVariable is set to 1.0 then the curve is + y = 0, and a NaN will be returned. + + @param normPassiveFiberTorque: the normalized passive fiber torque + @param blendingVariable: A variable between [0,1] where 0 + is equivalent to using the default passive-torque-angle + curve and 1 will set this curve to 0. Any value between + 0-1 will smoothly interpolate between these extremes. + @param passiveTorqueAngleCurveOffset: the angular shift of the + passive-torque-angle curve. + @returns fiber angle in radians at which the + blendable passive curve developes the desired curve. + */ + double calcFiberAngleGivenNormalizedPassiveTorque( + double normPassiveFiberTorque, + double blendingVariable, + double passiveTorqueAngleCurveOffset) const; + + /** + This function will calculate the value of a blended smooth + segmented curve \f$\mathbf{f}(\mathbf{c}, a,\lambda,\lambda^{max})\f$, + where \f$\mathbf{c}\f$ is a smooth-segmented-curve + \f[ + \mathbf{f}(a,\lambda,\lambda^{max},\mathbf{c}()) = \mathbf{c}(a) + (1-\lambda) + \lambda\lambda^{max} + \f] + + Note + At the moment this function only returns first derivatives. + + @param curveArgument: [\f$-\infty,infty\f$] + the argument \f$a\f$ to the smooth-segmented-curve + \f$\mathbf{c}()\f$. Units: depend on the curve. + + @param blendingVariable: [0-1]. + the value of \f$\lambda\f$ the blending variable. + Unitless + + @param maximumBlendingValue: the scalar value the blended function + tends to with a blending variable \f$\lambda=1\f$. + + @param derivativeOrderArgument: [0,1]. + The order of the derivative of the + SmoothSegmentedFunction w.r.t. + its scalar input argument. + + @param derivativeOrderBlendingVariable: [0,1]. + This parameter is set to choose the derivative order + with respect to the blending variable. + + @param curve: the SmoothSegmentedCurve to evaluate. + + @returns the value of the blended curve or its derivative + */ + double calcBlendedCurveDerivative( + double curveArgument, + double blendingVariable, + double maximumBlendingValue, + unsigned int derivativeOrderArgument, + unsigned int derivativeOrderBlendingVariable, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction + const &curve) const; + + + /** + This function will calculate the inverse value of a blended smooth + segmented curve, that is \f$a\f$, in the function + \f$\mathbf{f}(\mathbf{c}, a,\lambda,\lambda^{max})\f$, + where \f$\mathbf{c}\f$ is a smooth-segmented-curve + \f[ + \mathbf{f}(a,\lambda,\lambda^{max},\mathbf{c}()) = \mathbf{c}(a) + (1-\lambda) + \lambda\lambda^{max} + \f] + + Note + At the moment this function only returns first derivatives. + + + @param blendedCurveValue: + the value of \f$\mathbf{f}(a,\lambda,\lambda^{max},\mathbf{c}())\f$ + + @param argGuess: + an initial guess for the value of \f$a\f$, which is necessary + as not all blended functions are monotonic. + + @param blendingVariable: [0-1]. + the value of \f$\lambda\f$ the blending variable. + Unitless + + @param maximumBlendingValue: the scalar value the blended function + tends to with a blending variable \f$\lambda=1\f$. + + @param curve: the SmoothSegmentedCurve to evaluate. + + @returns the value of the argument \f$a\f$ which will evaluate + to the given value of \f$\mathbf{f}(a,\lambda,\lambda^{max},\mathbf{c}())\f$ + */ + double calcInverseBlendedCurveValue( + double blendedCurveValue, + double argGuess, + double blendingVariable, + double maximumBlendingValue, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction + const &curve) const; + + /** + This function evaluates all of the quantities needed to + compute the torque the muscle is generating but nothing extra. + + + @param activation: the activation of the muscle which can + range from 0-1. + @param jointAngle: the angle of the joint(radians) + @param jointAngularVelocity: the angular velocity of the joint + ((rad/s)) + @param activeTorqueAngleBlendingVariable: the blending variable + associated with the active-torque-angle curve + @param passiveTorqueAngleBlendingVariable: the blending variable + associated with the passive-torque-angle curve + @param activeTorqueAngularVelocityBlendingVariable: the blending + variable associated with the torque-angular velocity + curve. + @param activeTorqueAngleAngleScaling: + the scaling of the angle. By default this is set to 1.0, + but if desired, the active torque angle curve can be + re-scaled to make it wider or narrower. This scaling is + done such that the angular location of peak + active-torque-angle multiplier does not change. + @param activeTorqueAngleAtOneNormTorque: + the angle at which the torque-angle curve hits a + normalized value of 1.0. + @param passiveTorqueAngleCurveOffset: the angular shift of the + passive-torque-angle curve. + @param maxAngularVelocity: + the maximum angular velocity of the muscle - used + to normalize the angular velocity prior to evaluating + the force-velocity curve. + @param maxActiveIsometricTorque: the maximum isometric torque + @param updTms: the torque-muscle-summary structure to be + updated. + */ + void updTorqueMuscleSummary( + double activation, + double jointAngle, + double jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double activeTorqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActiveIsometricTorque, + TorqueMuscleSummary &updTms) const; + /** + This function evaluates the curves values that are a part of the + torque-muscle-summary structure. + + @param fiberAngle: the angle of the muscle fiber (radians) + @param normFiberAngularVelocity: the normalized angular + velocity of the fiber ((rad/s)/(rad/s)) + @param activeTorqueAngleBlendingVariable: the blending variable + associated with the active-torque-angle curve + @param passiveTorqueAngleBlendingVariable: the blending variable + associated with the passive-torque-angle curve + @param activeTorqueAngularVelocityBlendingVariable: the blending + variable associated with the torque-angular velocity + curve. + @param activeTorqueAngleAngleScaling: + the scaling of the angle. By default this is set to 1.0, + but if desired, the active torque angle curve can be + re-scaled to make it wider or narrower. This scaling is + done such that the angular location of peak + active-torque-angle multiplier does not change. + @param activeTorqueAngleAtOneNormTorque: + the angle at which the torque-angle curve hits a + normalized value of 1.0. + @param passiveTorqueAngleCurveOffset: the angular shift of the + passive-torque-angle curve. + @param updTms: the torque-muscle-summary structure to be + updated. + */ + void updTorqueMuscleSummaryCurveValues( + double fiberAngle, + double normFiberAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double activeTorqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + TorqueMuscleSummary &updTms) const; + + /** + This function evaluates the curves values and derivatives in + the TorqueMuscleInfo structue that are functions of fiberAngle + and the normalized fiber angular velocity. + + @param activation: the activation of the muscle [0-1] + @param jointAngle: the angle of the joint(radians) + @param jointAngularVelocity: the angular velocity of the joint + ((rad/s)) + @param activeTorqueAngleBlendingVariable: the blending variable + associated with the active-torque-angle curve + @param passiveTorqueAngleBlendingVariable: the blending variable + associated with the passive-torque-angle curve + @param activeTorqueAngularVelocityBlendingVariable: the blending + variable associated with the torque-angular velocity + curve. + @param activeTorqueAngleAngleScaling: + the scaling of the angle. By default this is set to 1.0, + but if desired, the active torque angle curve can be + re-scaled to make it wider or narrower. This scaling is + done such that the angular location of peak + active-torque-angle multiplier does not change. + @param activeTorqueAngleAtOneNormTorque: + the angle at which the torque-angle curve hits a + normalized value of 1.0. + @param passiveTorqueAngleCurveOffset: the angular shift of the + passive-torque-angle curve. + @param maxAngularVelocity: + the maximum angular velocity of the muscle - used + to normalize the angular velocity prior to evaluating + the force-velocity curve. + @param maxActiveIsometricTorque: + the maximum active isometric torque the muscle can + generate + @param updTmi: the torque-muscle-info structure to be + updated. + */ + void updTorqueMuscleInfo( + double activation, + double jointAngle, + double jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double activeTorqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActiveIsometricTorque, + TorqueMuscleInfo &updTmi) const; + + /** + This function evaluates all of the quantities needed to + compute the activation of the muscle given its torque. In this + case the activation assigned to the TorqueMuscleSummary + structure is allowed to go outside of the bounds [0,1]. + + @param jointTorque: the torque generated by the muscle at the + joint (Nm) + @param jointAngle: the angle of the joint(radians) + @param jointAngularVelocity: the angular velocity of the joint + ((rad/s)) + @param activeTorqueAngleBlendingVariable: the blending variable + associated with the active-torque-angle curve + @param passiveTorqueAngleBlendingVariable: the blending variable + associated with the passive-torque-angle curve + @param activeTorqueAngularVelocityBlendingVariable: the blending + variable associated with the torque-angular velocity + curve. + @param activeTorqueAngleAngleScaling: + the scaling of the angle. By default this is set to 1.0, + but if desired, the active torque angle curve can be + re-scaled to make it wider or narrower. This scaling is + done such that the angular location of peak + active-torque-angle multiplier does not change. + @param activeTorqueAngleAtOneNormTorque: + the angle at which the torque-angle curve hits a + normalized value of 1.0. + @param passiveTorqueAngleCurveOffset: the angular shift of the + passive-torque-angle curve. + @param maxAngularVelocity: + the maximum angular velocity of the muscle - used + to normalize the angular velocity prior to evaluating + the force-velocity curve. + @param maxActiveIsometricTorque: the maximum isometric torque + @param updTms: the torque-muscle-summary structure to be + updated. + + */ + void updInvertTorqueMuscleSummary( + double jointTorque, + double jointAngle, + double jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double activeTorqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActiveIsometricTorque, + TorqueMuscleSummary &updTms) const; + + + +}; - }; @@ -1307,4 +1853,6 @@ namespace RigidBodyDynamics { } } + + #endif diff --git a/addons/muscle/MuscleFunctionFactory.cc b/addons/muscle/MuscleFunctionFactory.cc index 8c09b36..703fdb8 100755 --- a/addons/muscle/MuscleFunctionFactory.cc +++ b/addons/muscle/MuscleFunctionFactory.cc @@ -20,19 +20,19 @@ * See the License for the specific language governing permissions and * * limitations under the License. * * -------------------------------------------------------------------------- */ - /* - Update: - This is a port of the original code so that it will work with - the multibody code RBDL written by Martin Felis. - - Author: - Matthew Millard - - Date: - Nov 2015 +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 */ - + //============================================================================= // INCLUDES //============================================================================= @@ -44,6 +44,8 @@ #include #include +#include + using namespace std; using namespace RigidBodyDynamics::Addons::Muscle; using namespace RigidBodyDynamics::Addons::Geometry; @@ -54,14 +56,14 @@ using namespace RigidBodyDynamics::Addons::Geometry; static int NUM_SAMPLE_PTS = 100; //The number of knot points to use to sample - //each Bezier corner section +//each Bezier corner section -static double SMOOTHING = 0; //The amount of smoothing to use when fitting - //3rd order splines to the quintic Bezier - //functions +static double SMOOTHING = 0; //The amount of smoothing to use when fitting +//3rd order splines to the quintic Bezier +//functions static bool DEBUG = true; //When this is set to true, each function's debug - //routine will be called, which ususally results - //in a text file of its output being produced +//routine will be called, which ususally results +//in a text file of its output being produced static double UTOL = (double)std::numeric_limits::epsilon()*1e2; @@ -76,64 +78,62 @@ static int MAXITER = 20; // MUSCLE CURVE FITTING FUNCTIONS //============================================================================= void MuscleFunctionFactory::createFiberActiveForceLengthCurve( - double x0, - double x1, - double x2, - double x3, - double ylow, - double dydx, - double curviness, - const std::string& curveName, - SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) + double x0, + double x1, + double x2, + double x3, + double ylow, + double dydx, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) { //Ensure that the inputs are within a valid range double rootEPS = sqrt(std::numeric_limits::epsilon()); - if( (!(x0>=0 && x1>x0+rootEPS && x2>x1+rootEPS && x3>x2+rootEPS) ) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberActiveForceLengthCurve: " - << curveName - << ": This must be true: 0 < lce0 < lce1 < lce2 < lce3" - << endl; - assert(0); - abort(); - + if( (!(x0>=0 && x1>x0+rootEPS && x2>x1+rootEPS && x3>x2+rootEPS) ) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberActiveForceLengthCurve: " + << curveName + << ": This must be true: 0 < lce0 < lce1 < lce2 < lce3" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( !(ylow >= 0) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberActiveForceLengthCurve:" - << curveName - <<": shoulderVal must be greater than, or equal to 0" - << endl; - assert(0); - abort(); - + if( !(ylow >= 0) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberActiveForceLengthCurve:" + << curveName + << ": shoulderVal must be greater than, or equal to 0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } double dydxUpperBound = (1-ylow)/(x2-x1); - if( !(dydx >= 0 && dydx < dydxUpperBound) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberActiveForceLengthCurve:" - << curveName - << ": plateauSlope must be greater than 0 and less than " - << dydxUpperBound - << endl; - assert(0); - abort(); + if( !(dydx >= 0 && dydx < dydxUpperBound) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberActiveForceLengthCurve:" + << curveName + << ": plateauSlope must be greater than 0 and less than " + << dydxUpperBound + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( !(curviness >= 0 && curviness <= 1) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberActiveForceLengthCurve:" - << curveName - << ": curviness must be between 0 and 1" - << endl; - assert(0); - abort(); + if( !(curviness >= 0 && curviness <= 1) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberActiveForceLengthCurve:" + << curveName + << ": curviness must be between 0 and 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } std::string name = curveName; @@ -142,183 +142,183 @@ void MuscleFunctionFactory::createFiberActiveForceLengthCurve( //Translate the users parameters into Bezier curves - double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); - //The active force length curve is made up of 5 elbow shaped sections. + //The active force length curve is made up of 5 elbow shaped sections. //Compute the locations of the joining point of each elbow section. //Calculate the location of the shoulder - double xDelta = 0.05*x2; //half the width of the sarcomere 0.0259, - //but TM.Winter's data has a wider shoulder than - //this - - double xs = (x2-xDelta);//x1 + 0.75*(x2-x1); - - //Calculate the intermediate points located on the ascending limb - double y0 = 0; - double dydx0 = 0; - - double y1 = 1 - dydx*(xs-x1); - double dydx01= 1.25*(y1-y0)/(x1-x0);//(y1-y0)/(x1-(x0+xDelta)); - - double x01 = x0 + 0.5*(x1-x0); //x0 + xDelta + 0.5*(x1-(x0+xDelta)); - double y01 = y0 + 0.5*(y1-y0); - - //Calculate the intermediate points of the shallow ascending plateau - double x1s = x1 + 0.5*(xs-x1); - double y1s = y1 + 0.5*(1-y1); - double dydx1s= dydx; - - //double dydx01c0 = 0.5*(y1s-y01)/(x1s-x01) + 0.5*(y01-y0)/(x01-x0); - //double dydx01c1 = 2*( (y1-y0)/(x1-x0)); - //double dydx01(1-c)*dydx01c0 + c*dydx01c1; - - //x2 entered - double y2 = 1; - double dydx2 = 0; - - //Descending limb - //x3 entered - double y3 = 0; - double dydx3 = 0; - - double x23 = (x2+xDelta) + 0.5*(x3-(x2+xDelta)); //x2 + 0.5*(x3-x2); - double y23 = y2 + 0.5*(y3-y2); - - //double dydx23c0 = 0.5*((y23-y2)/(x23-x2)) + 0.5*((y3-y23)/(x3-x23)); - //double dydx23c1 = 2*(y3-y2)/(x3-x2); - double dydx23 = (y3-y2)/((x3-xDelta)-(x2+xDelta)); - //(1-c)*dydx23c0 + c*dydx23c1; - + double xDelta = 0.05*x2; //half the width of the sarcomere 0.0259, + //but TM.Winter's data has a wider shoulder than + //this + + double xs = (x2-xDelta);//x1 + 0.75*(x2-x1); + + //Calculate the intermediate points located on the ascending limb + double y0 = 0; + double dydx0 = 0; + + double y1 = 1 - dydx*(xs-x1); + double dydx01= 1.25*(y1-y0)/(x1-x0);//(y1-y0)/(x1-(x0+xDelta)); + + double x01 = x0 + 0.5*(x1-x0); //x0 + xDelta + 0.5*(x1-(x0+xDelta)); + double y01 = y0 + 0.5*(y1-y0); + + //Calculate the intermediate points of the shallow ascending plateau + double x1s = x1 + 0.5*(xs-x1); + double y1s = y1 + 0.5*(1-y1); + double dydx1s= dydx; + + //double dydx01c0 = 0.5*(y1s-y01)/(x1s-x01) + 0.5*(y01-y0)/(x01-x0); + //double dydx01c1 = 2*( (y1-y0)/(x1-x0)); + //double dydx01(1-c)*dydx01c0 + c*dydx01c1; + + //x2 entered + double y2 = 1; + double dydx2 = 0; + + //Descending limb + //x3 entered + double y3 = 0; + double dydx3 = 0; + + double x23 = (x2+xDelta) + 0.5*(x3-(x2+xDelta)); //x2 + 0.5*(x3-x2); + double y23 = y2 + 0.5*(y3-y2); + + //double dydx23c0 = 0.5*((y23-y2)/(x23-x2)) + 0.5*((y3-y23)/(x3-x23)); + //double dydx23c1 = 2*(y3-y2)/(x3-x2); + double dydx23 = (y3-y2)/((x3-xDelta)-(x2+xDelta)); + //(1-c)*dydx23c0 + c*dydx23c1; + //Compute the locations of the control points - RigidBodyDynamics::Math::MatrixNd p0 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x0,ylow,dydx0,x01,y01,dydx01,c); - RigidBodyDynamics::Math::MatrixNd p1 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x01,y01,dydx01,x1s,y1s,dydx1s,c); - RigidBodyDynamics::Math::MatrixNd p2 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x1s,y1s,dydx1s,x2, y2, dydx2,c); - RigidBodyDynamics::Math::MatrixNd p3 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x2, y2, dydx2,x23,y23,dydx23,c); - RigidBodyDynamics::Math::MatrixNd p4 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x23,y23,dydx23,x3,ylow,dydx3,c); - - RigidBodyDynamics::Math::MatrixNd mX(6,5), mY(6,5); - mX.col(0) = p0.col(0); - mX.col(1) = p1.col(0); - mX.col(2) = p2.col(0); - mX.col(3) = p3.col(0); - mX.col(4) = p4.col(0); - - mY.col(0) = p0.col(1); - mY.col(1) = p1.col(1); - mY.col(2) = p2.col(1); - mY.col(3) = p3.col(1); - mY.col(4) = p4.col(1); - - smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( - mX,mY,x0,x3,ylow,ylow,0,0,curveName); + RigidBodyDynamics::Math::MatrixNd p0 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,ylow,dydx0,x01,y01,dydx01,c); + RigidBodyDynamics::Math::MatrixNd p1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x01,y01,dydx01,x1s,y1s,dydx1s,c); + RigidBodyDynamics::Math::MatrixNd p2 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1s,y1s,dydx1s,x2, y2, dydx2,c); + RigidBodyDynamics::Math::MatrixNd p3 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2, y2, dydx2,x23,y23,dydx23,c); + RigidBodyDynamics::Math::MatrixNd p4 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x23,y23,dydx23,x3,ylow,dydx3,c); + + RigidBodyDynamics::Math::MatrixNd mX(6,5), mY(6,5); + mX.col(0) = p0.col(0); + mX.col(1) = p1.col(0); + mX.col(2) = p2.col(0); + mX.col(3) = p3.col(0); + mX.col(4) = p4.col(0); + + mY.col(0) = p0.col(1); + mY.col(1) = p1.col(1); + mY.col(2) = p2.col(1); + mY.col(3) = p3.col(1); + mY.col(4) = p4.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX,mY,x0,x3,ylow,ylow,0,0,curveName); } void MuscleFunctionFactory::createFiberForceVelocityCurve( - double fmaxE, - double dydxC, - double dydxNearC, - double dydxIso, - double dydxE, - double dydxNearE, - double concCurviness, - double eccCurviness, - const std::string& curveName, - SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) + double fmaxE, + double dydxC, + double dydxNearC, + double dydxIso, + double dydxE, + double dydxNearE, + double concCurviness, + double eccCurviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) { //Ensure that the inputs are within a valid range - - if( !(fmaxE > 1.0) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityCurve: " - << curveName - <<": fmaxE must be greater than 1" - << endl; - assert(0); - abort(); - } - - if( !(dydxC >= 0.0 && dydxC < 1) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityCurve: " - << curveName - << ": dydxC must be greater than or equal to 0 " - <<" and less than 1" - << endl; - assert(0); - abort(); - } - - if( !(dydxNearC > dydxC && dydxNearC <= 1) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityCurve: " - << curveName - << ": dydxNearC must be greater than or equal to 0 " - << "and less than 1" - << endl; - assert(0); - abort(); - } - - if( !(dydxIso > 1) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityCurve: " - << curveName - << ": dydxIso must be greater than (fmaxE-1)/1 (" - << ((fmaxE-1.0)/1.0) - << ")" - << endl; - assert(0); - abort(); - } - - if( !(dydxE >= 0.0 && dydxE < (fmaxE-1)) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityCurve: " - << curveName - <<": dydxE must be greater than or equal to 0 " - << "and less than fmaxE-1 (" - << (fmaxE-1) << ")" - << endl; - assert(0); - abort(); - } - - if(!(dydxNearE >= dydxE && dydxNearE < (fmaxE-1))){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityCurve" - << curveName - << ": dydxNearE must be greater than or equal to dydxE " - << "and less than fmaxE-1 (" << (fmaxE-1) - << ")" - << endl; - assert(0); - abort(); - } - - if(! (concCurviness <= 1.0 && concCurviness >= 0)){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityCurve " - << curveName - << ": concCurviness must be between 0 and 1" - << endl; - assert(0); - abort(); - } - - if(! (eccCurviness <= 1.0 && eccCurviness >= 0)){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityCurve " - << curveName - << ": eccCurviness must be between 0 and 1" - << endl; - assert(0); - abort(); + + if( !(fmaxE > 1.0) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + <<": fmaxE must be greater than 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(dydxC >= 0.0 && dydxC < 1) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + << ": dydxC must be greater than or equal to 0 " + <<" and less than 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(dydxNearC > dydxC && dydxNearC <= 1) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + << ": dydxNearC must be greater than or equal to 0 " + << "and less than 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(dydxIso > 1) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + << ": dydxIso must be greater than (fmaxE-1)/1 (" + << ((fmaxE-1.0)/1.0) + << ")" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(dydxE >= 0.0 && dydxE < (fmaxE-1)) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + <<": dydxE must be greater than or equal to 0 " + << "and less than fmaxE-1 (" + << (fmaxE-1) << ")" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if(!(dydxNearE >= dydxE && dydxNearE < (fmaxE-1))) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve" + << curveName + << ": dydxNearE must be greater than or equal to dydxE " + << "and less than fmaxE-1 (" << (fmaxE-1) + << ")" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if(! (concCurviness <= 1.0 && concCurviness >= 0)) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve " + << curveName + << ": concCurviness must be between 0 and 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if(! (eccCurviness <= 1.0 && eccCurviness >= 0)) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve " + << curveName + << ": eccCurviness must be between 0 and 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } std::string name = curveName; @@ -327,11 +327,11 @@ void MuscleFunctionFactory::createFiberForceVelocityCurve( //Translate the users parameters into Bezier point locations double cC = SegmentedQuinticBezierToolkit::scaleCurviness(concCurviness); double cE = SegmentedQuinticBezierToolkit::scaleCurviness(eccCurviness); - + //Compute the concentric control point locations double xC = -1; double yC = 0; - + double xNearC = -0.9; double yNearC = yC + 0.5*dydxNearC*(xNearC-xC) + 0.5*dydxC*(xNearC-xC); @@ -346,17 +346,17 @@ void MuscleFunctionFactory::createFiberForceVelocityCurve( RigidBodyDynamics::Math::MatrixNd concPts1 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints( xC, yC, dydxC, - xNearC, yNearC,dydxNearC,cC); + calcQuinticBezierCornerControlPoints( xC, yC, dydxC, + xNearC, yNearC,dydxNearC,cC); RigidBodyDynamics::Math::MatrixNd concPts2 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(xNearC,yNearC,dydxNearC, - xIso, yIso, dydxIso, cC); + calcQuinticBezierCornerControlPoints(xNearC,yNearC,dydxNearC, + xIso, yIso, dydxIso, cC); RigidBodyDynamics::Math::MatrixNd eccPts1 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints( xIso, yIso, dydxIso, - xNearE, yNearE, dydxNearE, cE); + calcQuinticBezierCornerControlPoints( xIso, yIso, dydxIso, + xNearE, yNearE, dydxNearE, cE); RigidBodyDynamics::Math::MatrixNd eccPts2 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(xNearE, yNearE, dydxNearE, - xE, yE, dydxE, cE); + calcQuinticBezierCornerControlPoints(xNearE, yNearE, dydxNearE, + xE, yE, dydxE, cE); RigidBodyDynamics::Math::MatrixNd mX(6,4), mY(6,4); mX.col(0) = concPts1.col(0); @@ -370,113 +370,111 @@ void MuscleFunctionFactory::createFiberForceVelocityCurve( mY.col(3) = eccPts2.col(1); smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( - mX,mY,xC,xE,yC,yE,dydxC,dydxE,curveName); + mX,mY,xC,xE,yC,yE,dydxC,dydxE,curveName); } void MuscleFunctionFactory::createFiberForceVelocityInverseCurve( - double fmaxE, - double dydxC, - double dydxNearC, - double dydxIso, - double dydxE, - double dydxNearE, - double concCurviness, - double eccCurviness, - const std::string& curveName, - SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) + double fmaxE, + double dydxC, + double dydxNearC, + double dydxIso, + double dydxE, + double dydxNearE, + double concCurviness, + double eccCurviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) { - //Ensure that the inputs are within a valid range - if(! (fmaxE > 1.0 )){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityInverseCurve: " - << curveName - << ": fmaxE must be greater than 1" - << endl; - assert(0); - abort(); - } - - double SimTKSignificantReal = - pow((double)std::numeric_limits::epsilon(), 7.0/8.0); - - if(! (dydxC > SimTKSignificantReal && dydxC < 1 )){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityInverseCurve " - << curveName - << ": dydxC must be greater than 0" - << "and less than 1" - << endl; - assert(0); - abort(); - + //Ensure that the inputs are within a valid range + if(! (fmaxE > 1.0 )) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve: " + << curveName + << ": fmaxE must be greater than 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if(! (dydxNearC > dydxC && dydxNearC < 1 )){ - std::stringstream errMsg; - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityInverseCurve " - << ": dydxNearC must be greater than 0 " - << curveName - << " and less than 1" - << endl; - assert(0); - abort(); - } + double SimTKSignificantReal = + pow((double)std::numeric_limits::epsilon(), 7.0/8.0); - if(! (dydxIso > 1)){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityInverseCurve " - << curveName - << ": dydxIso must be greater than or equal to 1" - << endl; - assert(0); - abort(); + if(! (dydxC > SimTKSignificantReal && dydxC < 1 )) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": dydxC must be greater than 0" + << "and less than 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if(! (dydxNearC > dydxC && dydxNearC < 1 )) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << ": dydxNearC must be greater than 0 " + << curveName + << " and less than 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if(! (dydxIso > 1)) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": dydxIso must be greater than or equal to 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } //double SimTKSignificantReal = // pow(std::numeric_limits::epsilon(), 7.0/8.0); - if(! (dydxE > SimTKSignificantReal && dydxE < (fmaxE-1)) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityInverseCurve " - << curveName - << ": dydxE must be greater than or equal to 0" - << " and less than fmaxE-1 (" << (fmaxE-1) << ")" - << endl; - assert(0); - abort(); - } - - if(! (dydxNearE >= dydxE && dydxNearE < (fmaxE-1)) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityInverseCurve " - << curveName - << ": dydxNearE must be greater than or equal to dydxE" - << "and less than fmaxE-1 ("<< (fmaxE-1) << ")" - << endl; - assert(0); - abort(); - } - - if(! (concCurviness <= 1.0 && concCurviness >= 0) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityInverseCurve " - << curveName - << ": concCurviness must be between 0 and 1" - << endl; - assert(0); - abort(); - } - - if(! (eccCurviness <= 1.0 && eccCurviness >= 0) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceVelocityInverseCurve " - << curveName - << ": eccCurviness must be between 0 and 1" - << endl; - assert(0); - abort(); + if(! (dydxE > SimTKSignificantReal && dydxE < (fmaxE-1)) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": dydxE must be greater than or equal to 0" + << " and less than fmaxE-1 (" << (fmaxE-1) << ")" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if(! (dydxNearE >= dydxE && dydxNearE < (fmaxE-1)) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": dydxNearE must be greater than or equal to dydxE" + << "and less than fmaxE-1 ("<< (fmaxE-1) << ")" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if(! (concCurviness <= 1.0 && concCurviness >= 0) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": concCurviness must be between 0 and 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if(! (eccCurviness <= 1.0 && eccCurviness >= 0) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": eccCurviness must be between 0 and 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } std::string name = curveName; @@ -485,11 +483,11 @@ void MuscleFunctionFactory::createFiberForceVelocityInverseCurve( //Translate the users parameters into Bezier point locations double cC = SegmentedQuinticBezierToolkit::scaleCurviness(concCurviness); double cE = SegmentedQuinticBezierToolkit::scaleCurviness(eccCurviness); - + //Compute the concentric control point locations double xC = -1; double yC = 0; - + double xNearC = -0.9; double yNearC = yC + 0.5*dydxNearC*(xNearC-xC) + 0.5*dydxC*(xNearC-xC); @@ -504,17 +502,17 @@ void MuscleFunctionFactory::createFiberForceVelocityInverseCurve( RigidBodyDynamics::Math::MatrixNd concPts1 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints( xC, yC, dydxC, - xNearC, yNearC,dydxNearC,cC); + calcQuinticBezierCornerControlPoints( xC, yC, dydxC, + xNearC, yNearC,dydxNearC,cC); RigidBodyDynamics::Math::MatrixNd concPts2 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(xNearC,yNearC,dydxNearC, - xIso, yIso, dydxIso, cC); + calcQuinticBezierCornerControlPoints(xNearC,yNearC,dydxNearC, + xIso, yIso, dydxIso, cC); RigidBodyDynamics::Math::MatrixNd eccPts1 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints( xIso, yIso, dydxIso, - xNearE, yNearE, dydxNearE, cE); + calcQuinticBezierCornerControlPoints( xIso, yIso, dydxIso, + xNearE, yNearE, dydxNearE, cE); RigidBodyDynamics::Math::MatrixNd eccPts2 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(xNearE, yNearE, dydxNearE, - xE, yE, dydxE, cE); + calcQuinticBezierCornerControlPoints(xNearE, yNearE, dydxNearE, + xE, yE, dydxE, cE); RigidBodyDynamics::Math::MatrixNd mX(6,4), mY(6,4); mX.col(0) = concPts1.col(0); @@ -526,48 +524,48 @@ void MuscleFunctionFactory::createFiberForceVelocityInverseCurve( mY.col(1) = concPts2.col(1); mY.col(2) = eccPts1.col(1); mY.col(3) = eccPts2.col(1); - + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( - mY,mX,yC,yE,xC,xE,1/dydxC,1/dydxE, curveName); + mY,mX,yC,yE,xC,xE,1/dydxC,1/dydxE, curveName); } void MuscleFunctionFactory::createFiberCompressiveForcePennationCurve( - double phi0, - double k, - double curviness, - const std::string& curveName, - SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) + double phi0, + double k, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) { //Check the input arguments - if( !(phi0>0 && phi0<(M_PI/2.0)) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberCompressiveForcePennationCurve " - << curveName - << ": phi0 must be greater than 0, and less than Pi/2" - << endl; - assert(0); - abort(); - } - - if( !(k > (1.0/(M_PI/2.0-phi0))) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberCompressiveForcePennationCurve " - << curveName - << ": k must be greater than " << (1.0/(M_PI/2.0-phi0)) - << endl; - assert(0); - abort(); - } - - if( !(curviness>=0 && curviness <= 1) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberCompressiveForcePennationCurve " - << curveName - << ": curviness must be between 0.0 and 1.0" - << endl; - assert(0); - abort(); + if( !(phi0>0 && phi0<(M_PI/2.0)) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberCompressiveForcePennationCurve " + << curveName + << ": phi0 must be greater than 0, and less than Pi/2" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(k > (1.0/(M_PI/2.0-phi0))) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberCompressiveForcePennationCurve " + << curveName + << ": k must be greater than " << (1.0/(M_PI/2.0-phi0)) + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(curviness>=0 && curviness <= 1) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberCompressiveForcePennationCurve " + << curveName + << ": curviness must be between 0.0 and 1.0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } std::string name=curveName; @@ -583,53 +581,53 @@ void MuscleFunctionFactory::createFiberCompressiveForcePennationCurve( double dydx1 = k; RigidBodyDynamics::Math::MatrixNd ctrlPts = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c); - + calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c); + RigidBodyDynamics::Math::MatrixNd mX(6,1), mY(6,1); mX.col(0) = ctrlPts.col(0); mY.col(0) = ctrlPts.col(1); smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( - mX,mY,x0,x1,y0,y1,dydx0,dydx1,curveName); + mX,mY,x0,x1,y0,y1,dydx0,dydx1,curveName); } void MuscleFunctionFactory:: - createFiberCompressiveForceCosPennationCurve( - double cosPhi0, - double k, - double curviness, - const std::string& curveName, - SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +createFiberCompressiveForceCosPennationCurve( + double cosPhi0, + double k, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) { //Check the input arguments - if( !(cosPhi0>0 && cosPhi0 < 1) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberCompressiveForceCosPennationCurve " - << curveName - << ": cosPhi0 must be greater than 0, and less than 1" - << endl; - assert(0); - abort(); - } - - if( !(k < 1/cosPhi0) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberCompressiveForceCosPennationCurve " - << curveName - << ": k must be less than 0" - << endl; - assert(0); - abort(); - } - - if( !(curviness>=0 && curviness <= 1) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberCompressiveForceCosPennationCurve" - << curveName - << ": curviness must be between 0.0 and 1.0" - << endl; - assert(0); - abort(); + if( !(cosPhi0>0 && cosPhi0 < 1) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberCompressiveForceCosPennationCurve " + << curveName + << ": cosPhi0 must be greater than 0, and less than 1" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(k < 1/cosPhi0) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberCompressiveForceCosPennationCurve " + << curveName + << ": k must be less than 0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(curviness>=0 && curviness <= 1) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberCompressiveForceCosPennationCurve" + << curveName + << ": curviness must be between 0.0 and 1.0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } std::string name=curveName; @@ -645,53 +643,53 @@ void MuscleFunctionFactory:: double dydx1 = 0; RigidBodyDynamics::Math::MatrixNd ctrlPts = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c); - + calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c); + RigidBodyDynamics::Math::MatrixNd mX(6,1), mY(6,1); mX.col(0) = ctrlPts.col(0); mY.col(0) = ctrlPts.col(1); smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( - mX,mY,x0,x1,y0,y1,dydx0,dydx1,curveName); + mX,mY,x0,x1,y0,y1,dydx0,dydx1,curveName); } void MuscleFunctionFactory::createFiberCompressiveForceLengthCurve( - double lmax, - double k, - double curviness, - const std::string& curveName, - SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) + double lmax, + double k, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) { - if( !(lmax>0) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberCompressiveForceLength " - << curveName - << ": l0 must be greater than 0" - << endl; - assert(0); - abort(); - } - - if( !(k < -(1.0/lmax)) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberCompressiveForceLength " - << curveName - << ": k must be less than " - << -(1.0/lmax) - << endl; - assert(0); - abort(); - } - - if( !(curviness>=0 && curviness <= 1) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberCompressiveForceLength " - << curveName - << ": curviness must be between 0.0 and 1.0" - << endl; - assert(0); - abort(); + if( !(lmax>0) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberCompressiveForceLength " + << curveName + << ": l0 must be greater than 0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(k < -(1.0/lmax)) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberCompressiveForceLength " + << curveName + << ": k must be less than " + << -(1.0/lmax) + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(curviness>=0 && curviness <= 1) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberCompressiveForceLength " + << curveName + << ": curviness must be between 0.0 and 1.0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } std::string caller = curveName; @@ -707,82 +705,81 @@ void MuscleFunctionFactory::createFiberCompressiveForceLengthCurve( double dydx1 = 0; RigidBodyDynamics::Math::MatrixNd ctrlPts = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c); + calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c); RigidBodyDynamics::Math::MatrixNd mX(6,1), mY(6,1); mX.col(0) = ctrlPts.col(0); mY.col(0) = ctrlPts.col(1); smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( - mX,mY,x0,x1,y0,y1,dydx0,dydx1,curveName); + mX,mY,x0,x1,y0,y1,dydx0,dydx1,curveName); } void MuscleFunctionFactory::createFiberForceLengthCurve( - double eZero, - double eIso, - double kLow, - double kIso, - double curviness, - const std::string& curveName, - SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) + double eZero, + double eIso, + double kLow, + double kIso, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) { - - if( !(eIso > eZero) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceLength " - << curveName - << ": The following must hold: eIso > eZero" - << endl; - assert(0); - abort(); - } - - if( !(kIso > (1.0/(eIso-eZero))) ){ - std::stringstream errMsg; - cerr << "MuscleFunctionFactory::" - << "createFiberForceLength " - << curveName - << ": kiso must be greater than 1/(eIso-eZero) (" - << (1.0/(eIso-eZero)) << ")" - << endl; - assert(0); - abort(); - } - - if( !(kLow > 0.0 && kLow < 1/(eIso-eZero)) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceLength " - << curveName - << ": kLow must be greater than 0 and less than" - << 1.0/(eIso-eZero) - << endl; - assert(0); - abort(); - } - - if( !(curviness>=0 && curviness <= 1) ){ - cerr << "MuscleFunctionFactory::" - << "createFiberForceLength " - << curveName - << ": curviness must be between 0.0 and 1.0" - << endl; - assert(0); - abort(); + + if( !(eIso > eZero) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceLength " + << curveName + << ": The following must hold: eIso > eZero" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(kIso > (1.0/(eIso-eZero))) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceLength " + << curveName + << ": kiso must be greater than 1/(eIso-eZero) (" + << (1.0/(eIso-eZero)) << ")" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(kLow > 0.0 && kLow < 1/(eIso-eZero)) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceLength " + << curveName + << ": kLow must be greater than 0 and less than" + << 1.0/(eIso-eZero) + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(curviness>=0 && curviness <= 1) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createFiberForceLength " + << curveName + << ": curviness must be between 0.0 and 1.0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } std::string callerName = curveName; callerName.append(".createFiberForceLength"); - //Translate the user parameters to quintic Bezier points + //Translate the user parameters to quintic Bezier points double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); double xZero = 1+eZero; double yZero = 0; - + double xIso = 1 + eIso; double yIso = 1; - + double deltaX = std::min(0.1*(1.0/kIso), 0.1*(xIso-xZero)); double xLow = xZero + deltaX; @@ -792,12 +789,12 @@ void MuscleFunctionFactory::createFiberForceLengthCurve( //Compute the Quintic Bezier control points RigidBodyDynamics::Math::MatrixNd p0 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(xZero, yZero, 0, - xLow, yLow, kLow,c); - + calcQuinticBezierCornerControlPoints(xZero, yZero, 0, + xLow, yLow, kLow,c); + RigidBodyDynamics::Math::MatrixNd p1 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(xLow, yLow, kLow, - xIso, yIso, kIso, c); + calcQuinticBezierCornerControlPoints(xLow, yLow, kLow, + xIso, yIso, kIso, c); RigidBodyDynamics::Math::MatrixNd mX(6,2); RigidBodyDynamics::Math::MatrixNd mY(6,2); @@ -806,67 +803,66 @@ void MuscleFunctionFactory::createFiberForceLengthCurve( mX.col(1) = p1.col(0); mY.col(1) = p1.col(1); - + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( - mX, mY, xZero, xIso, yZero, yIso, 0.0, kIso, curveName); + mX, mY, xZero, xIso, yZero, yIso, 0.0, kIso, curveName); } void MuscleFunctionFactory:: - createTendonForceLengthCurve( double eIso, double kIso, - double fToe, double curviness, - const std::string& curveName, - SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +createTendonForceLengthCurve( double eIso, double kIso, + double fToe, double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) { - - if( !(eIso>0) ){ - cerr << "MuscleFunctionFactory::" - << "createTendonForceLengthCurve " - << curveName - << ": eIso must be greater than 0, but " - << eIso << " was entered" - << endl; - assert(0); - abort(); - - } - - if( !(fToe>0 && fToe < 1) ){ - cerr << "MuscleFunctionFactory::" - << "createTendonForceLengthCurve " - << curveName - << ": fToe must be greater than 0 and less than 1, but " - << fToe - << " was entered" - << endl; - assert(0); - abort(); - } - - if( !(kIso > (1/eIso)) ){ - cerr << "MuscleFunctionFactory::" - << "createTendonForceLengthCurve " - << curveName - << ": kIso must be greater than 1/eIso, (" - << (1/eIso) << "), but kIso (" - << kIso << ") was entered" - << endl; - assert(0); - abort(); - } - - - if( !(curviness>=0 && curviness <= 1) ){ - cerr << "MuscleFunctionFactory::" - << "createTendonForceLengthCurve " - << curveName - << " : curviness must be between 0.0 and 1.0, but " - << curviness << " was entered" - << endl; - assert(0); - abort(); + + if( !(eIso>0) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createTendonForceLengthCurve " + << curveName + << ": eIso must be greater than 0, but " + << eIso << " was entered" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(fToe>0 && fToe < 1) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createTendonForceLengthCurve " + << curveName + << ": fToe must be greater than 0 and less than 1, but " + << fToe + << " was entered" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( !(kIso > (1/eIso)) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createTendonForceLengthCurve " + << curveName + << ": kIso must be greater than 1/eIso, (" + << (1/eIso) << "), but kIso (" + << kIso << ") was entered" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + + if( !(curviness>=0 && curviness <= 1) ) { + ostringstream errormsg; + errormsg << "MuscleFunctionFactory::" + << "createTendonForceLengthCurve " + << curveName + << " : curviness must be between 0.0 and 1.0, but " + << curviness << " was entered" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } std::string callerName = curveName; @@ -889,8 +885,8 @@ void MuscleFunctionFactory:: //To limit the 2nd derivative of the toe region the line it tends to //has to intersect the x axis to the right of the origin - double xFoot = 1.0+(xToe-1.0)/10.0; - double yFoot = 0; + double xFoot = 1.0+(xToe-1.0)/10.0; + double yFoot = 0; double dydxToe = (yToe-yFoot)/(xToe-xFoot); //Compute the location of the corner formed by the average slope of the @@ -900,18 +896,18 @@ void MuscleFunctionFactory:: double dydxToeMid = (yToeMid-yFoot)/(xToeMid-xFoot); //Compute the location of the control point to the left of the corner - double xToeCtrl = xFoot + 0.5*(xToeMid-xFoot); + double xToeCtrl = xFoot + 0.5*(xToeMid-xFoot); double yToeCtrl = yFoot + dydxToeMid*(xToeCtrl-xFoot); //Compute the Quintic Bezier control points RigidBodyDynamics::Math::MatrixNd p0 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x0,y0,dydx0, - xToeCtrl,yToeCtrl,dydxToeMid,c); + calcQuinticBezierCornerControlPoints(x0,y0,dydx0, + xToeCtrl,yToeCtrl,dydxToeMid,c); RigidBodyDynamics::Math::MatrixNd p1 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(xToeCtrl, yToeCtrl, dydxToeMid, - xToe, yToe, dydxIso, c); + calcQuinticBezierCornerControlPoints(xToeCtrl, yToeCtrl, dydxToeMid, + xToe, yToe, dydxIso, c); RigidBodyDynamics::Math::MatrixNd mX(6,2); RigidBodyDynamics::Math::MatrixNd mY(6,2); @@ -922,6 +918,6 @@ void MuscleFunctionFactory:: mY.col(1) = p1.col(1); smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( - mX, mY, x0, xToe, y0, yToe, dydx0, dydxIso, curveName); - + mX, mY, x0, xToe, y0, yToe, dydx0, dydxIso, curveName); + } diff --git a/addons/muscle/README.md b/addons/muscle/README.md index 5905145..b41997a 100755 --- a/addons/muscle/README.md +++ b/addons/muscle/README.md @@ -4,7 +4,7 @@ @author Matthew Millard -\copyright 2016 Matthew Millard +\copyright 2016 Matthew Millard \b Requirements This addon depends on the geometry addon diff --git a/addons/muscle/TorqueMuscleFittingToolkit.cc b/addons/muscle/TorqueMuscleFittingToolkit.cc new file mode 100644 index 0000000..77710d0 --- /dev/null +++ b/addons/muscle/TorqueMuscleFittingToolkit.cc @@ -0,0 +1,992 @@ +#include "TorqueMuscleFittingToolkit.h" + +#include "IpIpoptApplication.hpp" + +using namespace Ipopt; +static double EPSILON = std::numeric_limits::epsilon(); +static double SQRTEPSILON = sqrt(EPSILON); + + +using namespace RigidBodyDynamics::Math; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace std; + + +void TorqueMuscleFittingToolkit:: + fitTorqueMuscleParameters( + Millard2016TorqueMuscle const &tqMcl, + VectorNd const &jointAngle, + VectorNd const &jointAngularVelocity, + VectorNd const &jointTorque, + double activationUpperBound, + double passiveTorqueAngleMultiplierUpperBound, + TorqueMuscleParameterFittingData ¶metersOfBestFit, + bool verbose) +{ + + if(tqMcl.mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableTqMcl = + const_cast(&tqMcl); + mutableTqMcl->updateTorqueMuscleCurves(); + } + + + assert(jointAngle.rows() > 1); + assert(jointAngularVelocity.rows() > 1); + assert(jointTorque.rows() > 1); + + unsigned int nrows = jointAngle.rows(); + + double activationLowerBound = 0.0; + assert(jointAngularVelocity.rows() == nrows); + assert(jointTorque.rows() == nrows); + assert(activationUpperBound > 0 && activationUpperBound <= 1.0); + assert(activationLowerBound >= 0 && activationLowerBound= 0){ + jointTorqueSingleSided[i] = jointTorque[i]; + }else{ + jointTorqueSingleSided[i] = 0.0; + } + } + + double tiso = tqMcl.getMaximumActiveIsometricTorque(); + double omegaMax = tqMcl.getMaximumConcentricJointAngularVelocity(); + + double maxConcentricAngularVelocityData = 0; + for(unsigned int i=0; i fabs(maxConcentricAngularVelocityData)){ + maxConcentricAngularVelocityData = fabs(jointAngularVelocity[i]); + } + } + + //Set ta and tv blending variables to a small finite value so that + //it is possible to solve for an activation of finite value when + //calcTorqueMuscleDataFeatures is called + double taLambda = sqrt(SQRTEPSILON); + double tvLambda = sqrt(SQRTEPSILON); + if(tqMcl.getActiveTorqueAngleCurveBlendingVariable() > taLambda){ + taLambda = tqMcl.getActiveTorqueAngleCurveBlendingVariable(); + } + if(tqMcl.getTorqueAngularVelocityCurveBlendingVariable() > tvLambda){ + tvLambda = tqMcl.getTorqueAngularVelocityCurveBlendingVariable(); + } + + double tpLambda = 0.0; + + double taAngleScaling = tqMcl.mTaAngleScaling; + double taAngleAtOneNormTorque = tqMcl.mAngleAtOneNormActiveTorque; + double tvOmegaMaxScale = 1.0; + double tpOffset = 0.0; + + double objectiveValue = 0.0; + double constraintError = 0.0; + + TorqueMuscleDataFeatures tmf; + + omegaMax = fabs(tqMcl.getMaximumConcentricJointAngularVelocity() + *tvOmegaMaxScale); + + //If the optimization run fails, the values from tmf are passed out to + //the user to help them debug the problem. + tqMcl.calcTorqueMuscleDataFeatures( + jointTorqueSingleSided,jointAngle,jointAngularVelocity, + taLambda,tpLambda,tvLambda, + taAngleScaling,taAngleAtOneNormTorque,tpOffset,omegaMax, + tiso,tmf); + + + Millard2016TorqueMuscle* mutableTqMcl = + const_cast(&tqMcl); + + SmartPtr fittingProblem + = new FitTorqueMuscleParameters( + jointAngle, + jointAngularVelocity, + jointTorqueSingleSided, + activationUpperBound, + passiveTorqueAngleMultiplierUpperBound, + taLambda, + tvLambda, + *mutableTqMcl); + + SmartPtr app = new IpoptApplication(false); + + app->RethrowNonIpoptException(true); + double solnTol = SQRTEPSILON; + app->Options()->SetNumericValue("tol",solnTol); + app->Options()->SetStringValue("mu_strategy","adaptive"); + + if(!verbose){ + app->Options()->SetIntegerValue("print_level",0); + } + + + ApplicationReturnStatus status; + + objectiveValue = std::numeric_limits::infinity(); + constraintError= std::numeric_limits::infinity(); + bool converged = false; + + double taAngleScaleStart = 1.0; + double tvOmegaMaxScaleStart =fabs(1.1*maxConcentricAngularVelocityData + /tqMcl.getMaximumConcentricJointAngularVelocity()); + + double tisoScale = 1.0; + bool updateStart = false; + + status = app->Initialize(); + if(status == Solve_Succeeded){ + status = app->OptimizeTNLP(fittingProblem); + if(status == Solve_Succeeded){ + converged = true; + updateStart = true; + taAngleScaling = fittingProblem + ->getSolutionActiveTorqueAngleAngleScaling(); + tvOmegaMaxScale = fittingProblem + ->getSolutionTorqueAngularVelocityOmegaMaxScale(); + tpLambda = fittingProblem + ->getSolutionPassiveTorqueAngleBlendingParameter(); + tpOffset = fittingProblem + ->getSolutionPassiveTorqueAngleCurveOffset(); + tisoScale = fittingProblem + ->getSolutionMaximumActiveIsometricTorqueScale(); + + tiso = tisoScale * tiso; + omegaMax = fabs(tvOmegaMaxScale + *tqMcl.getMaximumConcentricJointAngularVelocity()); + + objectiveValue = fittingProblem->getObjectiveValue(); + constraintError= fittingProblem->getConstraintError().norm(); + + tqMcl.calcTorqueMuscleDataFeatures( + jointTorqueSingleSided,jointAngle,jointAngularVelocity, + taLambda,tpLambda,tvLambda, + taAngleScaling,taAngleAtOneNormTorque,tpOffset,omegaMax, + tiso,tmf); + } + } + + + if(converged){ + parametersOfBestFit.fittingConverged = true; + parametersOfBestFit.isTorqueMuscleActive = !(tmf.isInactive); + + parametersOfBestFit.indexAtMaximumActivation=tmf.indexOfMaxActivation; + parametersOfBestFit.indexAtMinimumActivation=tmf.indexOfMinActivation; + parametersOfBestFit.indexAtMaxPassiveTorqueAngleMultiplier + =tmf.indexOfMaxPassiveTorqueAngleMultiplier; + + parametersOfBestFit.activeTorqueAngleBlendingVariable = taLambda; + parametersOfBestFit.torqueVelocityBlendingVariable = tvLambda; + parametersOfBestFit.passiveTorqueAngleBlendingVariable = tpLambda; + parametersOfBestFit.passiveTorqueAngleCurveOffset = tpOffset; + parametersOfBestFit.maximumActiveIsometricTorque = tiso; + parametersOfBestFit.activeTorqueAngleAngleScaling = taAngleScaling; + parametersOfBestFit.maximumAngularVelocity = fabs(omegaMax); + + parametersOfBestFit.summaryDataAtMaximumActivation + =tmf.summaryAtMaxActivation; + parametersOfBestFit.summaryDataAtMinimumActivation + =tmf.summaryAtMinActivation; + parametersOfBestFit.summaryDataAtMaximumPassiveTorqueAngleMultiplier + =tmf.summaryAtMaxPassiveTorqueAngleMultiplier; + }else{ + parametersOfBestFit.fittingConverged = false; + parametersOfBestFit.isTorqueMuscleActive = !(tmf.isInactive); + + parametersOfBestFit.indexAtMaximumActivation + =numeric_limits::signaling_NaN(); + parametersOfBestFit.indexAtMinimumActivation + =numeric_limits::signaling_NaN(); + parametersOfBestFit.indexAtMaxPassiveTorqueAngleMultiplier + =numeric_limits::signaling_NaN(); + + parametersOfBestFit.activeTorqueAngleBlendingVariable + = numeric_limits::signaling_NaN(); + parametersOfBestFit.passiveTorqueAngleBlendingVariable + = numeric_limits::signaling_NaN(); + parametersOfBestFit.passiveTorqueAngleCurveOffset + = numeric_limits::signaling_NaN(); + parametersOfBestFit.torqueVelocityBlendingVariable + = numeric_limits::signaling_NaN(); + parametersOfBestFit.maximumActiveIsometricTorque + = numeric_limits::signaling_NaN(); + parametersOfBestFit.activeTorqueAngleAngleScaling + = numeric_limits::signaling_NaN(); + parametersOfBestFit.maximumAngularVelocity + = numeric_limits::signaling_NaN(); + + parametersOfBestFit.summaryDataAtMaximumActivation + =tmf.summaryAtMaxActivation; + parametersOfBestFit.summaryDataAtMinimumActivation + =tmf.summaryAtMinActivation; + parametersOfBestFit.summaryDataAtMaximumPassiveTorqueAngleMultiplier + =tmf.summaryAtMaxPassiveTorqueAngleMultiplier; + } + +} + + + +FitTorqueMuscleParameters:: + FitTorqueMuscleParameters( + const RigidBodyDynamics::Math::VectorNd &jointAngle, + const RigidBodyDynamics::Math::VectorNd &jointAngularVelocity, + const RigidBodyDynamics::Math::VectorNd &jointTorque, + double maxActivation, + double maxPassiveTorqueAngleMultiplier, + double taLambda, + double tvLambda, + Millard2016TorqueMuscle &tqMcl): + mJointAngle(jointAngle), + mJointAngularVelocity(jointAngularVelocity), + mJointTorque(jointTorque), + mTqMcl(tqMcl) +{ + assert(jointTorque.rows() == jointAngle.rows()); + assert(jointTorque.rows() == jointAngularVelocity.rows()); + + //Initialize all member variables + mN = 5; + mM = 3*jointTorque.rows(); + + double angleMin = std::numeric_limits< double >::max(); + double omegaMin = std::numeric_limits< double >::max(); + double angleMax = -std::numeric_limits< double >::max(); + double omegaMax = -std::numeric_limits< double >::max(); + double tauMax = 0; + + for(unsigned int i=0; i angleMax){ + angleMax = mJointAngle[i]; + } + if(mJointAngularVelocity[i] < omegaMin){ + omegaMin = mJointAngularVelocity[i]; + } + if(mJointAngularVelocity[i] > omegaMax){ + omegaMax = mJointAngularVelocity[i]; + } + if(mJointTorque[i]*mTqMcl.getJointTorqueSign() > tauMax){ + tauMax = mJointTorque[i]*mTqMcl.getJointTorqueSign(); + } + } + + double tvScale = max(fabs(omegaMin),fabs(omegaMax)) + /fabs(mTqMcl.getMaximumConcentricJointAngularVelocity()); + double taScale = fabs(angleMax-angleMin) + /mTqMcl.getActiveTorqueAngleCurveWidth(); + + double tauScale = tauMax/fabs(mTqMcl.getMaximumActiveIsometricTorque()); + + + + + mConIdxTauActMaxStart = 0; + mConIdxTauActMaxEnd = jointTorque.rows()-1; + mConIdxTauActMinStart = jointTorque.rows(); + mConIdxTauActMinEnd = 2*jointTorque.rows()-1; + mConIdxTauPassiveStart = 2*jointTorque.rows(); + mConIdxTauPassiveEnd = 3*jointTorque.rows()-1; + + mMaxActivation = maxActivation; + mMinActivation = 0.0; + mMaxTp = maxPassiveTorqueAngleMultiplier; + mTauIso = mTqMcl.getMaximumActiveIsometricTorque(); + mTaLambda = taLambda; + mTvLambda = tvLambda; + mTaAngleAtOneNormTorque = mTqMcl.mAngleAtOneNormActiveTorque; + mOmegaMax = mTqMcl.mOmegaMax; + + mTaAngleScaleStart = max(taScale, + mTqMcl.getActiveTorqueAngleCurveAngleScaling()); + mTvOmegaMaxScaleStart = max(tvScale, 1.); + mTpLambdaStart = mTqMcl.getPassiveTorqueAngleCurveBlendingVariable(); + mTpAngleOffsetStart = mTqMcl.getPassiveCurveAngleOffset(); + mTauScalingStart = max(tauScale, 1.); + + mTaAngleScaleLB = 1.; + mTvOmegaMaxScaleLB = 1.; + mTpLambdaLB = 0.; + mTpAngleOffsetLB = -M_PI*0.5; + mTauScalingLB = 1.0; + + mTaAngleScaleUB = 2.0e19; + mTvOmegaMaxScaleUB = 2.0e19; + mTpLambdaUB = 1.0; + mTpAngleOffsetUB = M_PI*0.5; + mTauScalingUB = 2.0e19; + + mIndexTaAngleScale = 0; + mIndexTvOmegaMaxScale = 1; + mIndexTpLambda = 2; + mIndexTpOffset = 3; + mIndexTauScaling = 4; + + //Initialize vectors + mConstraintErrors.resize(mM); + mXOffset.resize(mN); + + mWeights.resize(mN); + for(unsigned int i=0; i= 0 + m = mM; + + //Constraint Jacobian + //This is a dense Jacobian + nnz_jac_g = mM*mN; + + //Hessian + //This is a symmetric matrix 5x5 matrix, but its densley populated so + //we must fill in the bottom left triangle. + nnz_h_lag = 15; + + index_style = Ipopt::TNLP::C_STYLE; + + return true; +} + + +bool FitTorqueMuscleParameters:: + get_bounds_info(Ipopt::Index n, + Ipopt::Number *x_l, + Ipopt::Number *x_u, + Ipopt::Index m, + Ipopt::Number *g_l, + Ipopt::Number *g_u) +{ + assert(n==mN); + assert(m==mM); + x_l[0] = mTaAngleScaleLB; + x_l[1] = mTvOmegaMaxScaleLB; + x_l[2] = mTpLambdaLB; + x_l[3] = mTpAngleOffsetLB; + x_l[4] = mTauScalingLB; + + x_u[0] = mTaAngleScaleUB; + x_u[1] = mTvOmegaMaxScaleUB; + x_u[2] = mTpLambdaUB; + x_u[3] = mTpAngleOffsetUB; + x_u[4] = mTauScalingUB; + + //The optimization routine will only strengthen + //the muscle to satisfy the constraint - but it + //will not weaken it. + + double tauActMaxLB = 0; + double tauActMaxUB = 0; + double tauActMinLB = 0; + double tauActMinUB = 0; + double tauPassiveLB = 0; + double tauPassiveUB = 0; + + + + if(mTqMcl.getJointTorqueSign() > 0){ + tauActMaxLB = 0; + tauActMaxUB = 2e19; + + tauActMinLB = -2e19; + tauActMinUB = 0; + + tauPassiveLB = -2e19; + tauPassiveUB = 0; + }else{ + tauActMaxLB = -2.e19; + tauActMaxUB = 0.; + + tauActMinLB = 0; + tauActMinUB = 2e19; + + tauPassiveLB = -2e19; + tauPassiveUB = 0; + } + + for(unsigned int i = mConIdxTauActMaxStart; i <= mConIdxTauActMaxEnd; ++i){ + g_l[i] = tauActMaxLB; + g_u[i] = tauActMaxUB; + } + for(unsigned int i = mConIdxTauActMinStart; i <= mConIdxTauActMinEnd; ++i){ + g_l[i] = tauActMinLB; + g_u[i] = tauActMinUB; + } + for(unsigned int i = mConIdxTauPassiveStart; i <= mConIdxTauPassiveEnd; ++i){ + g_l[i] = tauPassiveLB; + g_u[i] = tauPassiveUB; + } + + return true; +} + + +bool FitTorqueMuscleParameters:: + get_starting_point(Ipopt::Index n, + bool init_x, + Ipopt::Number *x, + bool init_z, + Ipopt::Number *z_L, + Ipopt::Number *z_U, + Ipopt::Index m, + bool init_lambda, + Ipopt::Number *lambda) +{ + assert(n==mN); + + x[0] = mTaAngleScaleStart; + x[1] = mTvOmegaMaxScaleStart; + x[2] = mTpLambdaStart; + x[3] = mTpAngleOffsetStart; + x[4] = mTauScalingStart; + + init_x = true; + init_z = false; + init_lambda = false; + + mDtp_Dx.resize(mN); + + return true; +} + +bool FitTorqueMuscleParameters:: + eval_f(Ipopt::Index n, + const Ipopt::Number *x, + bool new_x, + Ipopt::Number &obj_value) +{ + assert(n==mN); + + obj_value = 0.; + double xUpd = 0.; + for(unsigned i=0; i +#include +#include "IpTNLP.hpp" +#include "Millard2016TorqueMuscle.h" + +namespace RigidBodyDynamics { + namespace Addons { + namespace Muscle{ + + +class TorqueMuscleFittingToolkit { + public: + + + /** + \brief This function will adjust the parameters of the + muscle-torque-generator (MTG) so that the MTG is strong enough and + flexible enough that it can produce the measured output torque + across the vector of measured joint angles and angular + velocities without exceeding the desired activation upper + bound, nor the passive forces exceeding the prescribed upper bound. + This function requires that IPOPT is installed + and that cmake has been correctly configured to point at + IPOPT's install directory. For a detailed description of the method please + see : M.Millard, A.L.Kleesattel, M.Harant, & K.Mombaur. A reduced muscle + model and planar musculoskeletal model fit for the synthesis of whole body + movements. Journal of Biomechanics. (under review as of August 2018) + + The method adjusts five parameters: + + -# \f$s^A\f$: the scale of the domain (angle) of the + active-torque-angle-curve. This scaling is applied so that + the angle which corresponds to the peak value of the + active-torque-angle-curve remains unchanged. + -# \f$s^V\f$: the scaling applied to the maximum angular velocity at + which the concentric side of the torque-angular-velocity + curve goes to zero. + -# \f$\lambda^P\f$: the passive-torque-angle-blending-variable + -# \f$\Delta^P\f$: the passive-torque-angle-offset + -# \f$s^\tau\f$: the scaling applied to \f$\tau_{o}\f$ the + maximum active isometric torque + + These 5 parameters are a subset of the fitting variables that are available, + and are illustrated below: + + \image html fig_MuscleAddon_BlendableTorqueMuscle.png "Examples of the adjustable characteristic curves" + + + To peform the fitting, the user must provide vectors of the + joint angle, joint angular velocity, and the joint torque + for the joint that the muscle-torque-generator (MTG) actuates. The sign of + all of these quantities must be consistent with the multibody + and the torque muscle. IPOPT solves for the fitting parameters + \f$\mathbf{x}\f$ such that the final result is as close as possible to the + values the MTG has before the fitting is performed \f$\mathbf{x}_o\f$ + \f[ + (\mathbf{x}-\mathbf{x}_o)^T \mathbf{K} (\mathbf{x}-\mathbf{x}_o) + \f] + such that at every point \f$i\f$ in the vector of joint angles, velocities, + and torques the following constraints are satisfied + \f[ + \tau^M(a^{max},\theta_i,\dot{\theta}_i,\mathbf{x})-\tau_i \ge 0 + \f] + \f[ + \tau^M(0,\theta_i,\dot{\theta}_i,\mathbf{x})-\tau_i \le 0 + \f] + \f[ + \mathbf{t}_{P}(\theta_i) - \mathbf{t}_{P}^{max} \le 0 + \f] + \f[ + s^A, s^V, s^{\tau} \ge 1 + \f] + \f[ + 1 \ge \lambda^P \ge 0 + \f] + \f[ + \pi/2 \ge \Delta^P \ge -\pi/2. + \f] + These constraints ensure that the muscle can deliver the + requested torque with an activation of \f$a^{max}\f$ or less, such that + the active forces of the muscle are never fighting the passive forces, + and that passive forces that are \f$\mathbf{t}_{P}^{max}\f$ or less. + The bound limits on the various optimization variables ensure that the + muscle is not made to be weaker or slower than the default settings for + the muscle. Thus if your subject is weaker than the default settings + for the muscle, you should first weaken the MTG and then run this function. + + Notes + -# This routine will return the parameters that + fit the data, even if they are unreasonable: it is your + responsibility to have a look at this data to sanity check it + before using it. + -# This function will not update the internal + parameters of the model - it just calculates what they need + to be and returns them in the + TorqueMuscleParametersFittingData structure. If you decide + that these parameters are parameters are suitable, then they + can be assigned to the model using the function + Millard2016TorqueMuscle.setFittedParameters. The solve and set functions + have been deliberately split up to reflect the inherentely + iterative, and interactive nature of fitting the strength + of a musculoskeletal model to data. + -# Before attempting to fit the strength of the muscles + to experimental data we recommend that you pre-process the + data first. Normally this involves filtering the joint angles + using a zero-phase 2nd order low-pass Butterworth filter with + a cutoff frequency of 8-10 Hz (for human data). This can be + achieved in Matlab using the functions 'butter' and + 'filtfilt'. Following this filtering step joint angular + velocities can be safely calculated numerically using a + central-difference method. If you fail to filter measured + joint angles it is likely that the joint angular velocities + you calculate will be noisy and will result in a terrible + fit. This function is not magic, and so the rule + garbage in, garbage out applies. + + @param tqMcl: + The Millard2016TorqueMuscle object that is to be fitted. + @param jointAngle (radians) + A vector of measured joint angles. The sign of this vector + must be consistent with the multibody model and this + torque muscle. + @param jointAngularVelocity (radians/s) + A vector of joint angular velocities. + The sign of this vector must be consistent with the + multibody model and this torque muscle. + @param jointTorque (Nm) + A vector of joint torques. Only the + values in this vector that have the same torque sign as + the muscle are considered during the fitting process. This + means, inherently, that the muscle fitting is done assuming that + there is no co-contraction. If you want co-contraction to be included + in the fitting process, you must preprocess this vector so that it just + contains the (estimated) single-signed torque of the muscle in question. + The sign of this vector must be consistent with the + multibody model and this torque muscle. + @param activationUpperBound (0,1] + The maximum activation that is permitted for the given + set of joint angles, joint angular velocities, and + joint torques. + @param passiveTorqueAngleMultiplierUpperBound (0, \f$\infty\f$] + The maximum passive torque angle multipler that you expect + to see when the muscle moves through the values in + jointAngle. Note that a value of 1 is quite large: this + corresponds to 1 maximum active isometric torque. + @param parametersOfBestFit + A structure that contains the parameters of best fit along + with a summary of the internal variables of the muscle + model at the points in the data where the peak passive + forces are developed and where the maximum activation is + developed. + @param verbose + Setting this to will print information from IPOPT to the screen + during the solution process. + + */ + static void fitTorqueMuscleParameters( + Millard2016TorqueMuscle const &tqMcl, + RigidBodyDynamics::Math::VectorNd const &jointAngle, + RigidBodyDynamics::Math::VectorNd const &jointAngularVelocity, + RigidBodyDynamics::Math::VectorNd const &jointTorque, + double activationUpperBound, + double passiveTorqueAngleMultiplierUpperBound, + TorqueMuscleParameterFittingData ¶metersOfBestFit, + bool verbose=false); + + +}; + + + + +//A class that defines the virtual functions needed +//to use Ipopt to solve the problem +class FitTorqueMuscleParameters : public Ipopt::TNLP{ + public: + FitTorqueMuscleParameters( + const RigidBodyDynamics::Math::VectorNd &jointAngle, + const RigidBodyDynamics::Math::VectorNd &jointAngularVelocity, + const RigidBodyDynamics::Math::VectorNd &jointTorque, + double maxActivation, + double maxPassiveTorqueAngleMultiplier, + double taLambda, + double tvLambda, + Millard2016TorqueMuscle &tqMcl); + + + //Manditory functions of the TNLP interface + virtual bool get_nlp_info(Ipopt::Index &n, + Ipopt::Index &m, + Ipopt::Index &nnz_jac_g, + Ipopt::Index &nnz_h_lag, + Ipopt::TNLP::IndexStyleEnum &index_style); + + + virtual bool get_bounds_info(Ipopt::Index n, + Ipopt::Number *x_l, + Ipopt::Number *x_u, + Ipopt::Index m, + Ipopt::Number *g_l, + Ipopt::Number *g_u); + + + virtual bool get_starting_point(Ipopt::Index n, + bool init_x, + Ipopt::Number *x, + bool init_z, + Ipopt::Number *z_L, + Ipopt::Number *z_U, + Ipopt::Index m, + bool init_lambda, + Ipopt::Number *lambda); + + virtual bool eval_f(Ipopt::Index n, + const Ipopt::Number *x, + bool new_x, + Ipopt::Number &obj_value); + + virtual bool eval_grad_f(Ipopt::Index n, + const Ipopt::Number *x, + bool new_x, + Ipopt::Number *grad_f); + + virtual bool eval_g(Ipopt::Index n, + const Ipopt::Number *x, + bool new_x, + Ipopt::Index m, + Ipopt::Number *g); + + virtual bool eval_jac_g(Ipopt::Index n, + const Ipopt::Number *x, + bool new_x, + Ipopt::Index m, + Ipopt::Index nele_jac, + Ipopt::Index *iRow, + Ipopt::Index *jCol, + Ipopt::Number *values); + + virtual void finalize_solution( + Ipopt::SolverReturn status, + Ipopt::Index n, + const Ipopt::Number *x, + const Ipopt::Number *z_L, + const Ipopt::Number *z_U, + Ipopt::Index m, + const Ipopt::Number *g, + const Ipopt::Number *lambda, + Ipopt:: Number obj_value, + const Ipopt::IpoptData *ip_data, + Ipopt::IpoptCalculatedQuantities *ip_cq); + + //Optional functions + virtual bool eval_h(Ipopt::Index n, + const Ipopt::Number *x, + bool new_x, + Ipopt::Number obj_factor, + Ipopt::Index m, + const Ipopt::Number *lambda, + bool new_lambda, + Ipopt::Index nele_hess, + Ipopt::Index *iRow, + Ipopt::Index *jCol, + Ipopt::Number *values); + + double getSolutionActiveTorqueAngleAngleScaling(); + double getSolutionPassiveTorqueAngleBlendingParameter(); + double getSolutionTorqueAngularVelocityOmegaMaxScale(); + double getSolutionPassiveTorqueAngleCurveOffset(); + double getSolutionMaximumActiveIsometricTorqueScale(); + double getObjectiveValue(); + RigidBodyDynamics::Math::VectorNd& getConstraintError(); + + void updOptimizationVariables(const Ipopt::Number *x); + private: + unsigned int mN, mM; + + double mMaxActivation; + double mMinActivation; + double mMaxTp; + double mTauIso; + + double mTaAngleAtOneNormTorque; + double mOmegaMax; + double mTaLambda; + double mTvLambda; + + + double mTaAngleScaleStart; + double mTvOmegaMaxScaleStart; + double mTpLambdaStart; + double mTpAngleOffsetStart; + double mTauScalingStart; + + + double mTaAngleScaleLB; + double mTvOmegaMaxScaleLB; + double mTpLambdaLB; + double mTpAngleOffsetLB; + double mTauScalingLB; + + + double mTaAngleScaleUB; + double mTvOmegaMaxScaleUB; + double mTpLambdaUB; + double mTpAngleOffsetUB; + double mTauScalingUB; + + + double mTaAngleScale; + double mTvOmegaMaxScale; + double mTpLambda; + double mTpAngleOffset; + double mTauScaling; + + unsigned int mIndexTaAngleScale; + unsigned int mIndexTvOmegaMaxScale; + unsigned int mIndexTpLambda; + unsigned int mIndexTpOffset; + unsigned int mIndexTauScaling; + + unsigned int mConIdxTauActMaxStart; + unsigned int mConIdxTauActMaxEnd; + unsigned int mConIdxTauActMinStart; + unsigned int mConIdxTauActMinEnd; + unsigned int mConIdxTauPassiveStart; + unsigned int mConIdxTauPassiveEnd; + + + const RigidBodyDynamics::Math::VectorNd &mJointAngle; + const RigidBodyDynamics::Math::VectorNd &mJointAngularVelocity; + const RigidBodyDynamics::Math::VectorNd &mJointTorque; + + RigidBodyDynamics::Math::VectorNd mXOffset; + RigidBodyDynamics::Math::VectorNd mDtp_Dx; + RigidBodyDynamics::Math::VectorNd mConstraintErrors; + double mObjValue; + RigidBodyDynamics::Math::VectorNd mWeights; + Millard2016TorqueMuscle &mTqMcl; + TorqueMuscleInfo mTmi; + TorqueMuscleSummary mTms; +}; + + + + + } + } +} + +#endif + diff --git a/addons/muscle/TorqueMuscleFunctionFactory.cc b/addons/muscle/TorqueMuscleFunctionFactory.cc index 056135a..635ee8a 100755 --- a/addons/muscle/TorqueMuscleFunctionFactory.cc +++ b/addons/muscle/TorqueMuscleFunctionFactory.cc @@ -36,6 +36,8 @@ #include #include +#include + using namespace std; using namespace RigidBodyDynamics::Math; using namespace RigidBodyDynamics::Addons::Muscle; @@ -48,21 +50,21 @@ static double SQRTEPS = sqrt( (double)std::numeric_limits::epsilon()); //============================================================================= void TorqueMuscleFunctionFactory:: - createAnderson2007ActiveTorqueAngleCurve( - double c2, - double c3, - const std::string& curveName, - SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +createAnderson2007ActiveTorqueAngleCurve( + double c2, + double c3, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) { //Check the input arguments - if( !(c2 > 0) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createAnderson2007ActiveTorqueAngleCurve " - << curveName - << ": c2 must be greater than 0" - << endl; - assert(0); - abort(); + if( !(c2 > 0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createAnderson2007ActiveTorqueAngleCurve " + << curveName + << ": c2 must be greater than 0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } @@ -98,25 +100,25 @@ void TorqueMuscleFunctionFactory:: //Compute the Quintic Bezier control points - RigidBodyDynamics::Math::MatrixNd p0 = + RigidBodyDynamics::Math::MatrixNd p0 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x0,y0,dydx0, - x1,y1,dydx1,c); + calcQuinticBezierCornerControlPoints(x0,y0,dydx0, + x1,y1,dydx1,c); - RigidBodyDynamics::Math::MatrixNd p1 = + RigidBodyDynamics::Math::MatrixNd p1 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x1,y1,dydx1, - x2,y2,dydx2,c); + calcQuinticBezierCornerControlPoints(x1,y1,dydx1, + x2,y2,dydx2,c); - RigidBodyDynamics::Math::MatrixNd p2 = + RigidBodyDynamics::Math::MatrixNd p2 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x2,y2,dydx2, - x3,y3,dydx3,c); + calcQuinticBezierCornerControlPoints(x2,y2,dydx2, + x3,y3,dydx3,c); - RigidBodyDynamics::Math::MatrixNd p3 = + RigidBodyDynamics::Math::MatrixNd p3 = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x3,y3,dydx3, - x4,y4,dydx4,c); + calcQuinticBezierCornerControlPoints(x3,y3,dydx3, + x4,y4,dydx4,c); RigidBodyDynamics::Math::MatrixNd mX(6,4); RigidBodyDynamics::Math::MatrixNd mY(6,4); @@ -132,72 +134,72 @@ void TorqueMuscleFunctionFactory:: smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( - mX, mY, x0, x4, y0, y4, dydx0, dydx4, curveName); + mX, mY, x0, x4, y0, y4, dydx0, dydx4, curveName); } //============================================================================= // ANDERSON 2007 Active Torque Angular Velocity Curve //============================================================================= void TorqueMuscleFunctionFactory:: - createAnderson2007ActiveTorqueVelocityCurve( - double c4, - double c5, - double c6, - double minEccentricMultiplier, - double maxEccentricMultiplier, - const std::string& curveName, - SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +createAnderson2007ActiveTorqueVelocityCurve( + double c4, + double c5, + double c6, + double minEccentricMultiplier, + double maxEccentricMultiplier, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) { //Check the input arguments - if( !(c4 < c5) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createAndersonActiveTorqueVelocityCurve " - << curveName - << ": c4 must be greater than c5" - << endl; - assert(0); - abort(); + if( !(c4 < c5) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": c4 must be greater than c5" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( !((c4 > 0)) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createAndersonActiveTorqueVelocityCurve " - << curveName - << ": c4 must be greater than 0" - << endl; - assert(0); - abort(); + if( !((c4 > 0)) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": c4 must be greater than 0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( !(c6 > 0.0) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createAndersonActiveTorqueVelocityCurve " - << curveName - << ": c6 must be greater than 1.0" - << endl; - assert(0); - abort(); + if( !(c6 > 0.0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": c6 must be greater than 1.0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( !(minEccentricMultiplier > 1.0) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createAndersonActiveTorqueVelocityCurve " - << curveName - << ": minEccentricMultiplier must be greater than 1.0" - << endl; - assert(0); - abort(); + if( !(minEccentricMultiplier > 1.0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": minEccentricMultiplier must be greater than 1.0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( !(maxEccentricMultiplier > minEccentricMultiplier) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createAndersonActiveTorqueVelocityCurve " - << curveName - << ": maxEccentricMultiplier must be greater than " - << " minEccentricMultiplier" - << endl; - assert(0); - abort(); + if( !(maxEccentricMultiplier > minEccentricMultiplier) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": maxEccentricMultiplier must be greater than " + << " minEccentricMultiplier" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } //Advanced settings that we'll hide for now @@ -205,7 +207,7 @@ void TorqueMuscleFunctionFactory:: double curviness = 0.75; double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); - //Go and get the value of the curve that is closest to + //Go and get the value of the curve that is closest to //the maximum contraction velocity by setting rhs of Eqn. 9 //to 0 and solving double dthMaxConc = abs( 2.0*c4*c5/(c5-3.0*c4) ); @@ -230,69 +232,69 @@ void TorqueMuscleFunctionFactory:: //============================================================================= // ANDERSON 2007 Passive Torque Angle Curve //============================================================================= -void TorqueMuscleFunctionFactory:: - createAnderson2007PassiveTorqueAngleCurve( - double scale, - double c1, - double b1, - double k1, - double b2, - double k2, - const std::string& curveName, - SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +void TorqueMuscleFunctionFactory:: +createAnderson2007PassiveTorqueAngleCurve( + double scale, + double c1, + double b1, + double k1, + double b2, + double k2, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) { - if( !(scale > 0) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createAnderson2007PassiveTorqueAngleCurve " - << curveName - << ": scale must be greater than 0" - << endl; - assert(0); - abort(); + if( !(scale > 0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createAnderson2007PassiveTorqueAngleCurve " + << curveName + << ": scale must be greater than 0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( !(c1 > 0) ) { - cerr << "TorqueMuscleFunctionFactory::" - << "createAnderson2007PassiveTorqueAngleCurve " - << curveName - << ": c1 must be greater than 0" - << endl; - assert(0); - abort(); + if( !(c1 > 0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createAnderson2007PassiveTorqueAngleCurve " + << curveName + << ": c1 must be greater than 0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } //Advanced settings that we'll hide for now double curviness = 0.75; double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); double minShoulderSlopeMagnitude = 0; - - //Zero out the coefficients associated with a + + //Zero out the coefficients associated with a //the side of the curve that goes negative. bool flag_oneSided = true; - if(flag_oneSided){ - if(fabs(b1) > 0){ - if(b1 > 0){ + if(flag_oneSided) { + if(fabs(b1) > 0) { + if(b1 > 0) { b2 = 0; k2 = 0; - }else{ - b1 = 0; + } else { + b1 = 0; k1 = 0; } - }else if(fabs(b2) > 0){ - if(b2 > 0){ - b1 = 0; - k1 = 0; - }else{ + } else if(fabs(b2) > 0) { + if(b2 > 0) { + b1 = 0; + k1 = 0; + } else { b2 = 0; k2 = 0; } - } + } } //Divide up the curve into a left half - //and a right half, rather than 1 and 2. + //and a right half, rather than 1 and 2. //Why? These two different halves require different // Bezier curves. @@ -310,29 +312,29 @@ void TorqueMuscleFunctionFactory:: int flag_thL = 0; int flag_thR = 0; - if(fabs(k1)>0 && fabs(b1)>0){ + if(fabs(k1)>0 && fabs(b1)>0) { //The value of theta where the passive force generated by the //muscle is equal to 1 maximum isometric contraction. thL = (1/k1)*log(fabs( c1Scale/b1 )); - DtauDthL = b1*k1*exp(thL*k1); + DtauDthL = b1*k1*exp(thL*k1); bL = b1; kL = k1; flag_thL = 1; } - - if(fabs(k2)>0 && fabs(b2)>0){ + + if(fabs(k2)>0 && fabs(b2)>0) { //The value of theta where the passive force generated by the //muscle is equal to 1 maximum isometric contraction. thR = (1/k2)*log(fabs( c1Scale/b2 )); - DtauDthR = b2*k2*exp(thR*k2); + DtauDthR = b2*k2*exp(thR*k2); bR = b2; - kR = k2; + kR = k2; flag_thR = 1; } //A 'left' curve has a negative slope, //A 'right' curve has a positive slope. - if(DtauDthL > DtauDthR){ + if(DtauDthL > DtauDthR) { double tmpD = thL; thL = thR; thR = tmpD; @@ -351,14 +353,14 @@ void TorqueMuscleFunctionFactory:: int tmpI = flag_thL; flag_thL = flag_thR; - flag_thR = tmpI; + flag_thR = tmpI; } - if(flag_thL){ + if(flag_thL) { curveType = curveType + 1; } - if(flag_thR){ + if(flag_thR) { curveType = curveType + 2; } @@ -373,203 +375,203 @@ void TorqueMuscleFunctionFactory:: double dydxEnd = 0; - switch(curveType){ - - case 0: - {//No curve - it's a flat line - RigidBodyDynamics::Math::MatrixNd p0 = - SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(0.,0.,0., - 1.,0.,0.,c); - - mX.col(0) = p0.col(0); - mY.col(0) = p0.col(1); - - }break; - case 1: - { - //Get a point on the curve that is close to 0. - double x1 = (1/kL)*log(fabs(0.01*c1Scale/bL) ); - double y1 = bL*exp(kL*x1); - double dydx1 = bL*kL*exp(kL*x1); - - //Get a point that is at 1 maximum isometric torque - double x3 = thL; - double y3 = bL*exp(kL*x3); - double dydx3 = bL*kL*exp(kL*x3); - - //Get a mid-point - double x2 = 0.5*(x1+x3); - double y2 = bL*exp(kL*x2); - double dydx2 = bL*kL*exp(kL*x2); - - //Past the crossing point of the linear extrapolation - double x0 = x1 - 2*y1/dydx1; - double y0 = 0; - double dydx0 = minShoulderSlopeMagnitude*copysign(1.0,dydx1); - - xStart = x3; - xEnd = x0; - yStart = y3; - yEnd = y0; - dydxStart = dydx3; - dydxEnd = dydx0; - - RigidBodyDynamics::Math::MatrixNd p0 = - SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x3,y3,dydx3, - x2,y2,dydx2,c); - RigidBodyDynamics::Math::MatrixNd p1 = - SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x2,y2,dydx2, - x1,y1,dydx1,c); - RigidBodyDynamics::Math::MatrixNd p2 = - SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x1,y1,dydx1, - x0,y0,dydx0,c); - - mX.resize(6,3); - mY.resize(6,3); - - mX.col(0) = p0.col(0); - mY.col(0) = p0.col(1); - mX.col(1) = p1.col(0); - mY.col(1) = p1.col(1); - mX.col(2) = p2.col(0); - mY.col(2) = p2.col(1); - - }break; - case 2: - { - //Get a point on the curve that is close to 0. - double x1 = (1/kR)*log(fabs(0.01*c1Scale/bR) ); - double y1 = bR*exp(kR*x1); - double dydx1 = bR*kR*exp(kR*x1); - - //Go just past the crossing point of the linear extrapolation - double x0 = x1 - 2*y1/dydx1; - double y0 = 0; - double dydx0 = minShoulderSlopeMagnitude*copysign(1.0,dydx1); - - //Get a point close to 1 maximum isometric torque - double x3 = thR; - double y3 = bR*exp(kR*x3); - double dydx3 = bR*kR*exp(kR*x3); - - //Get a mid point. - double x2 = 0.5*(x1+x3); - double y2 = bR*exp(kR*x2); - double dydx2 = bR*kR*exp(kR*x2); - - xStart = x0; - xEnd = x3; - yStart = y0; - yEnd = y3; - dydxStart = dydx0; - dydxEnd = dydx3; - - RigidBodyDynamics::Math::MatrixNd p0 = - SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x0,y0,dydx0, - x1,y1,dydx1,c); - RigidBodyDynamics::Math::MatrixNd p1 = - SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x1,y1,dydx1, - x2,y2,dydx2,c); - RigidBodyDynamics::Math::MatrixNd p2 = - SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x2,y2,dydx2, - x3,y3,dydx3,c); - mX.resize(6,3); - mY.resize(6,3); - - mX.col(0) = p0.col(0); - mY.col(0) = p0.col(1); - mX.col(1) = p1.col(0); - mY.col(1) = p1.col(1); - mX.col(2) = p2.col(0); - mY.col(2) = p2.col(1); - - }break; - case 3: - { - //Only when flag_oneSided = false; - double x0 = thL; - double x4 = thR; - - double x2 = 0.5*(x0 + x4); - double x1 = 0.5*(x0 + x2); - double x3 = 0.5*(x2 + x4); - - double y0 = b1*exp(k1*x0) - + b2*exp(k2*x0); - double y1 = b1*exp(k1*x1) - + b2*exp(k2*x1); - double y2 = b1*exp(k1*x2) - + b2*exp(k2*x2); - double y3 = b1*exp(k1*x3) - + b2*exp(k2*x3); - double y4 = b1*exp(k1*x4) - + b2*exp(k2*x4); - - double dydx0 = b1*k1*exp(k1*x0) - + b2*k2*exp(k2*x0); - double dydx1 = b1*k1*exp(k1*x1) - + b2*k2*exp(k2*x1); - double dydx2 = b1*k1*exp(k1*x2) - + b2*k2*exp(k2*x2); - double dydx3 = b1*k1*exp(k1*x3) - + b2*k2*exp(k2*x3); - double dydx4 = b1*k1*exp(k1*x4) - + b2*k2*exp(k2*x4); - - xStart = x0; - xEnd = x4; - yStart = y0; - yEnd = y4; - dydxStart = dydx0; - dydxEnd = dydx4; - - RigidBodyDynamics::Math::MatrixNd p0 = - SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x0,y0,dydx0, - x1,y1,dydx1,c); - RigidBodyDynamics::Math::MatrixNd p1 = - SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x1,y1,dydx1, - x2,y2,dydx2,c); - RigidBodyDynamics::Math::MatrixNd p2 = - SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x2,y2,dydx2, - x3,y3,dydx3,c); - RigidBodyDynamics::Math::MatrixNd p3 = - SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x3,y3,dydx3, - x4,y4,dydx4,c); - - mX.resize(6,4); - mY.resize(6,4); - - mX.col(0) = p0.col(0); - mY.col(0) = p0.col(1); - mX.col(1) = p1.col(0); - mY.col(1) = p1.col(1); - mX.col(2) = p2.col(0); - mY.col(2) = p2.col(1); - mX.col(3) = p3.col(0); - mY.col(3) = p3.col(1); - - }break; - default: - { - cerr << "TorqueMuscleFunctionFactory::" - << "createAnderson2007PassiveTorqueAngleCurve " - << curveName - << ": undefined curveType" - << endl; - assert(0); - abort(); - } + switch(curveType) { + + case 0: { + //No curve - it's a flat line + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(0.,0.,0., + 1.,0.,0.,c); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + + } + break; + case 1: { + //Get a point on the curve that is close to 0. + double x1 = (1/kL)*log(fabs(0.01*c1Scale/bL) ); + double y1 = bL*exp(kL*x1); + double dydx1 = bL*kL*exp(kL*x1); + + //Get a point that is at 1 maximum isometric torque + double x3 = thL; + double y3 = bL*exp(kL*x3); + double dydx3 = bL*kL*exp(kL*x3); + + //Get a mid-point + double x2 = 0.5*(x1+x3); + double y2 = bL*exp(kL*x2); + double dydx2 = bL*kL*exp(kL*x2); + + //Past the crossing point of the linear extrapolation + double x0 = x1 - 2*y1/dydx1; + double y0 = 0; + double dydx0 = minShoulderSlopeMagnitude*copysign(1.0,dydx1); + + xStart = x3; + xEnd = x0; + yStart = y3; + yEnd = y0; + dydxStart = dydx3; + dydxEnd = dydx0; + + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x3,y3,dydx3, + x2,y2,dydx2,c); + RigidBodyDynamics::Math::MatrixNd p1 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2,y2,dydx2, + x1,y1,dydx1,c); + RigidBodyDynamics::Math::MatrixNd p2 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1,y1,dydx1, + x0,y0,dydx0,c); + + mX.resize(6,3); + mY.resize(6,3); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + mX.col(2) = p2.col(0); + mY.col(2) = p2.col(1); + + } + break; + case 2: { + //Get a point on the curve that is close to 0. + double x1 = (1/kR)*log(fabs(0.01*c1Scale/bR) ); + double y1 = bR*exp(kR*x1); + double dydx1 = bR*kR*exp(kR*x1); + + //Go just past the crossing point of the linear extrapolation + double x0 = x1 - 2*y1/dydx1; + double y0 = 0; + double dydx0 = minShoulderSlopeMagnitude*copysign(1.0,dydx1); + + //Get a point close to 1 maximum isometric torque + double x3 = thR; + double y3 = bR*exp(kR*x3); + double dydx3 = bR*kR*exp(kR*x3); + + //Get a mid point. + double x2 = 0.5*(x1+x3); + double y2 = bR*exp(kR*x2); + double dydx2 = bR*kR*exp(kR*x2); + + xStart = x0; + xEnd = x3; + yStart = y0; + yEnd = y3; + dydxStart = dydx0; + dydxEnd = dydx3; + + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0, + x1,y1,dydx1,c); + RigidBodyDynamics::Math::MatrixNd p1 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1,y1,dydx1, + x2,y2,dydx2,c); + RigidBodyDynamics::Math::MatrixNd p2 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2,y2,dydx2, + x3,y3,dydx3,c); + mX.resize(6,3); + mY.resize(6,3); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + mX.col(2) = p2.col(0); + mY.col(2) = p2.col(1); + + } + break; + case 3: { + //Only when flag_oneSided = false; + double x0 = thL; + double x4 = thR; + + double x2 = 0.5*(x0 + x4); + double x1 = 0.5*(x0 + x2); + double x3 = 0.5*(x2 + x4); + + double y0 = b1*exp(k1*x0) + + b2*exp(k2*x0); + double y1 = b1*exp(k1*x1) + + b2*exp(k2*x1); + double y2 = b1*exp(k1*x2) + + b2*exp(k2*x2); + double y3 = b1*exp(k1*x3) + + b2*exp(k2*x3); + double y4 = b1*exp(k1*x4) + + b2*exp(k2*x4); + + double dydx0 = b1*k1*exp(k1*x0) + + b2*k2*exp(k2*x0); + double dydx1 = b1*k1*exp(k1*x1) + + b2*k2*exp(k2*x1); + double dydx2 = b1*k1*exp(k1*x2) + + b2*k2*exp(k2*x2); + double dydx3 = b1*k1*exp(k1*x3) + + b2*k2*exp(k2*x3); + double dydx4 = b1*k1*exp(k1*x4) + + b2*k2*exp(k2*x4); + + xStart = x0; + xEnd = x4; + yStart = y0; + yEnd = y4; + dydxStart = dydx0; + dydxEnd = dydx4; + + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0, + x1,y1,dydx1,c); + RigidBodyDynamics::Math::MatrixNd p1 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1,y1,dydx1, + x2,y2,dydx2,c); + RigidBodyDynamics::Math::MatrixNd p2 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2,y2,dydx2, + x3,y3,dydx3,c); + RigidBodyDynamics::Math::MatrixNd p3 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x3,y3,dydx3, + x4,y4,dydx4,c); + + mX.resize(6,4); + mY.resize(6,4); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + mX.col(2) = p2.col(0); + mY.col(2) = p2.col(1); + mX.col(3) = p3.col(0); + mY.col(3) = p3.col(1); + + } + break; + default: { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createAnderson2007PassiveTorqueAngleCurve " + << curveName + << ": undefined curveType" + << endl; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); + } }; @@ -577,8 +579,8 @@ void TorqueMuscleFunctionFactory:: mY = mY*(1/c1Scale); smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( - mX, mY, - xStart, xEnd, + mX, mY, + xStart, xEnd, yStart/c1Scale, yEnd/c1Scale, dydxStart/c1Scale, dydxEnd/c1Scale, curveName); @@ -596,107 +598,107 @@ void TorqueMuscleFunctionFactory:: //============================================================================= void TorqueMuscleFunctionFactory::createTorqueVelocityCurve( - double tvAtEccentricOmegaMax, - double tvAtHalfConcentricOmegaMax, - const std::string& curveName, - RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& - smoothSegmentedFunctionToUpdate ) - { - - double slopeAtConcentricOmegaMax = 0.0; - double slopeNearEccentricOmegaMax = -0.025; - double slopeAtEccentricOmegaMax = 0.0; - double eccentricCurviness = 0.75; - - createTorqueVelocityCurve( - tvAtEccentricOmegaMax, - tvAtHalfConcentricOmegaMax, - slopeAtConcentricOmegaMax, - slopeNearEccentricOmegaMax, - slopeAtEccentricOmegaMax, - eccentricCurviness, - curveName, - smoothSegmentedFunctionToUpdate); + double tvAtEccentricOmegaMax, + double tvAtHalfConcentricOmegaMax, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) +{ - } + double slopeAtConcentricOmegaMax = 0.0; + double slopeNearEccentricOmegaMax = -0.025; + double slopeAtEccentricOmegaMax = 0.0; + double eccentricCurviness = 0.75; + + createTorqueVelocityCurve( + tvAtEccentricOmegaMax, + tvAtHalfConcentricOmegaMax, + slopeAtConcentricOmegaMax, + slopeNearEccentricOmegaMax, + slopeAtEccentricOmegaMax, + eccentricCurviness, + curveName, + smoothSegmentedFunctionToUpdate); + +} void TorqueMuscleFunctionFactory::createTorqueVelocityCurve( - double tvAtEccentricOmegaMax, - double tvAtHalfConcentricOmegaMax, - double slopeAtConcentricOmegaMax, - double slopeNearEccentricOmegaMax, - double slopeAtEccentricOmegaMax, - double eccentricCurviness, - const std::string& curveName, - RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& - smoothSegmentedFunctionToUpdate ) + double tvAtEccentricOmegaMax, + double tvAtHalfConcentricOmegaMax, + double slopeAtConcentricOmegaMax, + double slopeNearEccentricOmegaMax, + double slopeAtEccentricOmegaMax, + double eccentricCurviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) { - if( (tvAtEccentricOmegaMax < 1.05) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createTorqueVelocityCurve " - << curveName - << ": tvAtEccentricOmegaMax must be greater than 1.05" - << endl; - assert(0); - abort(); + if( (tvAtEccentricOmegaMax < 1.05) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": tvAtEccentricOmegaMax must be greater than 1.05" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } if( (tvAtHalfConcentricOmegaMax < 0.05 - || tvAtHalfConcentricOmegaMax > 0.45) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createTorqueVelocityCurve " - << curveName - << ": tvAtHalfOmegaMax must be in the interval [0.05,0.45]" - << endl; - assert(0); - abort(); + || tvAtHalfConcentricOmegaMax > 0.45) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": tvAtHalfOmegaMax must be in the interval [0.05,0.45]" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( (slopeAtConcentricOmegaMax > 0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": slopeAtConcentricOmegaMax cannot be less than 0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( (slopeAtConcentricOmegaMax > 0) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createTorqueVelocityCurve " - << curveName - << ": slopeAtConcentricOmegaMax cannot be less than 0" - << endl; - assert(0); - abort(); - } - - if( (slopeNearEccentricOmegaMax > 0) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createTorqueVelocityCurve " - << curveName - << ": slopeNearEccentricOmegaMax cannot be less than 0" - << endl; - assert(0); - abort(); + if( (slopeNearEccentricOmegaMax > 0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": slopeNearEccentricOmegaMax cannot be less than 0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( (slopeAtEccentricOmegaMax > 0) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createTorqueVelocityCurve " - << curveName - << ": slopeAtEccentricOmegaMax cannot be less than 0" - << endl; - assert(0); - abort(); + if( (slopeAtEccentricOmegaMax > 0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": slopeAtEccentricOmegaMax cannot be less than 0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( (eccentricCurviness < 0 || eccentricCurviness > 1.0) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createTorqueVelocityCurve " - << curveName - << ": eccentricCurviness must be in the interval [0,1]" - << endl; - assert(0); - abort(); - } + if( (eccentricCurviness < 0 || eccentricCurviness > 1.0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": eccentricCurviness must be in the interval [0,1]" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } double omegaMax = 1.0; double wmaxC = omegaMax; //In biomechanics the concentric side gets a - //a +'ve signed velocity + //a +'ve signed velocity double wmaxE = -omegaMax; //----------------------------------------------------------------------- @@ -704,22 +706,22 @@ void TorqueMuscleFunctionFactory::createTorqueVelocityCurve( //----------------------------------------------------------------------- /* - First we compute the terms that are consistent with a Hill-type concentric + First we compute the terms that are consistent with a Hill-type concentric contraction. Starting from Hill's hyperbolic equation - + f(w) = (fiso*b - a*w) / (b+w) - - + + Taking a derivative w.r.t omega yields - + df(w)/dw = [ (fiso*b - a*w + a*(b+w))] / (b+w)^2 - + at w = wmaxC the numerator goes to 0 - + (fiso*b - a*wmaxC) / (b + wmaxC) = 0 (fiso*b - a*wmaxC) = 0; b = a*wmaxC/fiso; - + Subtituting this expression for b into the expression when f(wHalf) = tvAtHalfOmegaMax yields this expression for parameter a @@ -731,22 +733,22 @@ void TorqueMuscleFunctionFactory::createTorqueVelocityCurve( double fiso = 1.0; double w = 0.5*wmaxC; double a = -tvAtHalfConcentricOmegaMax*w*fiso - / (wmaxC*tvAtHalfConcentricOmegaMax - fiso*wmaxC + fiso*w); + / (wmaxC*tvAtHalfConcentricOmegaMax - fiso*wmaxC + fiso*w); double b = a*wmaxC/fiso; double yCheck = (b*fiso-a*w)/(b+w); - if( abs(yCheck-tvAtHalfConcentricOmegaMax) > SQRTEPS ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createTorqueVelocityCurve " - << curveName - << ": Internal error fitting the concentric curve to Hill's " - << "hyperbolic curve. This error condition was true: " - << "abs(yCheck-tvAtHalfOmegaMax) > sqrt(eps)" - << "Consult the maintainers of this addon." - << endl; - assert(0); - abort(); + if( abs(yCheck-tvAtHalfConcentricOmegaMax) > SQRTEPS ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": Internal error fitting the concentric curve to Hill's " + << "hyperbolic curve. This error condition was true: " + << "abs(yCheck-tvAtHalfOmegaMax) > sqrt(eps)" + << "Consult the maintainers of this addon." + << endl; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); } @@ -758,17 +760,17 @@ void TorqueMuscleFunctionFactory::createTorqueVelocityCurve( double dydxNearC = (-(a)*(b+w) - (b*fiso-a*w)) / ((b+w)*(b+w)); - if( dydxNearC > slopeAtConcentricOmegaMax || abs(dydxNearC) > abs(1/wmaxC) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createTorqueVelocityCurve " - << curveName - << ": Internal error fitting the concentric curve to Hill's " - << "hyperbolic curve. This error condition was true: " - << " dydxNearC < dydxC || dydxNearC > abs(1/wmaxC)" - << "Consult the maintainers of this addon." - << endl; - assert(0); - abort(); + if( dydxNearC > slopeAtConcentricOmegaMax || abs(dydxNearC) > abs(1/wmaxC) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": Internal error fitting the concentric curve to Hill's " + << "hyperbolic curve. This error condition was true: " + << " dydxNearC < dydxC || dydxNearC > abs(1/wmaxC)" + << "Consult the maintainers of this addon." + << endl; + throw RigidBodyDynamics::Errors::RBDLError(errormsg.str()); } //---------------------------------------------------------------------------- @@ -784,20 +786,20 @@ void TorqueMuscleFunctionFactory::createTorqueVelocityCurve( double cC = 0.5; MatrixNd pts = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(xIso, yIso, dydxIso, - xNearC,yNearC, dydxNearC, - cC); + calcQuinticBezierCornerControlPoints(xIso, yIso, dydxIso, + xNearC,yNearC, dydxNearC, + cC); MatrixNd xpts(6,1); xpts.col(0) = pts.col(0); MatrixNd ypts(6,1); ypts.col(0) = pts.col(1); SmoothSegmentedFunction fvCFcn = SmoothSegmentedFunction( - xpts,ypts, - xIso,xNearC, - yIso,yNearC, - dydxIso,dydxNearC, - "tvFcn"); + xpts,ypts, + xIso,xNearC, + yIso,yNearC, + dydxIso,dydxNearC, + "tvFcn"); SmoothSegmentedFunction fvCFcnLeft = SmoothSegmentedFunction(); SmoothSegmentedFunction fvCFcnRight = SmoothSegmentedFunction(); @@ -807,7 +809,7 @@ void TorqueMuscleFunctionFactory::createTorqueVelocityCurve( //Calculate the error of the Bezier curve with the starting curviness value //of 0.5 - for(int j=0; j (0.9/abs(angleAtOneNormTorque-angleAtZeroTorque)) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createPassiveTorqueAngleCurve " - << curveName - << ": stiffnessAtLowTorque has a magnitude that is too " - << "large, it exceeds 0.9/abs(angleAtOneNormTorque-angleAtZeroTorque)" - << endl; - assert(0); - abort(); - } - - if( abs(stiffnessAtOneNormTorque) - < (1.1/abs(angleAtOneNormTorque-angleAtZeroTorque)) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createPassiveTorqueAngleCurve " - << curveName - << ": stiffnessAtOneNormTorque has a magnitude that is too " - << "small, it is less than 1.1/abs(angleAtOneNormTorque-angleAtZeroTorque)" - << endl; - assert(0); - abort(); - } - - if( stiffnessAtOneNormTorque*stiffnessAtLowTorque < 0.0 ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createPassiveTorqueAngleCurve " - << curveName - << ": stiffnessAtLowTorque and stiffnessAtOneNormTorque must have the" - << " same sign." - << endl; - assert(0); - abort(); + if( abs(stiffnessAtLowTorque) > (0.9/abs(angleAtOneNormTorque + -angleAtZeroTorque)) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": stiffnessAtLowTorque has a magnitude that is too " + << "large, it exceeds 0.9/abs(angleAtOneNormTorque-angleAtZeroTorque)" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( stiffnessAtOneNormTorque - *(angleAtOneNormTorque-angleAtZeroTorque) < 0.0){ - cerr << "TorqueMuscleFunctionFactory::" - << "createPassiveTorqueAngleCurve " - << curveName - << ": stiffnessAtOneNormTorque must have the same sign as " - << "(angleAtOneNormTorque-angleAtZeroTorque)" - << endl; - assert(0); - abort(); + if( abs(stiffnessAtOneNormTorque) < (1.1/abs(angleAtOneNormTorque + -angleAtZeroTorque)) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": stiffnessAtOneNormTorque has a magnitude that is too " + << "small, it is less than 1.1/abs(angleAtOneNormTorque-angleAtZeroTorque)" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } - if( (curviness < 0 || curviness > 1.0) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createPassiveTorqueAngleCurve " - << curveName - << ": curviness must be in the interval [0,1]" - << endl; - assert(0); - abort(); - } + if( stiffnessAtOneNormTorque*stiffnessAtLowTorque < 0.0 ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": stiffnessAtLowTorque and stiffnessAtOneNormTorque must have the" + << " same sign." + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( stiffnessAtOneNormTorque*(angleAtOneNormTorque-angleAtZeroTorque) < 0.0) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": stiffnessAtOneNormTorque must have the same sign as " + << "(angleAtOneNormTorque-angleAtZeroTorque)" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( (curviness < 0 || curviness > 1.0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": curviness must be in the interval [0,1]" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } MatrixNd xM(6,2); MatrixNd yM(6,2); MatrixNd pts(6,2); double x0,x1,y0,y1,dydx0,dydx1; - if(angleAtZeroTorque < angleAtOneNormTorque){ + if(angleAtZeroTorque < angleAtOneNormTorque) { x0 = angleAtZeroTorque; x1 = angleAtOneNormTorque; y0 = 0.; y1 = 1.; dydx0 = 0.0; dydx1 = stiffnessAtOneNormTorque; - }else{ + } else { x0 = angleAtOneNormTorque; x1 = angleAtZeroTorque; y0 = 1.0; @@ -1082,11 +1083,11 @@ void TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); - if(abs(stiffnessAtOneNormTorque) > SQRTEPS){ + if(abs(stiffnessAtOneNormTorque) > SQRTEPS) { double delta = min( 0.1*(1.0-abs(1.0/stiffnessAtOneNormTorque)), 0.05*abs(x1-x0)); - if(stiffnessAtOneNormTorque < 0.){ + if(stiffnessAtOneNormTorque < 0.) { delta *= -1.0; } @@ -1096,33 +1097,33 @@ void TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( double yLow = yFoot + stiffnessAtLowTorque*(xLow-xFoot); pts = SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints( - x0, y0, dydx0, + x0, y0, dydx0, xLow,yLow,stiffnessAtLowTorque, c); xM.col(0) = pts.col(0); yM.col(0) = pts.col(1); pts = SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints( xLow,yLow,stiffnessAtLowTorque, - x1, y1, dydx1, c); + x1, y1, dydx1, c); xM.col(1) = pts.col(0); yM.col(1) = pts.col(1); - }else{ + } else { pts = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints( - angleAtZeroTorque,0, 0, - angleAtOneNormTorque,0, 0, - c); + calcQuinticBezierCornerControlPoints( + angleAtZeroTorque,0, 0, + angleAtOneNormTorque,0, 0, + c); xM.col(0) = pts.col(0); yM.col(1) = pts.col(1); } - smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( - xM, yM, - x0, x1, - y0, y1, - dydx0, dydx1, - curveName); + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + xM, yM, + x0, x1, + y0, y1, + dydx0, dydx1, + curveName); } @@ -1133,11 +1134,11 @@ void TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( - double angleAtOneNormTorque, - double angularStandardDeviation, - const std::string& curveName, - RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& - smoothSegmentedFunctionToUpdate ) + double angleAtOneNormTorque, + double angularStandardDeviation, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) { createGaussianShapedActiveTorqueAngleCurve( @@ -1149,67 +1150,67 @@ void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( curveName, smoothSegmentedFunctionToUpdate ); -} +} void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( - double angleAtOneNormTorque, - double angularStandardDeviation, - double minSlopeAtShoulders, - double minValueAtShoulders, - double curviness, - const std::string& curveName, - RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& - smoothSegmentedFunctionToUpdate ) + double angleAtOneNormTorque, + double angularStandardDeviation, + double minSlopeAtShoulders, + double minValueAtShoulders, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) { - if( (angularStandardDeviation < SQRTEPS) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createGaussianShapedActiveTorqueAngleCurve " - << curveName - << ": angularStandardDeviation is less than sqrt(eps)" - << endl; - assert(0); - abort(); - } - - if( (minValueAtShoulders < 0) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createGaussianShapedActiveTorqueAngleCurve " - << curveName - << ": minValueAtShoulders is less than 0" - << endl; - assert(0); - abort(); - } - - - if( (minSlopeAtShoulders < 0) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createGaussianShapedActiveTorqueAngleCurve " - << curveName - << ": minSlopeAtShoulders is less than 0" - << endl; - assert(0); - abort(); - } - - if( (curviness < 0 || curviness > 1.0) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createGaussianShapedActiveTorqueAngleCurve " - << curveName - << ": curviness must be in the interval [0,1]" - << endl; - assert(0); - abort(); - } + if( (angularStandardDeviation < SQRTEPS) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createGaussianShapedActiveTorqueAngleCurve " + << curveName + << ": angularStandardDeviation is less than sqrt(eps)" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( (minValueAtShoulders < 0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createGaussianShapedActiveTorqueAngleCurve " + << curveName + << ": minValueAtShoulders is less than 0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + + if( (minSlopeAtShoulders < 0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createGaussianShapedActiveTorqueAngleCurve " + << curveName + << ": minSlopeAtShoulders is less than 0" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( (curviness < 0 || curviness > 1.0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createGaussianShapedActiveTorqueAngleCurve " + << curveName + << ": curviness must be in the interval [0,1]" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } double taCutoff = 1e-3; - if(taCutoff < minValueAtShoulders ){ + if(taCutoff < minValueAtShoulders ) { taCutoff = minValueAtShoulders; } double angularStandardDeviationSq = - angularStandardDeviation*angularStandardDeviation; + angularStandardDeviation*angularStandardDeviation; double thetaWidth = sqrt(-log(taCutoff)*2*angularStandardDeviationSq); double thetaMin = -thetaWidth + angleAtOneNormTorque; double thetaMax = thetaWidth + angleAtOneNormTorque; @@ -1223,7 +1224,7 @@ void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( double x3 = angleAtOneNormTorque + angularStandardDeviation; if( (angleAtOneNormTorque-thetaMin) < - (angleAtOneNormTorque-angularStandardDeviation)){ + (angleAtOneNormTorque-angularStandardDeviation)) { x1 = angleAtOneNormTorque - 0.5*thetaMin; x3 = angleAtOneNormTorque + 0.5*thetaMin; } @@ -1232,11 +1233,11 @@ void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( double y0 = minValueAtShoulders; double y1 = exp(-(x1-angleAtOneNormTorque)*(x1-angleAtOneNormTorque) - / (2*angularStandardDeviationSq) ); + / (2*angularStandardDeviationSq) ); double y2 = exp(-(x2-angleAtOneNormTorque)*(x2-angleAtOneNormTorque) - / (2*angularStandardDeviationSq) ); + / (2*angularStandardDeviationSq) ); double y3 = exp(-(x3-angleAtOneNormTorque)*(x3-angleAtOneNormTorque) - / (2*angularStandardDeviationSq) ); + / (2*angularStandardDeviationSq) ); double y4 = minValueAtShoulders; @@ -1250,10 +1251,10 @@ void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( double dydx0 = minSlopeAtShoulders; double dydx4 = minSlopeAtShoulders; - if(dydx1 < 0){ + if(dydx1 < 0) { dydx0 *= -1.0; } - if(dydx3 < 0){ + if(dydx3 < 0) { dydx4 *= -1.0; } @@ -1263,43 +1264,43 @@ void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( MatrixNd pts(6,2); pts = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints( - x0,y0, dydx0, - x1,y1, dydx1, c); + calcQuinticBezierCornerControlPoints( + x0,y0, dydx0, + x1,y1, dydx1, c); xM.col(0) = pts.col(0); yM.col(0) = pts.col(1); pts = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints( - x1,y1, dydx1, - x2,y2, dydx2, c); + calcQuinticBezierCornerControlPoints( + x1,y1, dydx1, + x2,y2, dydx2, c); xM.col(1) = pts.col(0); yM.col(1) = pts.col(1); pts = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints( - x2,y2, dydx2, - x3,y3, dydx3, c); + calcQuinticBezierCornerControlPoints( + x2,y2, dydx2, + x3,y3, dydx3, c); xM.col(2) = pts.col(0); yM.col(2) = pts.col(1); pts = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints( - x3,y3, dydx3, - x4,y4, dydx4, c); + calcQuinticBezierCornerControlPoints( + x3,y3, dydx3, + x4,y4, dydx4, c); xM.col(3) = pts.col(0); yM.col(3) = pts.col(1); smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( - xM, yM, - x0, x4, - y0, y4, - dydx0,dydx4, - curveName); + xM, yM, + x0, x4, + y0, y4, + dydx0,dydx4, + curveName); } @@ -1309,74 +1310,74 @@ void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( //============================================================================= void TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( - double angularStretchAtOneNormTorque, - const std::string& curveName, - RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& - smoothSegmentedFunctionToUpdate ) + double angularStretchAtOneNormTorque, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) { createTendonTorqueAngleCurve( - angularStretchAtOneNormTorque, - 1.5/angularStretchAtOneNormTorque, - 1.0/3.0, - 0.5, - curveName, - smoothSegmentedFunctionToUpdate ); + angularStretchAtOneNormTorque, + 1.5/angularStretchAtOneNormTorque, + 1.0/3.0, + 0.5, + curveName, + smoothSegmentedFunctionToUpdate ); } void TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( - double angularStretchAtOneNormTorque, - double stiffnessAtOneNormTorque, - double normTorqueAtToeEnd, - double curviness, - const std::string& curveName, - RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& - smoothSegmentedFunctionToUpdate ) + double angularStretchAtOneNormTorque, + double stiffnessAtOneNormTorque, + double normTorqueAtToeEnd, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) { - if( angularStretchAtOneNormTorque < SQRTEPS ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createTendonTorqueAngleCurve " - << curveName - << ": angularStretchAtOneNormTorque should be greater than sqrt(eps)" - << endl; - assert(0); - abort(); - } - - if( stiffnessAtOneNormTorque < 1.1/angularStretchAtOneNormTorque){ - cerr << "TorqueMuscleFunctionFactory::" - << "createTendonTorqueAngleCurve " - << curveName - << ": stiffnessAtOneNormTorque should be greater " - << " than 1.1/angularStretchAtOneNormTorque" - << endl; - assert(0); - abort(); - } - - if( normTorqueAtToeEnd < SQRTEPS || normTorqueAtToeEnd > 0.99){ - cerr << "TorqueMuscleFunctionFactory::" - << "createTendonTorqueAngleCurve " - << curveName - << ": normTorqueAtToeEnd must be in the inteval [sqrt(eps), 0.99]" - << endl; - assert(0); - abort(); - } - - if( (curviness < 0 || curviness > 1.0) ){ - cerr << "TorqueMuscleFunctionFactory::" - << "createTendonTorqueAngleCurve " - << curveName - << ": curviness must be in the interval [0,1]" - << endl; - assert(0); - abort(); - } + if( angularStretchAtOneNormTorque < SQRTEPS ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createTendonTorqueAngleCurve " + << curveName + << ": angularStretchAtOneNormTorque should be greater than sqrt(eps)" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( stiffnessAtOneNormTorque < 1.1/angularStretchAtOneNormTorque) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createTendonTorqueAngleCurve " + << curveName + << ": stiffnessAtOneNormTorque should be greater " + << " than 1.1/angularStretchAtOneNormTorque" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( normTorqueAtToeEnd < SQRTEPS || normTorqueAtToeEnd > 0.99) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createTendonTorqueAngleCurve " + << curveName + << ": normTorqueAtToeEnd must be in the inteval [sqrt(eps), 0.99]" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } + + if( (curviness < 0 || curviness > 1.0) ) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createTendonTorqueAngleCurve " + << curveName + << ": curviness must be in the interval [0,1]" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); + } double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); double x0 = 0; @@ -1414,22 +1415,22 @@ void TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( //Compute the Quintic Bezier control points pts = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x0, y0, dydx0, - xToeCtrl,yToeCtrl,dydxToeMid, c); + calcQuinticBezierCornerControlPoints(x0, y0, dydx0, + xToeCtrl,yToeCtrl,dydxToeMid, c); xM.col(0) = pts.col(0); yM.col(0) = pts.col(1); pts = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(xToeCtrl, yToeCtrl, dydxToeMid, - xToe, yToe, dydxIso, c); + calcQuinticBezierCornerControlPoints(xToeCtrl, yToeCtrl, dydxToeMid, + xToe, yToe, dydxIso, c); xM.col(1) = pts.col(0); yM.col(1) = pts.col(1); smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction(xM,yM, - x0,xToe, - y0,yToe, - dydx0,dydxIso, - curveName); + x0,xToe, + y0,yToe, + dydx0,dydxIso, + curveName); } @@ -1439,29 +1440,29 @@ void TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( //============================================================================= void TorqueMuscleFunctionFactory::createDampingBlendingCurve( - double normAngularVelocityAtMaximumDamping, - const std::string& curveName, - RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& - smoothSegmentedFunctionToUpdate) + double normAngularVelocityAtMaximumDamping, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate) { - if( abs(normAngularVelocityAtMaximumDamping) < SQRTEPS){ - cerr << "TorqueMuscleFunctionFactory::" - << "createDampingBlendingCurve " - << curveName - << ": |normAngularVelocityAtMaximumDamping| < SQRTEPS" - << endl; - assert(0); - abort(); + if( abs(normAngularVelocityAtMaximumDamping) < SQRTEPS) { + ostringstream errormsg; + errormsg << "TorqueMuscleFunctionFactory::" + << "createDampingBlendingCurve " + << curveName + << ": |normAngularVelocityAtMaximumDamping| < SQRTEPS" + << endl; + throw RigidBodyDynamics::Errors::RBDLInvalidParameterError(errormsg.str()); } double x0,x1,x2,y0,y1,y2,dydx0,dydx1,dydx2; - if(normAngularVelocityAtMaximumDamping > 0){ + if(normAngularVelocityAtMaximumDamping > 0) { x0 = 0.; x2 = normAngularVelocityAtMaximumDamping; y0 = 0.; y2 = 1.; - }else{ + } else { x0 = normAngularVelocityAtMaximumDamping; x2 = 0.; y0 = 1.0; @@ -1482,22 +1483,22 @@ void TorqueMuscleFunctionFactory::createDampingBlendingCurve( //Compute the Quintic Bezier control points pts = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x0, y0, dydx0, - x1, y1, dydx1, c); + calcQuinticBezierCornerControlPoints(x0, y0, dydx0, + x1, y1, dydx1, c); xM.col(0) = pts.col(0); yM.col(0) = pts.col(1); pts = SegmentedQuinticBezierToolkit:: - calcQuinticBezierCornerControlPoints(x1, y1, dydx1, - x2, y2, dydx2, c); + calcQuinticBezierCornerControlPoints(x1, y1, dydx1, + x2, y2, dydx2, c); xM.col(1) = pts.col(0); yM.col(1) = pts.col(1); smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction(xM,yM, - x0,x2, - y0,y2, - dydx0,dydx2, - curveName); + x0,x2, + y0,y2, + dydx0,dydx2, + curveName); } diff --git a/addons/muscle/csvtools.cc b/addons/muscle/csvtools.cc index b8b5517..ff440a8 100755 --- a/addons/muscle/csvtools.cc +++ b/addons/muscle/csvtools.cc @@ -61,11 +61,11 @@ std::vector > readMatrixFromFile( pos2 = line.find(",",pos1); //if this is the first time running this loop, count //the number of columns - if(pos2 != std::string::npos && row == 0) + if(pos2 >= 0 && pos2 < line.length() && row == 0) matrixColNum++; pos1 = pos2+1; - }while(pos2 != std::string::npos); + }while(pos2 >= 0 && pos2 < line.length()); //Initial matrix sizing if(row == 0){ @@ -82,7 +82,7 @@ std::vector > readMatrixFromFile( pos2 = 0; for(int i=0; i < matrixColNum; i++){ pos2 = line.find(",",pos1); - if(pos2 == std::string::npos) + if(pos2 < 0 && pos2 >= line.length()) pos2 = line.length(); entry = line.substr(pos1,pos2-pos1); pos1 = pos2+1; diff --git a/addons/muscle/muscle.h b/addons/muscle/muscle.h index f348e7b..f849506 100755 --- a/addons/muscle/muscle.h +++ b/addons/muscle/muscle.h @@ -1,7 +1,7 @@ //============================================================================== /* * RBDL - Rigid Body Dynamics Library: Addon : muscle - * Copyright (c) 2016 Matthew Millard + * Copyright (c) 2016 Matthew Millard * * Licensed under the zlib license. See LICENSE for more details. */ @@ -9,6 +9,8 @@ #define MUSCLE_H_ #include "Millard2016TorqueMuscle.h" -#include "MuscleFunctionFactory.h" -#include "TorqueMuscleFunctionFactory.h" +#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING + #include "TorqueMuscleFittingToolkit.h" +#endif + #endif diff --git a/addons/muscle/tests/CMakeLists.txt b/addons/muscle/tests/CMakeLists.txt index c76f2bf..197e169 100755 --- a/addons/muscle/tests/CMakeLists.txt +++ b/addons/muscle/tests/CMakeLists.txt @@ -1,35 +1,48 @@ CMAKE_MINIMUM_REQUIRED (VERSION 3.0) -SET ( RBDL_ADDON_MUSCLE_TESTS_VERSION_MAJOR 1 ) +#CMAKE_POLICY(SET CMP0048 NEW) +#CMAKE_POLICY(SET CMP0040 NEW) + +SET ( RBDL_ADDON_MUSCLE_TESTS_VERSION_MAJOR 2 ) SET ( RBDL_ADDON_MUSCLE_TESTS_VERSION_MINOR 0 ) SET ( RBDL_ADDON_MUSCLE_TESTS_VERSION_PATCH 0 ) -SET ( RBDL_ADDON_MUSCLE_VERSION - ${RBDL_ADDON_MUSCLE_TESTS_VERSION_MAJOR}.${RBDL_ADDON_MUSCLE_TESTS_VERSION_MINOR}.${RBDL_ADDON_MUSCLE_TESTS_VERSION_PATCH} +SET ( RBDL_ADDON_MUSCLE_TESTS_VERSION + ${RBDL_ADDON_MUSCLE_TESTS_VERSION_MAJOR}.${RBDL_ADDON_MUSCLE_TESTS_VERSION_MINOR}.${RBDL_ADDON_MUSCLE_TESTS_VERSION_PATCH} ) -PROJECT (RBDL_MUSCLE_TESTS VERSION ${RBDL_ADDON_MUSCLE_VERSION}) -#SET (PROJECT_VERSION ${RBDL_ADDON_MUSCLE_TESTS_VERSION}) +PROJECT (RBDL_MUSCLE_TESTS VERSION ${RBDL_ADDON_MUSCLE_TESTS_VERSION}) -# Needed for UnitTest++ -LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../CMake ) +# Look for catch2 +FIND_PACKAGE(Catch2 REQUIRED) +# on unix systems add /usr/local/include to include dirs +# this was added since macos homebrew installs many things there but macos +# does not automatically search their, and on linux systems this does not hurt +IF (NOT WIN32) + INCLUDE_DIRECTORIES(/usr/local/include) +ENDIF (NOT WIN32) -# Look for unittest++ -FIND_PACKAGE (UnitTest++ REQUIRED) -INCLUDE_DIRECTORIES (${UNITTEST++_INCLUDE_DIR}) +INCLUDE_DIRECTORIES (${CMAKE_SOURCE_DIR}/tests) -SET ( MUSCLE_TESTS_SRCS - testMillard2016TorqueMuscle.cc - testMuscleFunctionFactory.cc - testTorqueMuscleFunctionFactory.cc +IF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + INCLUDE_DIRECTORIES (${IPOPT_INCLUDE_DIR}) +ENDIF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + +IF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + SET ( MUSCLE_TESTS_SRCS + testMillard2016TorqueMuscle.cc + testMuscleFunctionFactory.cc + testTorqueMuscleFunctionFactory.cc ../muscle.h ../MuscleFunctionFactory.h ../MuscleFunctionFactory.cc ../TorqueMuscleFunctionFactory.h - ../TorqueMuscleFunctionFactory.cc + ../TorqueMuscleFunctionFactory.cc ../Millard2016TorqueMuscle.h ../Millard2016TorqueMuscle.cc + ../TorqueMuscleFittingToolkit.cc + ../TorqueMuscleFittingToolkit.h ../csvtools.h ../csvtools.cc ../../geometry/geometry.h @@ -38,10 +51,38 @@ SET ( MUSCLE_TESTS_SRCS ../../geometry/SegmentedQuinticBezierToolkit.cc ../../geometry/SmoothSegmentedFunction.cc ../../geometry/tests/numericalTestFunctions.cc - ../../geometry/tests/numericalTestFunctions.h - ) + ../../geometry/tests/numericalTestFunctions.h + ) + +ELSE (RBDL_BUILD_ADDON_MUSCLE_FITTING) + + SET ( MUSCLE_TESTS_SRCS + testMillard2016TorqueMuscle.cc + testMuscleFunctionFactory.cc + testTorqueMuscleFunctionFactory.cc + ../muscle.h + ../MuscleFunctionFactory.h + ../MuscleFunctionFactory.cc + ../TorqueMuscleFunctionFactory.h + ../TorqueMuscleFunctionFactory.cc + ../Millard2016TorqueMuscle.h + ../Millard2016TorqueMuscle.cc + ../csvtools.h + ../csvtools.cc + ../../geometry/geometry.h + ../../geometry/SegmentedQuinticBezierToolkit.h + ../../geometry/SmoothSegmentedFunction.h + ../../geometry/SegmentedQuinticBezierToolkit.cc + ../../geometry/SmoothSegmentedFunction.cc + ../../geometry/tests/numericalTestFunctions.cc + ../../geometry/tests/numericalTestFunctions.h + ) + + +ENDIF(RBDL_BUILD_ADDON_MUSCLE_FITTING) + + -INCLUDE_DIRECTORIES ( ../../../geometry ) SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES LINKER_LANGUAGE CXX @@ -50,26 +91,32 @@ SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES ADD_EXECUTABLE ( rbdl_muscle_tests ${MUSCLE_TESTS_SRCS} ) SET_TARGET_PROPERTIES ( rbdl_muscle_tests PROPERTIES - LINKER_LANGUAGE CXX - OUTPUT_NAME runMuscleTests - ) + LINKER_LANGUAGE CXX + OUTPUT_NAME muscle_tests + ) SET (RBDL_LIBRARY rbdl) IF (RBDL_BUILD_STATIC) - SET (RBDL_LIBRARY rbdl-static) + SET (RBDL_LIBRARY rbdl-static) ENDIF (RBDL_BUILD_STATIC) +IF (RBDL_BUILD_ADDON_MUSCLE_FITTING) TARGET_LINK_LIBRARIES ( rbdl_muscle_tests - ${UNITTEST++_LIBRARY} - ${RBDL_LIBRARY} - ) - + ${RBDL_LIBRARY} + ${IPOPT_LIBRARY} + ) + +ELSE (RBDL_BUILD_ADDON_MUSCLE_FITTING) +TARGET_LINK_LIBRARIES ( rbdl_muscle_tests + ${RBDL_LIBRARY} +) +ENDIF (RBDL_BUILD_ADDON_MUSCLE_FITTING) OPTION (RUN_AUTOMATIC_TESTS "Perform automatic tests after compilation?" OFF) IF (RUN_AUTOMATIC_TESTS) ADD_CUSTOM_COMMAND (TARGET rbdl_muscle_tests - POST_BUILD - COMMAND ./runMuscleTests - COMMENT "Running automated addon muscle tests..." - ) + POST_BUILD + COMMAND ./muscle_tests + COMMENT "Running automated addon muscle tests..." + ) ENDIF (RUN_AUTOMATIC_TESTS) diff --git a/addons/muscle/tests/testMillard2016TorqueMuscle.cc b/addons/muscle/tests/testMillard2016TorqueMuscle.cc index 0f3f07b..9612810 100755 --- a/addons/muscle/tests/testMillard2016TorqueMuscle.cc +++ b/addons/muscle/tests/testMillard2016TorqueMuscle.cc @@ -1,814 +1,1706 @@ -/* * - * - * Copyright (c) 2016 Matthew Millard - * - * Licensed under the zlib license. See LICENSE for more details. - */ - - -//============================================================================== -// INCLUDES -//============================================================================== - - - -#include "../Millard2016TorqueMuscle.h" -#include "../csvtools.h" -#include "../../geometry/tests/numericalTestFunctions.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -using namespace RigidBodyDynamics::Addons::Muscle; -using namespace RigidBodyDynamics::Addons::Geometry; -using namespace std; -/* - Constructor tests: - 1. Coefficients are copied over correctly. - 2. Curves are made correctly - - calcTorqueMuscleInfo test - stiffness calculation - power calculation - -*/ - -TEST(ConstructorRegularCallCheck) -{ - - - //Check that the 3 constructors when called properly - //do not abort - Millard2016TorqueMuscle test0 = Millard2016TorqueMuscle(); - - - SubjectInformation subjectInfo; - subjectInfo.gender = GenderSet::Male; - subjectInfo.ageGroup = AgeGroupSet::Young18To25; - subjectInfo.heightInMeters = 1.732; - subjectInfo.massInKg = 69.0; - - - Millard2016TorqueMuscle test2 = - Millard2016TorqueMuscle( - DataSet::Anderson2007, - subjectInfo, - Anderson2007::HipExtension, - 0.0, - 1.0, - 1.0, - "test_easyConstructor"); - - CHECK(abs( test2.getPassiveTorqueScale()-1.0) < TOL); - -} - - - -TEST(calcJointTorqueCorrectnessTests){ - - - double jointAngleOffset = 0; - double signOfJointAngle = 1; - double signOfJointTorque = 1; - double err = 0.0; - - std::string name("test"); - - SubjectInformation subjectInfo; - subjectInfo.gender = GenderSet::Male; - subjectInfo.ageGroup = AgeGroupSet::Young18To25; - subjectInfo.heightInMeters = 1.732; - subjectInfo.massInKg = 69.0; - - Millard2016TorqueMuscle tq = - Millard2016TorqueMuscle(DataSet::Anderson2007, - subjectInfo, - Anderson2007::HipExtension, - jointAngleOffset, - signOfJointAngle, - signOfJointTorque, - name); - - bool flagMakeTestVector = false; - if(flagMakeTestVector){ - Millard2016TorqueMuscle tqG = - Millard2016TorqueMuscle(DataSet::Gymnast, - subjectInfo, - Gymnast::HipExtension, - jointAngleOffset, - signOfJointAngle, - signOfJointTorque, - name); - TorqueMuscleInfo tmiG; - tqG.calcTorqueMuscleInfo(M_PI/3.0,0.1,0.77,tmiG); - - printf("%f\n",tmiG.fiberAngle); - printf("%f\n",tmiG.fiberAngularVelocity); - printf("%f\n",tmiG.activation); - printf("%f\n",tmiG.fiberTorque); - printf("%f\n",tmiG.fiberStiffness); - printf("%f\n",tmiG.fiberPassiveTorqueAngleMultiplier); - printf("%f\n",tmiG.fiberActiveTorqueAngleMultiplier); - printf("%f\n",tmiG.fiberActiveTorqueAngularVelocityMultiplier); - printf("%f\n",tmiG.fiberPassiveTorque); - printf("%f\n",tmiG.fiberActiveTorque); - printf("%f\n",tmiG.fiberDampingTorque); - printf("%f\n",tmiG.fiberNormDampingTorque); - printf("%f\n",tmiG.fiberActivePower); - printf("%f\n",tmiG.fiberPassivePower); - printf("%f\n",tmiG.fiberPower); - printf("%f\n",tmiG.DjointTorqueDjointAngle); - printf("%f\n",tmiG.DjointTorqueDjointAngularVelocity); - printf("%f\n",tmiG.DjointTorqueDactivation); - - } - - - //Zero out the passive forces so that calcMuscleTorque reports - //just the active force - this allows us to test its correctness. - tq.setPassiveTorqueScale(0.0); - double tmp = tq.calcJointTorque(0,0,1.0); - - //Test that the get and set functions work for - //maximum isometric torque - double tauMaxOld = tq.getMaximumActiveIsometricTorque(); - double tauMax = tauMaxOld*10.0; - tq.setMaximumActiveIsometricTorque(tauMax); - tmp = tq.calcJointTorque(0,0,1.0); - //ensures updateTorqueMuscleCurves is called - CHECK(abs( tq.getMaximumActiveIsometricTorque()-tauMax) - < TOL ); - - double omegaMaxOld = tq.getMaximumConcentricJointAngularVelocity(); - double omegaMax = 2.0*fabs(omegaMaxOld); - tq.setMaximumConcentricJointAngularVelocity(omegaMax); - tmp = tq.calcJointTorque(0,0,1.0); - //ensures updateTorqueMuscleCurves is called - CHECK(abs( abs(tq.getMaximumConcentricJointAngularVelocity())-omegaMax) - < TOL ); - - //getParametersC1C2C3C4C5C6() has been removed and so this - //test is no longer possible. It is the responsibility of - //the underlying active-torque-angle curve to ensure that - //it peaks at 1.0 - /* - RigidBodyDynamics::Math::VectorNd c1c2c3c4c5c6 = - tq.getParametersC1C2C3C4C5C6(); - double thetaAtTauMax = c1c2c3c4c5c6[2]; - */ - //TorqueMuscleInfo tqInfo; - - //getParametersC1C2C3C4C5C6() has been removed. It is the - //responsibility of the underlying curve test code to - //check for correctness. - //double torque = tq.calcJointTorque(thetaAtTauMax,0.0,1.0); - //err = abs(torque -tauMax); - //CHECK(err< TOL); - - //These tests have been updated because Anderson - //torque velocity curve had to be replaced because it - //can behave badly (e.g. the concentric curve of the ankle - //extensors of a young man does not interest the x axis.) - // - //The new curves do not pass exactly through the points - //0.5*tauMax and 0.75*tauMax, but within 5% of this - //value. - - //getParametersC1C2C3C4C5C6() has been removed. It is the - //responsibility of the underlying curve test code to - //check for correctness. - //double omegaAt75TauMax = c1c2c3c4c5c6[3]; - //torque = tq.calcJointTorque(thetaAtTauMax,omegaAt75TauMax,1.0); - //err = abs(torque - 0.75*tauMax)/tauMax; - //CHECK( err < 0.05); - - //double omegaAt50TauMax = c1c2c3c4c5c6[4]; - //torque = tq.calcJointTorque(thetaAtTauMax,omegaAt50TauMax,1.0); - //err = abs(torque -0.50*tauMax)/tauMax; - //CHECK(err < 0.05); - - -} - -TEST(dampingTermTests){ - - double err = 0.; - double jointAngleOffset = 0; - double signOfJointAngle = 1; - double signOfJointTorque = 1; - double signOfJointVelocity = signOfJointTorque; - - std::string name("test"); - - SubjectInformation subjectInfo; - subjectInfo.gender = GenderSet::Female; - subjectInfo.ageGroup = AgeGroupSet::SeniorOver65; - subjectInfo.heightInMeters = 1.732; - subjectInfo.massInKg = 69.0; - - Millard2016TorqueMuscle tq = - Millard2016TorqueMuscle(DataSet::Anderson2007, - subjectInfo, - Anderson2007::HipExtension, - jointAngleOffset, - signOfJointAngle, - signOfJointTorque, - name); - TorqueMuscleInfo tmi0; - //Test the damping term - double beta = tq.getNormalizedDampingCoefficient(); - tq.setNormalizedDampingCoefficient(beta+0.1); - CHECK(abs(beta+0.1-tq.getNormalizedDampingCoefficient()) 0){ - idx = 0; - }else if(b1k1b2k2[2] > 0){ - idx = 2; - } - */ - - /* - double b = b1k1b2k2[idx]; - double k = b1k1b2k2[idx+1]; - double thetaAtPassiveTauMax = log(tauMax/b)/k; - - double thetaAtTauMax = c1c2c3c4c5c6[2]; - double omegaAt75TauMax = c1c2c3c4c5c6[3]; - double omegaAt50TauMax = c1c2c3c4c5c6[4]; - - double jointAngleAtTauMax = - signOfJointAngle*thetaAtTauMax+jointAngleOffset; - */ - - double jointAngleAtTauMax = tq.getJointAngleAtMaximumActiveIsometricTorque(); - TorqueMuscleInfo tmi; - tq.calcTorqueMuscleInfo(jointAngleAtTauMax, - 0., - activation, - tmi); - - //Keypoint check: active force components + fiber kinematics - - CHECK(abs(tmi.activation-1) < EPSILON); - CHECK(abs(tmi.jointAngle-jointAngleAtTauMax)< TOL); - CHECK(abs(tmi.jointAngularVelocity-0.) < TOL); - - CHECK(abs(tmi.activation-1) < EPSILON); - //CHECK(abs(tmi.fiberAngle-thetaAtTauMax) < TOL); - CHECK(abs(tmi.fiberAngularVelocity-0.) < TOL); - - CHECK(abs(tmi.fiberActiveTorque - tauMax) < TOL); - CHECK(abs(tmi.fiberActiveTorqueAngleMultiplier-1.0) < TOL); - CHECK(abs(tmi.fiberActiveTorqueAngularVelocityMultiplier-1.0) muscleVector; - - bool exception = false; - - double angleTorqueSigns[][2] = {{-1, 1}, - {-1,-1}, - { 1,-1}, - { 1, 1}, - {-1, 1}, - {-1,-1}}; - - Millard2016TorqueMuscle tqMuscle; - std::stringstream tqName; - int tqIdx; - - for(int i=0; i < Anderson2007::LastJointTorque; ++i){ - - tqName.str(""); - tqName << DataSet.names[0] - < + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +//Some particularly aggressive experimental data +unsigned int TorqueMuscleFittingHardTestCaseRows = 100; +unsigned int TorqueMuscleFittingHardTestCaseCols = 4; +double const TorqueMuscleFittingHardTestCase[100][4] = + {{0.,1.9148,-17.618,107.25 }, + {0.0044242,1.8318,-18.277,108.39 }, + {0.0088485,1.7485,-18.949,110.49 }, + {0.013273,1.6636,-19.618,114.57 }, + {0.017697,1.5761,-20.243,121.32 }, + {0.022121,1.486,-20.758,130.97 }, + {0.026545,1.3938,-21.085,143.31 }, + {0.03097,1.3004,-21.144,157.68 }, + {0.035394,1.2074,-20.866,173.11 }, + {0.039818,1.1164,-20.199,188.42 }, + {0.044242,1.0292,-19.118,202.36 }, + {0.048667,0.94777,-17.626,213.77 }, + {0.053091,0.87376,-15.762,221.64 }, + {0.057515,0.80872,-13.594,225.25 }, + {0.061939,0.75378,-11.22,224.18 }, + {0.066364,0.70958,-8.7587,218.42 }, + {0.070788,0.67624,-6.3387,208.34 }, + {0.075212,0.65329,-4.0821,194.73 }, + {0.079636,0.63975,-2.0906,178.7 }, + {0.084061,0.63427,-0.43189,161.6 }, + {0.088485,0.63528,0.87052,144.75 }, + {0.092909,0.6412,1.8387,129.18 }, + {0.097333,0.6507,2.5306,115.24 }, + {0.10176,0.66277,3.015,102.36 }, + {0.10618,0.67685,3.3393,88.889 }, + {0.11061,0.69264,3.517,72.481 }, + {0.11503,0.71007,3.5756,50.979 }, + {0.11945,0.72927,3.6372,23.504 }, + {0.12388,0.75048,3.852,-8.9796 }, + {0.1283,0.77332,4.1461,-43.995 }, + {0.13273,0.79611,4.1835,-78.49 }, + {0.13715,0.81635,3.6883,-109.71 }, + {0.14158,0.83172,2.6498,-135.63 }, + {0.146,0.84066,1.2202,-154.98 }, + {0.15042,0.84231,-0.42406,-167.16 }, + {0.15485,0.83637,-2.1355,-172.03 }, + {0.15927,0.82297,-3.8027,-169.8 }, + {0.1637,0.80254,-5.3408,-160.95 }, + {0.16812,0.7758,-6.6819,-146.16 }, + {0.17255,0.74371,-7.771,-126.31 }, + {0.17697,0.70746,-8.5684,-102.56 }, + {0.18139,0.66837,-9.0566,-76.369 }, + {0.18582,0.62778,-9.2487,-49.623 }, + {0.19024,0.58691,-9.1941,-24.563 }, + {0.19467,0.54667,-8.9737,-3.6038 }, + {0.19909,0.50763,-8.6741,11.165 }, + {0.20352,0.47001,-8.3313,18.713 }, + {0.20794,0.43418,-7.8528,19.766 }, + {0.21236,0.40128,-6.9609,16.835 }, + {0.21679,0.37397,-5.266,12.889 }, + {0.22121,0.35642,-2.5349,9.4338 }, + {0.22564,0.35296,1.0546,6.2364 }, + {0.23006,0.36634,4.9935,2.8315 }, + {0.23448,0.39689,8.7586,-0.69647 }, + {0.23891,0.44304,11.998,-4.1689 }, + {0.24333,0.50198,14.522,-7.5873 }, + {0.24776,0.57037,16.269,-11.01 }, + {0.25218,0.64488,17.306,-14.511 }, + {0.25661,0.72269,17.793,-18.219 }, + {0.26103,0.8018,17.929,-22.301 }, + {0.26545,0.8811,17.902,-26.91 }, + {0.26988,0.96017,17.846,-32.128 }, + {0.2743,1.0391,17.834,-37.93 }, + {0.27873,1.1181,17.879,-44.158 }, + {0.28315,1.1973,17.961,-50.527 }, + {0.28758,1.277,18.039,-56.666 }, + {0.292,1.3569,18.078,-62.174 }, + {0.29642,1.4368,18.054,-66.692 }, + {0.30085,1.5165,17.955,-69.98 }, + {0.30527,1.5956,17.784,-71.983 }, + {0.3097,1.6738,17.552,-72.861 }, + {0.31412,1.7509,17.272,-72.976 }, + {0.31855,1.8266,16.952,-72.834 }, + {0.32297,1.9008,16.594,-72.982 }, + {0.32739,1.9733,16.191,-73.899 }, + {0.33182,2.0439,15.724,-75.903 }, + {0.33624,2.1123,15.162,-79.088 }, + {0.34067,2.1779,14.47,-83.322 }, + {0.34509,2.24,13.609,-88.273 }, + {0.34952,2.2979,12.55,-93.472 }, + {0.35394,2.3507,11.283,-98.369 }, + {0.35836,2.3974,9.8194,-102.39 }, + {0.36279,2.4373,8.1988,-105.01 }, + {0.36721,2.4698,6.4835,-105.77 }, + {0.37164,2.4947,4.7502,-104.37 }, + {0.37606,2.512,3.0759,-100.74 }, + {0.38048,2.5221,1.5245,-95.063 }, + {0.38491,2.5257,0.13371,-87.783 }, + {0.38933,2.5235,-1.0913,-79.537 }, + {0.39376,2.5163,-2.178,-71.042 }, + {0.39818,2.5045,-3.1764,-62.958 }, + {0.40261,2.4884,-4.1428,-55.768 }, + {0.40703,2.468,-5.122,-49.731 }, + {0.41145,2.443,-6.136,-44.898 }, + {0.41588,2.4132,-7.1827,-41.179 }, + {0.4203,2.3784,-8.2437,-38.413 }, + {0.42473,2.3383,-9.2979,-36.422 }, + {0.42915,2.2933,-10.332,-35.021 }, + {0.43358,2.2443,-11.347,-34.007 }, + {0.438,2.193,-12.35,-33.158 }}; + +//============================================================================== +// INCLUDES +//============================================================================== + +#define CATCH_CONFIG_MAIN +#include "../Millard2016TorqueMuscle.h" +#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING + #include "../TorqueMuscleFittingToolkit.h" +#endif +#include "../csvtools.h" +#include "../../geometry/tests/numericalTestFunctions.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rbdl_tests.h" + +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace std; +/* + Constructor tests: + 1. Coefficients are copied over correctly. + 2. Curves are made correctly + + calcTorqueMuscleInfo test + stiffness calculation + power calculation + +*/ + + +TEST_CASE(__FILE__"_ConstructorRegularCallCheck", "") +{ + + + //Check that the 3 constructors when called properly + //do not abort + Millard2016TorqueMuscle test0 = Millard2016TorqueMuscle(); + + + SubjectInformation subjectInfo; + subjectInfo.gender = GenderSet::Male; + subjectInfo.ageGroup = AgeGroupSet::Young18To25; + subjectInfo.heightInMeters = 1.732; + subjectInfo.massInKg = 69.0; + + + Millard2016TorqueMuscle test2 = + Millard2016TorqueMuscle( + DataSet::Anderson2007, + subjectInfo, + Anderson2007::HipExtension, + 0.0, + 1.0, + 1.0, + "test_easyConstructor"); + + CHECK(fabs( test2.getPassiveTorqueScale()-1.0) < TOL); + +} + + + +TEST_CASE(__FILE__"_calcJointTorqueCorrectnessTests", ""){ + + double jointAngleOffset = 0; + double signOfJointAngle = 1; + double signOfJointTorque = 1; + double err = 0.0; + + std::string name("test"); + + SubjectInformation subjectInfo; + subjectInfo.gender = GenderSet::Male; + subjectInfo.ageGroup = AgeGroupSet::Young18To25; + subjectInfo.heightInMeters = 1.732; + subjectInfo.massInKg = 69.0; + + Millard2016TorqueMuscle tq = + Millard2016TorqueMuscle(DataSet::Anderson2007, + subjectInfo, + Anderson2007::HipExtension, + jointAngleOffset, + signOfJointAngle, + signOfJointTorque, + name); + + bool flagMakeTestVector = false; + if(flagMakeTestVector){ + Millard2016TorqueMuscle tqG = + Millard2016TorqueMuscle(DataSet::Gymnast, + subjectInfo, + Gymnast::HipExtension, + jointAngleOffset, + signOfJointAngle, + signOfJointTorque, + name); + TorqueMuscleInfo tmiG; + tqG.calcTorqueMuscleInfo(M_PI/3.0,0.1,0.77,tmiG); + + printf("%f\n",tmiG.fiberAngle); + printf("%f\n",tmiG.fiberAngularVelocity); + printf("%f\n",tmiG.activation); + printf("%f\n",tmiG.fiberTorque); + printf("%f\n",tmiG.fiberStiffness); + printf("%f\n",tmiG.fiberPassiveTorqueAngleMultiplier); + printf("%f\n",tmiG.fiberActiveTorqueAngleMultiplier); + printf("%f\n",tmiG.fiberTorqueAngularVelocityMultiplier); + printf("%f\n",tmiG.fiberPassiveTorque); + printf("%f\n",tmiG.fiberActiveTorque); + printf("%f\n",tmiG.fiberDampingTorque); + printf("%f\n",tmiG.fiberNormDampingTorque); + printf("%f\n",tmiG.fiberActivePower); + printf("%f\n",tmiG.fiberPassivePower); + printf("%f\n",tmiG.fiberPower); + printf("%f\n",tmiG.DjointTorque_DjointAngle); + printf("%f\n",tmiG.DjointTorque_DjointAngularVelocity); + printf("%f\n",tmiG.DjointTorque_Dactivation); + + } + + + //Zero out the passive forces so that calcMuscleTorque reports + //just the active force - this allows us to test its correctness. + tq.setPassiveTorqueScale(0.0); + double tmp = tq.calcJointTorque(0,0,1.0); + + //Test that the get and set functions work for + //maximum isometric torque + double tauMaxOld = tq.getMaximumActiveIsometricTorque(); + double tauMax = tauMaxOld*10.0; + tq.setMaximumActiveIsometricTorque(tauMax); + tmp = tq.calcJointTorque(0,0,1.0); + //ensures updateTorqueMuscleCurves is called + CHECK(fabs( tq.getMaximumActiveIsometricTorque()-tauMax) + < TOL ); + + double omegaMaxOld = tq.getMaximumConcentricJointAngularVelocity(); + double omegaMax = 2.0*fabs(omegaMaxOld); + tq.setMaximumConcentricJointAngularVelocity(omegaMax); + tmp = tq.calcJointTorque(0,0,1.0); + //ensures updateTorqueMuscleCurves is called + CHECK(fabs( fabs(tq.getMaximumConcentricJointAngularVelocity())-omegaMax) + < TOL ); + + + double taAngleScalingOld = tq.getActiveTorqueAngleCurveAngleScaling(); + double taAngleScaling = 2.0*taAngleScalingOld; + tq.setActiveTorqueAngleCurveAngleScaling(taAngleScaling); + + CHECK(fabs(taAngleScaling-tq.getActiveTorqueAngleCurveAngleScaling()) = 23); + + //For joint-torque related constrains that use x = [a,v,p,o] + //(see the code for the parts of calcTorqueMuscleInfo that evaluate + //the second derivatives for further detail. + //Test the first column of the Hessian + //0: D^2 tau / D lambdaTa^2 + tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda); + tq.setTorqueAngularVelocityCurveBlendingVariable( tvLambda); + tq.setActiveTorqueAngleCurveBlendingVariable( taLambda); + + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setActiveTorqueAngleCurveBlendingVariable( taLambda-h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setActiveTorqueAngleCurveBlendingVariable( taLambda+h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + double D2tau_DtaLambda2 = + (tmiR.DjointTorque_DactiveTorqueAngleBlendingVariable + - tmiL.DjointTorque_DactiveTorqueAngleBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtaLambda2 + - tmi.fittingInfo[0]); + CHECK(err < SQRTEPSILON*100); + + + //1: Test D^2 tau / D TaLambda D TvLambda + double D2tau_DtaLambdaDtvLambda = + (tmiR.DjointTorque_DtorqueAngularVelocityBlendingVariable + - tmiL.DjointTorque_DtorqueAngularVelocityBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtaLambdaDtvLambda + -tmi.fittingInfo[1]); + CHECK(err < SQRTEPSILON*100); + + //3: Test D^2 tau/ D TaLambda D TpLambda + double D2tau_DtaLambdaDtpLambda = + (tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable + -tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtaLambdaDtpLambda + -tmi.fittingInfo[3]); + CHECK(err < SQRTEPSILON*100); + + //6: Test D^2 tau/ D TaLambda DTpOffset + double D2tau_DtaLambdaDtpOffset = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + -tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtaLambdaDtpOffset - + tmi.fittingInfo[6]); + + //Check the 2nd column of the Hessian + //2: Test D^2 tau/ D Tvlambda^2 + tq.setActiveTorqueAngleCurveBlendingVariable( taLambda); + + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setTorqueAngularVelocityCurveBlendingVariable( tvLambda - h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setTorqueAngularVelocityCurveBlendingVariable( tvLambda + h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + double D2tau_DtvLambda2 = + (tmiR.DjointTorque_DtorqueAngularVelocityBlendingVariable + - tmiL.DjointTorque_DtorqueAngularVelocityBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtvLambda2 + - tmi.fittingInfo[2]); + CHECK(err < SQRTEPSILON*100); + + //4: Test D^2 tau/ D TvLambda D Tplambda + double D2tau_DtvLambdaDtpLambda = + (tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable + - tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtvLambdaDtpLambda + - tmi.fittingInfo[4]); + CHECK(err < SQRTEPSILON*100); + + //7: Test D^2 tau/ D TvLambda D TpOffset + double D2tau_DtvLambdaDtpOffset = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + - tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtvLambdaDtpOffset + - tmi.fittingInfo[7]); + CHECK(err < SQRTEPSILON*100); + + //Check the 3rd column of the Hessian. + //5: D^2 tau / D TpLambda^2 + tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda); + + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda - h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda + h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + double D2tau_DtpLambda2 = + (tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable + - tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtpLambda2 + - tmi.fittingInfo[5]); + CHECK(err < SQRTEPSILON*100); + + //8: D^2 tau/ D TpLambda DTpOffset + double D2tau_DtpLambdaDtpOffset = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + - tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtpLambdaDtpOffset + - tmi.fittingInfo[8]); + CHECK(err < SQRTEPSILON*100); + + //Check the 4th column of the Hessian. + //9: D^2 tau/ D TpOffset^2 + tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda); + + tq.setPassiveCurveAngleOffset(0.); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setPassiveCurveAngleOffset(-h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setPassiveCurveAngleOffset( h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + double D2tau_DtpOffset2 = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + - tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtpOffset2 + - tmi.fittingInfo[9]); + CHECK(err < SQRTEPSILON*1000); + + +// For constraints on the value of the passive element +// 10: d2p/dp2 + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda-h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda+h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + double D2tp_DtpLambda2 = + (tmiR.DfiberPassiveTorqueAngleMultiplier_DblendingVariable + - tmiL.DfiberPassiveTorqueAngleMultiplier_DblendingVariable)/(2.0*h); + + err = fabs(D2tp_DtpLambda2 + - tmi.fittingInfo[10]); + CHECK(err < SQRTEPSILON*1000); + + // 11: d2p/dpdo + double D2tp_DtpLambda_DangleOffset = + (tmiR.DfiberPassiveTorqueAngleMultiplier_DangleOffset + -tmiL.DfiberPassiveTorqueAngleMultiplier_DangleOffset)/(2.0*h); + + err = fabs(D2tp_DtpLambda_DangleOffset + -tmi.fittingInfo[11]); + CHECK(err < SQRTEPSILON*1000); + +// 12: d2p/do2 + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda); + tq.setPassiveCurveAngleOffset(0.); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setPassiveCurveAngleOffset(-h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setPassiveCurveAngleOffset( h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + double D2tp_DtpOffset2 = + (tmiR.DfiberPassiveTorqueAngleMultiplier_DangleOffset + - tmiL.DfiberPassiveTorqueAngleMultiplier_DangleOffset)/(2.0*h); + + err = fabs(D2tp_DtpOffset2 + - tmi.fittingInfo[12]); + CHECK(err < SQRTEPSILON*1000); + + +// For joint-torque related constraints that use x = [s,m,p,o] +// 13: d2t/ds2 + tq.setPassiveTorqueAngleCurveBlendingVariable( 0.); + tq.setTorqueAngularVelocityCurveBlendingVariable( 0.); + tq.setActiveTorqueAngleCurveBlendingVariable( 0.); + + jointAngleAtOneNormTorque = tq.getJointAngleAtMaximumActiveIsometricTorque(); + + tq.setActiveTorqueAngleCurveAngleScaling(1.0); + tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+0.5,omegaMid,1.0,tmi); + tq.setActiveTorqueAngleCurveAngleScaling(1.0-h); + tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+0.5,omegaMid,1.0,tmiL); + tq.setActiveTorqueAngleCurveAngleScaling(1.0+h); + tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+0.5,omegaMid,1.0,tmiR); + + double D2tau_DtaAngleScaling2 = + (tmiR.DjointTorque_DactiveTorqueAngleAngleScaling + - tmiL.DjointTorque_DactiveTorqueAngleAngleScaling)/(2.0*h); + + err = fabs(D2tau_DtaAngleScaling2 + - tmi.fittingInfo[13]); + CHECK(err < SQRTEPSILON*1000); + +// 14: d2t/dsdm + double D2tau_DtaAngleScaling_DomegaMax = + (tmiR.DjointTorque_DmaximumAngularVelocity + - tmiL.DjointTorque_DmaximumAngularVelocity)/(2.0*h); + + err = fabs(D2tau_DtaAngleScaling_DomegaMax + - tmi.fittingInfo[14]); + CHECK(err < SQRTEPSILON*100); + + // 16: d2t/dsdp + double D2tau_DtaAngleScaling_DtpLambda = + (tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable + -tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtaAngleScaling_DtpLambda + -tmi.fittingInfo[16]); + CHECK(err < SQRTEPSILON*100); + // 19: d2t/dsdo + double D2tau_DtaAngleScaling_DtpOffset = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + -tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtaAngleScaling_DtpOffset + -tmi.fittingInfo[19]); + CHECK(err < SQRTEPSILON*100); + +// 15: d2t/dm2 + tq.setActiveTorqueAngleCurveAngleScaling(1.0); + omegaMax = tq.getMaximumConcentricJointAngularVelocity(); + omegaMid = 0.5*omegaMax; + + tq.setMaximumConcentricJointAngularVelocity(omegaMax); + tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,omegaMid,1.0,tmi); + tq.setMaximumConcentricJointAngularVelocity(omegaMax-h); + tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,omegaMid,1.0,tmiL); + tq.setMaximumConcentricJointAngularVelocity(omegaMax+h); + tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,omegaMid,1.0,tmiR); + + double D2tau_DomegaMax2 = + (tmiR.DjointTorque_DmaximumAngularVelocity + - tmiL.DjointTorque_DmaximumAngularVelocity)/(2.0*h); + + err = fabs(D2tau_DomegaMax2 + - tmi.fittingInfo[15]); + CHECK(err < SQRTEPSILON*100); + + // 17: d2t/dmdp + double D2tau_DomegaMax_DtpLambda = + (tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable + -tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h); + err = fabs(D2tau_DomegaMax_DtpLambda + - tmi.fittingInfo[17]); + CHECK(err < SQRTEPSILON*100); + + // 20: d2t/dmdo + double D2tau_DomegaMax_DtpOffset = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + -tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + err = fabs(D2tau_DomegaMax_DtpOffset + - tmi.fittingInfo[20]); + CHECK(err < SQRTEPSILON*100); + +// 18: d2t/dp2 + tq.setActiveTorqueAngleCurveBlendingVariable(taLambda); + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda); + tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda); + tq.setPassiveCurveAngleOffset(0.0); + + tq.setActiveTorqueAngleCurveAngleScaling(1.0); + tq.setMaximumConcentricJointAngularVelocity(omegaMax); + + + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda - h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda + h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + D2tau_DtpLambda2 = + (tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable + - tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtpLambda2 + - tmi.fittingInfo[18]); + CHECK(err < SQRTEPSILON*100); + + // 21: d2t/dpdo + + D2tau_DtpLambdaDtpOffset = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + - tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtpLambdaDtpOffset + - tmi.fittingInfo[21]); + CHECK(err < SQRTEPSILON*100); + + + // 22: d2t/do2 + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda); + tq.setPassiveCurveAngleOffset(0.); + + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setPassiveCurveAngleOffset(-h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setPassiveCurveAngleOffset( h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + D2tau_DtpOffset2 = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + - tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtpOffset2 + - tmi.fittingInfo[22]); + CHECK(err < SQRTEPSILON*1000); + + + +#endif + +} + + +#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING + +TEST_CASE(__FILE__"_fittingEasyTest", ""){ + + double jointAngleOffset = 0; + double signOfJointAngle = 1; + double signOfJointTorque = 1; + double signOfJointVelocity = signOfJointTorque; + + std::string name("test"); + + SubjectInformation subjectInfo; + subjectInfo.gender = GenderSet::Male; + subjectInfo.ageGroup = AgeGroupSet::Young18To25; + subjectInfo.heightInMeters = 1.732; + subjectInfo.massInKg = 69.0; + + Millard2016TorqueMuscle tq = + Millard2016TorqueMuscle(DataSet::Gymnast, + subjectInfo, + Gymnast::HipExtension, + jointAngleOffset, + signOfJointAngle, + signOfJointTorque, + name); + +// Testing approach +// 1. Generate a data set consistent with a muscle that is more flexible, +// is stronger, and has modified curves from the default. +// 2. Return the muscle back to its default values of flexibility, maximum +// isometric torque and blending variables. +// 3. Run the fitting algorithm to see if the parameters from step 1 can be +// identified. + + + double tisoOriginal = tq.getMaximumActiveIsometricTorque(); + double tisoUpd = 1.2*tisoOriginal; + double omegaMaxOrig = tq.getMaximumConcentricJointAngularVelocity(); + double omegaMaxUpd = 2.5*omegaMaxOrig; + + double taAngleScalingOrig = tq.getActiveTorqueAngleCurveAngleScaling(); + double taAngleScalingUpd = taAngleScalingOrig*1.35; + + double taLambda = 0.0; + double tpLambda = 0.0; + double tvLambda = 0.0; + + + double angleToOneOrig = + tq.getJointAngleAtOneNormalizedPassiveIsometricTorque(); + + tq.setActiveTorqueAngleCurveBlendingVariable(taLambda); + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda); + tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda); + tq.setMaximumActiveIsometricTorque(tisoUpd); + tq.setMaximumConcentricJointAngularVelocity(omegaMaxUpd); + tq.setActiveTorqueAngleCurveAngleScaling(taAngleScalingUpd); + + unsigned int npts = 100; + RigidBodyDynamics::Math::VectorNd angle, angularVelocity, torque; + angle.resize(npts); + angularVelocity.resize(npts); + torque.resize(npts); + + //Generate the data set. + double activationUpperBound = 0.95; + double passiveTorqueAngleMultiplierUpperBound = 0.; + TorqueMuscleInfo tmi; + TorqueMuscleSummary tms; + + double angularRange = M_PI/3.0; + double angleTpOne = + tq.getJointAngleAtOneNormalizedPassiveIsometricTorque(); + double angularOffset = angleTpOne-angularRange; + double time = 0.; + double normTime = 0.; + double omega = omegaMaxUpd/angularRange; + for(unsigned int i = 0; i passiveTorqueAngleMultiplierUpperBound){ + passiveTorqueAngleMultiplierUpperBound = + tmi.fiberPassiveTorqueAngleMultiplier; + } + } + //Return the muscle back to its defaults. + tq.setActiveTorqueAngleCurveBlendingVariable(0.0); + tq.setPassiveTorqueAngleCurveBlendingVariable(0.0); + tq.setTorqueAngularVelocityCurveBlendingVariable(0.0); + tq.setMaximumActiveIsometricTorque(tisoOriginal); + tq.setMaximumConcentricJointAngularVelocity(omegaMaxOrig); + tq.setActiveTorqueAngleCurveAngleScaling(taAngleScalingOrig); + + //Run the fitting routine. + passiveTorqueAngleMultiplierUpperBound *= 0.75; + TorqueMuscleParameterFittingData tmFittingData; + + + TorqueMuscleFittingToolkit::fitTorqueMuscleParameters( + tq, angle,angularVelocity,torque, + activationUpperBound, + passiveTorqueAngleMultiplierUpperBound, + tmFittingData, false); + //Now to test the result, update the parameters of the + //model and run through all of the test vectors. The muscle + //should produce a torque that is >= the desired torque + //of the same sign. There should be one value where it + //,to numerical precision produces just the required torque + //and no more. + + CHECK(tmFittingData.fittingConverged == true); + + + tq.setFittedParameters(tmFittingData); + + double minActivation = 1.e20; + double maxActivation = -1.e20; + double maxPassiveTorqueAngleMultiplier = -1.0e20; + for(unsigned int i=0; i 0){ + tq.calcActivation(angle[i],angularVelocity[i],torque[i],tms); + + if(tms.activation < minActivation){ + minActivation = tms.activation; + } + if(tms.activation > maxActivation){ + maxActivation = tms.activation; + } + + if(tms.fiberPassiveTorqueAngleMultiplier + > maxPassiveTorqueAngleMultiplier){ + maxPassiveTorqueAngleMultiplier = tms.fiberPassiveTorqueAngleMultiplier; + } + } + } + double err = maxActivation-activationUpperBound; + CHECK(err <= 10.0*SQRTEPSILON); + + err = minActivation; + CHECK(err >= -10.0*SQRTEPSILON); + + err = (maxPassiveTorqueAngleMultiplier + - passiveTorqueAngleMultiplierUpperBound); + CHECK(err < SQRTEPSILON); + +} + +TEST_CASE(__FILE__"_fittingHardTest", "") +{ + + std::string name("hardTest"); + + SubjectInformation subjectInfo; + subjectInfo.gender = GenderSet::Male; + subjectInfo.ageGroup = AgeGroupSet::Young18To25; + subjectInfo.heightInMeters = 1.732; + subjectInfo.massInKg = 69.0; + + double angleOffset = 0.; + double angleSign = 1; + double torqueSign =-1; + + Millard2016TorqueMuscle kneeExt(DataSet::Gymnast, + subjectInfo, + Gymnast::KneeExtension, + angleOffset, + angleSign, + torqueSign, + name); + + RigidBodyDynamics::Math::VectorNd q, qDot, tau; + q.resize( TorqueMuscleFittingHardTestCaseRows); + qDot.resize(TorqueMuscleFittingHardTestCaseRows); + tau.resize( TorqueMuscleFittingHardTestCaseRows); + + double tauMax = 0; + double omegaMax = 0; + + for(unsigned int i=0; iomegaMax){ + omegaMax = fabs(qDot[i]); + } + if(fabs(tau[i])>tauMax){ + tauMax = fabs(tau[i]); + } + } + + + double activationUB = 0.9; + double tpUB = 0.75; + bool verbose = false; + + + TorqueMuscleParameterFittingData fittingData; + TorqueMuscleFittingToolkit::fitTorqueMuscleParameters(kneeExt,q,qDot,tau, + activationUB,tpUB, + fittingData,verbose); + CHECK(fittingData.fittingConverged == true); +} + +#endif + + +TEST_CASE(__FILE__"_exampleUsage", ""){ + + + bool printCurves = false; + bool printAllCurves = false; + + //int dataSet = DataSetAnderson2007; + + //int gender = 0; //male + //int age = 0; //young + + double jointAngleOffset = 0; + double signOfJointAngle = 1; + double signOfJointTorque = 1; + + std::string name("test"); + + SubjectInformation subjectInfo; + subjectInfo.gender = GenderSet::Male; + subjectInfo.ageGroup = AgeGroupSet::Young18To25; + subjectInfo.heightInMeters = 1.732; + subjectInfo.massInKg = 69.0; + + std::vector< Millard2016TorqueMuscle > muscleVector; + + bool exception = false; + + double angleTorqueSigns[][2] = {{-1, 1}, + {-1,-1}, + { 1,-1}, + { 1, 1}, + {-1, 1}, + {-1,-1}}; + + Millard2016TorqueMuscle tqMuscle; + std::stringstream tqName; + int tqIdx; + + for(int i=0; i < Anderson2007::LastJointTorque; ++i){ + + tqName.str(""); + tqName << DataSet.names[0] + < -#include -#include -#include -#include -#include -#include - -using namespace RigidBodyDynamics::Addons::Geometry; -using namespace RigidBodyDynamics::Addons::Muscle; -using namespace std; -//using namespace OpenSim; -//using namespace SimTK; - -/* -static double EPSILON = numeric_limits::epsilon(); - -static bool FLAG_PLOT_CURVES = false; -static string FILE_PATH = ""; -static double TOL_DX = 5e-3; -static double TOL_DX_BIG = 1e-2; -static double TOL_BIG = 1e-6; -static double TOL_SMALL = 1e-12; -*/ - - - -TEST(tendonCurve) -{ - //cout <<"**************************************************"< +#include +#include +#include +#include +#include + +#include "rbdl_tests.h" + +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace std; +//using namespace OpenSim; +//using namespace SimTK; + +/* +static double EPSILON = numeric_limits::epsilon(); + +static bool FLAG_PLOT_CURVES = false; +static string FILE_PATH = ""; +static double TOL_DX = 5e-3; +static double TOL_DX_BIG = 1e-2; +static double TOL_BIG = 1e-6; +static double TOL_SMALL = 1e-12; +*/ + + + +TEST_CASE(__FILE__"_tendonCurve", "") +{ + //cout <<"**************************************************"< -#include -#include -#include -#include -#include -#include - -using namespace RigidBodyDynamics::Addons::Geometry; -using namespace RigidBodyDynamics::Addons::Muscle; -using namespace std; - -/* -static double EPSILON = numeric_limits::epsilon(); - -static bool FLAG_PLOT_CURVES = false; -static string FILE_PATH = ""; -static double TOL_DX = 5e-3; -static double TOL_DX_BIG = 1e-2; -static double TOL_BIG = 1e-6; -static double TOL_SMALL = 1e-12; -*/ - -TEST(Anderson2007ActiveTorqueAngleCurve) -{ - double subjectWeight = 75.0*9.81; - double subjectHeight = 1.75; - double scale = subjectHeight*subjectWeight; - - //These parameters are taken from table 3 for hip extension for - //men between the ages of 18-25 - double c1 = 0.161; //normalized maximum hip joint torque - double c2 = 0.958; // pi/(theta_max - theta_min) - double c3 = 0.932; //theta_max_iso_torque - double c4 = 1.578; //omega_1: angular velocity at 75% tq_iso_max - double c5 = 3.190; //omega_2: angular velocity at 50% tq_iso_max - double c6 = 0.242; //E, where eccentric slope = (1+E)*concentricSlope - //Passive torque angle curve parameters - double b1 =-1.210; // torque_passive = b1*exp(k1*theta) - double k1 =-6.351; // +b2*exp(k2*theta) - double b2 = 0.476; - double k2 = 5.910; - - - - //cout < tauMax){ - double tmp = tauMin; - tauMin = tauMax; - tauMax = tmp; - tauMinAngle = angleMax; - } - - CHECK( abs(tauMin) < TOL_SMALL); - CHECK( abs(tauMax - 1.0) < TOL_SMALL); - CHECK( abs(andersonTpCurve.calcDerivative(tauMinAngle,1)) < TOL_SMALL); - - //cout << " passed " << endl; - - //cout << " Continuity and Smoothness Testing " << endl; - bool areCurveDerivativesGood = - areCurveDerivativesCloseToNumericDerivatives( - andersonTpCurve, - andersonTpCurveSample, - TOL_DX); - CHECK(areCurveDerivativesGood); - - bool curveIsContinuous = isCurveC2Continuous(andersonTpCurve, - andersonTpCurveSample, - TOL_BIG); - CHECK(curveIsContinuous); - - bool curveIsMonotonic = isCurveMontonic(andersonTpCurveSample); - CHECK(curveIsMonotonic); - //cout << " passed " << endl; - - - } - - SmoothSegmentedFunction andersonTpCurve = SmoothSegmentedFunction(); - TorqueMuscleFunctionFactory:: - createAnderson2007PassiveTorqueAngleCurve( - scale, - c1, - b1, - k1, - b2, - k2, - "test_passiveTorqueAngleCurve", - andersonTpCurve); - - if(FLAG_PLOT_CURVES){ - andersonTpCurve.printCurveToCSVFile( - FILE_PATH, - "anderson2007PassiveTorqueAngleCurve", - andersonTpCurve.getCurveDomain()[0]-0.1, - andersonTpCurve.getCurveDomain()[1]+0.1); - } - -} - -TEST(Anderson2007ActiveTorqueVelocityCurve) -{ - double subjectWeight = 75.0*9.81; - double subjectHeight = 1.75; - double scale = subjectHeight*subjectWeight; - - //These parameters are taken from table 3 for hip extension for - //men between the ages of 18-25 - double c1 = 0.161; //normalized maximum hip joint torque - double c2 = 0.958; // pi/(theta_max - theta_min) - double c3 = 0.932; //theta_max_iso_torque - double c4 = 1.578; //omega_1: angular velocity at 75% tq_iso_max - double c5 = 3.190; //omega_2: angular velocity at 50% tq_iso_max - double c6 = 0.242; //E, where eccentric slope = (1+E)*concentricSlope - //Passive torque angle curve parameters - double b1 =-1.210; // torque_passive = b1*exp(k1*theta) - double k1 =-6.351; // +b2*exp(k2*theta) - double b2 = 0.476; - double k2 = 5.910; - - //cout <= minEccentricMultiplier); - CHECK(maxTv <= maxEccentricMultiplier); - - CHECK(abs(andersonTvCurve.calcDerivative - (angularVelocityMax/angularVelocityMax,1)) wmaxC){ - CHECK( abs( tv.calcValue(w) ) < TOL_SMALL); - CHECK( abs( tv.calcDerivative(w,1) ) < TOL_SMALL); - CHECK( abs(tvX.calcDerivative(w,1) - - slopeAtConcentricOmegaMax ) < TOL_BIG); - }else if(w > 0 && w <= wmaxC){ - tvHill = (b*fiso-a*w)/(b+w); - errHill = abs(tv.calcValue(w)-tvHill); - //printf("%i. Err %f, ",i,errHill); - CHECK( errHill < 0.02); - errHill = abs(tvX.calcValue(w)-tvHill); - //printf(" Err %f\n",errHill); - CHECK( errHill < 0.02); - }else if(w < 0 & w > wmaxE){ - CHECK(tv.calcValue(w) > 1.0); - }else if(w < wmaxE){ - CHECK(abs( tv.calcValue(w) - tvAtEccentricOmegaMax ) < TOL_SMALL); - CHECK(abs( tv.calcDerivative(w,1) - 0.0 ) < TOL_SMALL); - //CHECK(abs( tvX.calcValue(w) - tvAtEccentricOmegaMax ) ); - CHECK(abs( tvX.calcDerivative(w,1) - - slopeAtEccentricOmegaMax ) < TOL_SMALL ); - } - } - - RigidBodyDynamics::Math::VectorNd curveDomain = tv.getCurveDomain(); - - double angularVelocityMin = curveDomain[0]; - double angularVelocityMax = curveDomain[1]; - - - RigidBodyDynamics::Math::MatrixNd tvCurveSample - = tv.calcSampledCurve( 6, - angularVelocityMin-0.1, - angularVelocityMax+0.1); - - bool areCurveDerivativesGood = - areCurveDerivativesCloseToNumericDerivatives( - tv, - tvCurveSample, - TOL_DX); - - CHECK(areCurveDerivativesGood); - - bool curveIsContinuous = isCurveC2Continuous( tv, - tvCurveSample, - TOL_BIG); - CHECK(curveIsContinuous); - - bool curveIsMonotonic = isCurveMontonic(tvCurveSample); - CHECK(curveIsMonotonic); - - if(FLAG_PLOT_CURVES){ - tv.printCurveToCSVFile( - FILE_PATH, - "millard2016TorqueVelocityCurve", - -1.1, - 1.1); - } - -} - -//============================================================================== -TEST(PassiveTorqueAngleCurve) -{ - SmoothSegmentedFunction tp = SmoothSegmentedFunction(); - SmoothSegmentedFunction tpX = SmoothSegmentedFunction(); - double angleAtZeroTorque0 = 0; - double angleAtOneNormTorque0 = -M_PI; - - double angleAtZeroTorque1 = 0; - double angleAtOneNormTorque1 = M_PI; - - double stiffnessAtOneNormTorque1 = - 5.6/(angleAtOneNormTorque1-angleAtZeroTorque1); - double stiffnessAtLowTorque1 = - 0.05*stiffnessAtOneNormTorque1; - double curviness1 = 0.75; - - std::string curveName0("tpTest0"); - std::string curveName1("tpTest1"); - - - TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( - angleAtZeroTorque0, - angleAtOneNormTorque0, - curveName0, - tp); - - TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( - angleAtZeroTorque1, - angleAtOneNormTorque1, - stiffnessAtLowTorque1, - stiffnessAtOneNormTorque1, - curviness1, - curveName1, - tpX); - - CHECK( abs(tp.calcValue(angleAtZeroTorque0)) < TOL_SMALL); - CHECK( abs(tp.calcValue(angleAtOneNormTorque0)-1.0) < TOL_SMALL); - - CHECK( abs(tpX.calcValue(angleAtZeroTorque1)) < TOL_SMALL); - CHECK( abs(tpX.calcValue(angleAtOneNormTorque1) - 1.0) < TOL_SMALL); - CHECK( abs(tpX.calcDerivative(angleAtZeroTorque1,1)) < TOL_SMALL); - CHECK( abs(tpX.calcDerivative(angleAtOneNormTorque1,1) - -stiffnessAtOneNormTorque1) < TOL_SMALL); - - RigidBodyDynamics::Math::VectorNd curveDomain0 = tp.getCurveDomain(); - RigidBodyDynamics::Math::VectorNd curveDomain1 = tpX.getCurveDomain(); - - RigidBodyDynamics::Math::MatrixNd tpSample0 - = tp.calcSampledCurve( 6, - curveDomain0[0]-0.1, - curveDomain0[1]+0.1); - - RigidBodyDynamics::Math::MatrixNd tpSample1 - = tpX.calcSampledCurve( 6, - curveDomain1[0]-0.1, - curveDomain1[1]+0.1); - - bool areCurveDerivativesGood = - areCurveDerivativesCloseToNumericDerivatives( - tp, - tpSample0, - TOL_DX); - - CHECK(areCurveDerivativesGood); - - areCurveDerivativesGood = - areCurveDerivativesCloseToNumericDerivatives( - tpX, - tpSample1, - TOL_DX); - - CHECK(areCurveDerivativesGood); - bool curveIsContinuous = isCurveC2Continuous( tp, - tpSample0, - TOL_BIG); - CHECK(curveIsContinuous); - curveIsContinuous = isCurveC2Continuous(tpX, - tpSample1, - TOL_BIG); - CHECK(curveIsContinuous); - - bool curveIsMonotonic = isCurveMontonic(tpSample0); - CHECK(curveIsMonotonic); - - curveIsMonotonic = isCurveMontonic(tpSample1); - CHECK(curveIsMonotonic); - - - if(FLAG_PLOT_CURVES){ - tp.printCurveToCSVFile( - FILE_PATH, - "millard2016PassiveTorqueAngleCurve", - curveDomain0[0]-0.1, - curveDomain0[1]+0.1); - } -} - -//============================================================================== -TEST(ActiveTorqueAngleCurve) -{ - SmoothSegmentedFunction ta = SmoothSegmentedFunction(); - SmoothSegmentedFunction taX = SmoothSegmentedFunction(); - double angleAtOneNormTorque = 1.5; - double angularStandardDeviation = 1.0; - double angularStandardDeviationSq = - angularStandardDeviation*angularStandardDeviation; - - double minSlopeAtShoulders = 0.2; - double minValueAtShoulders = 0.1; - double xTrans = sqrt(-log(minValueAtShoulders)*2*angularStandardDeviationSq); - double delta = abs(xTrans+angleAtOneNormTorque); - double curviness = 0.75; - - std::string curveName0("tpTest0"); - std::string curveName1("tpTest1"); - - - TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( - angleAtOneNormTorque, - angularStandardDeviation, - curveName0, - ta); - - TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( - angleAtOneNormTorque, - angularStandardDeviation, - minSlopeAtShoulders, - minValueAtShoulders, - curviness, - curveName1, - taX); - - CHECK(abs(ta.calcValue(angleAtOneNormTorque)-1.0) < TOL_SMALL); - - CHECK(abs(ta.calcValue(angleAtOneNormTorque - +10.0*angularStandardDeviation)) < TOL_SMALL); - CHECK(abs(ta.calcValue(angleAtOneNormTorque - -10.0*angularStandardDeviation)) < TOL_SMALL); - - CHECK(abs(taX.calcValue(angleAtOneNormTorque)-1.0) < TOL_SMALL); - - double err = abs(taX.calcDerivative(angleAtOneNormTorque + delta,1) - + minSlopeAtShoulders); - CHECK(err < TOL_SMALL); - err = abs(taX.calcDerivative(angleAtOneNormTorque - delta,1) - - minSlopeAtShoulders); - CHECK(err < TOL_SMALL); - - - RigidBodyDynamics::Math::VectorNd curveDomain0 = ta.getCurveDomain(); - RigidBodyDynamics::Math::VectorNd curveDomain1 = taX.getCurveDomain(); - - RigidBodyDynamics::Math::MatrixNd taSample0 - = ta.calcSampledCurve( 6, - curveDomain0[0]-0.1, - curveDomain0[1]+0.1); - - RigidBodyDynamics::Math::MatrixNd taSample1 - = taX.calcSampledCurve( 6, - curveDomain1[0]-0.1, - curveDomain1[1]+0.1); - - bool areCurveDerivativesGood = - areCurveDerivativesCloseToNumericDerivatives( - ta, - taSample0, - TOL_DX); - - - CHECK(areCurveDerivativesGood); - - areCurveDerivativesGood = - areCurveDerivativesCloseToNumericDerivatives( - taX, - taSample1, - TOL_DX); - - CHECK(areCurveDerivativesGood); - bool curveIsContinuous = isCurveC2Continuous( ta, - taSample0, - TOL_BIG); - CHECK(curveIsContinuous); - curveIsContinuous = isCurveC2Continuous(taX, - taSample1, - TOL_BIG); - CHECK(curveIsContinuous); - - if(FLAG_PLOT_CURVES){ - ta.printCurveToCSVFile( - FILE_PATH, - "millard2016ActiveTorqueAngleCurve", - curveDomain0[0]-0.1, - curveDomain0[1]+0.1); - } - -} - -//============================================================================== -TEST(TendonTorqueAngleCurve) -{ - SmoothSegmentedFunction tt = SmoothSegmentedFunction(); - SmoothSegmentedFunction ttX = SmoothSegmentedFunction(); - - double angularStretchAtOneNormTorque = M_PI/2.0; - double stiffnessAtOneNormTorque = 2.5/angularStretchAtOneNormTorque; - double normTorqueAtToeEnd = 2.0/3.0; - double curviness = 0.5; - - std::string curveName0("ttTest0"); - std::string curveName1("ttTest1"); - - - TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( - angularStretchAtOneNormTorque, - curveName0, - tt); - - TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( - angularStretchAtOneNormTorque, - stiffnessAtOneNormTorque, - normTorqueAtToeEnd, - curviness, - curveName1, - ttX); - - CHECK(abs(tt.calcValue(0)) < TOL_SMALL); - CHECK(abs(ttX.calcValue(0)) < TOL_SMALL); - - CHECK(abs(tt.calcValue(angularStretchAtOneNormTorque)-1.0) < TOL_SMALL); - CHECK(abs(ttX.calcValue(angularStretchAtOneNormTorque)-1.0) < TOL_SMALL); - - CHECK(abs(ttX.calcDerivative(angularStretchAtOneNormTorque,1) - - stiffnessAtOneNormTorque) < TOL_SMALL); - - RigidBodyDynamics::Math::VectorNd curveDomain0 = tt.getCurveDomain(); - RigidBodyDynamics::Math::VectorNd curveDomain1 = ttX.getCurveDomain(); - - RigidBodyDynamics::Math::MatrixNd ttSample0 - = tt.calcSampledCurve( 6, - curveDomain0[0]-0.1, - curveDomain0[1]+0.1); - - RigidBodyDynamics::Math::MatrixNd ttSample1 - = ttX.calcSampledCurve( 6, - curveDomain1[0]-0.1, - curveDomain1[1]+0.1); - - bool areCurveDerivativesGood = - areCurveDerivativesCloseToNumericDerivatives( - tt, - ttSample0, - TOL_DX); - - CHECK(areCurveDerivativesGood); - - areCurveDerivativesGood = - areCurveDerivativesCloseToNumericDerivatives( - ttX, - ttSample1, - TOL_DX); - - CHECK(areCurveDerivativesGood); - bool curveIsContinuous = isCurveC2Continuous( tt, - ttSample0, - TOL_BIG); - CHECK(curveIsContinuous); - curveIsContinuous = isCurveC2Continuous(ttX, - ttSample1, - TOL_BIG); - CHECK(curveIsContinuous); - - bool curveIsMonotonic = isCurveMontonic(ttSample0); - CHECK(curveIsMonotonic); - - curveIsMonotonic = isCurveMontonic(ttSample1); - CHECK(curveIsMonotonic); - - - if(FLAG_PLOT_CURVES){ - tt.printCurveToCSVFile( - FILE_PATH, - "millard2016ActiveTorqueAngleCurve", - curveDomain0[0]-0.1, - angularStretchAtOneNormTorque); - } - -} - -//============================================================================== -TEST(DampingBlendingCurve) -{ - SmoothSegmentedFunction negOne = SmoothSegmentedFunction(); - SmoothSegmentedFunction posOne = SmoothSegmentedFunction(); - - std::string curveName0("neg1"); - std::string curveName1("pos1"); - - - TorqueMuscleFunctionFactory::createDampingBlendingCurve( - -1.0,curveName0,negOne); - - TorqueMuscleFunctionFactory::createDampingBlendingCurve( - 1.0,curveName0,posOne); - - - CHECK(abs(negOne.calcValue(0)) < TOL_SMALL); - CHECK(abs(posOne.calcValue(0)) < TOL_SMALL); - - CHECK(abs(negOne.calcValue(-1.0)-1.0) < TOL_SMALL); - CHECK(abs(posOne.calcValue(1.0)-1.0) < TOL_SMALL); - - CHECK(abs(negOne.calcDerivative( 0,1)) < TOL_SMALL); - CHECK(abs(negOne.calcDerivative(-1,1)) < TOL_SMALL); - - CHECK(abs(posOne.calcDerivative( 0,1)) < TOL_SMALL); - CHECK(abs(posOne.calcDerivative( 1,1)) < TOL_SMALL); - - RigidBodyDynamics::Math::VectorNd curveDomainNegOne = negOne.getCurveDomain(); - RigidBodyDynamics::Math::VectorNd curveDomainPosOne = posOne.getCurveDomain(); - - RigidBodyDynamics::Math::MatrixNd sampleNegOne - = negOne.calcSampledCurve( 6, - curveDomainNegOne[0]-0.1, - curveDomainNegOne[1]+0.1); - - RigidBodyDynamics::Math::MatrixNd samplePosOne - = posOne.calcSampledCurve( 6, - curveDomainPosOne[0]-0.1, - curveDomainPosOne[1]+0.1); - - bool areCurveDerivativesGood = - areCurveDerivativesCloseToNumericDerivatives( - negOne, - sampleNegOne, - TOL_DX); - - CHECK(areCurveDerivativesGood); - - areCurveDerivativesGood = - areCurveDerivativesCloseToNumericDerivatives( - posOne, - samplePosOne, - TOL_DX); - - CHECK(areCurveDerivativesGood); - bool curveIsContinuous = isCurveC2Continuous( negOne, - sampleNegOne, - TOL_BIG); - CHECK(curveIsContinuous); - curveIsContinuous = isCurveC2Continuous(posOne, - samplePosOne, - TOL_BIG); - CHECK(curveIsContinuous); - - bool curveIsMonotonic = isCurveMontonic(sampleNegOne); - CHECK(curveIsMonotonic); - - curveIsMonotonic = isCurveMontonic(samplePosOne); - CHECK(curveIsMonotonic); - - - if(FLAG_PLOT_CURVES){ - negOne.printCurveToCSVFile( - FILE_PATH, - "millard2016DampingBlendingCurve", - curveDomainNegOne[0]-0.1, - curveDomainNegOne[1]+0.1); - } - -} +/* -------------------------------------------------------------------------- * + * OpenSim: testSmoothSegmentedFunctionFactory.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + This also includes additional curves (the Anderson2007 curves) + which are not presently in OpenSim. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ +/* +Below is a basic bench mark simulation for the SmoothSegmentedFunctionFactory +class, a class that enables the easy generation of C2 continuous curves +that define the various characteristic curves required in a muscle model + */ + +// Author: Matthew Millard + +//============================================================================== +// INCLUDES +//============================================================================== + +#include "../TorqueMuscleFunctionFactory.h" +#include "../../geometry/geometry.h" +#include "../../geometry/tests/numericalTestFunctions.h" + +#include +#include +#include +#include +#include +#include + +#include "rbdl_tests.h" + +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace std; + +/* +static double EPSILON = numeric_limits::epsilon(); + +static bool FLAG_PLOT_CURVES = false; +static string FILE_PATH = ""; +static double TOL_DX = 5e-3; +static double TOL_DX_BIG = 1e-2; +static double TOL_BIG = 1e-6; +static double TOL_SMALL = 1e-12; +*/ + +TEST_CASE(__FILE__"_Anderson2007ActiveTorqueAngleCurve", "") +{ + double subjectWeight = 75.0*9.81; + double subjectHeight = 1.75; + double scale = subjectHeight*subjectWeight; + + //These parameters are taken from table 3 for hip extension for + //men between the ages of 18-25 + double c1 = 0.161; //normalized maximum hip joint torque + double c2 = 0.958; // pi/(theta_max - theta_min) + double c3 = 0.932; //theta_max_iso_torque + double c4 = 1.578; //omega_1: angular velocity at 75% tq_iso_max + double c5 = 3.190; //omega_2: angular velocity at 50% tq_iso_max + double c6 = 0.242; //E, where eccentric slope = (1+E)*concentricSlope + //Passive torque angle curve parameters + double b1 =-1.210; // torque_passive = b1*exp(k1*theta) + double k1 =-6.351; // +b2*exp(k2*theta) + double b2 = 0.476; + double k2 = 5.910; + + + + //cout < tauMax){ + double tmp = tauMin; + tauMin = tauMax; + tauMax = tmp; + tauMinAngle = angleMax; + } + + CHECK( abs(tauMin) < TOL_SMALL); + CHECK( abs(tauMax - 1.0) < TOL_SMALL); + CHECK( abs(andersonTpCurve.calcDerivative(tauMinAngle,1)) < TOL_SMALL); + + //cout << " passed " << endl; + + //cout << " Continuity and Smoothness Testing " << endl; + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + andersonTpCurve, + andersonTpCurveSample, + TOL_DX); + CHECK(areCurveDerivativesGood); + + bool curveIsContinuous = isCurveC2Continuous(andersonTpCurve, + andersonTpCurveSample, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(andersonTpCurveSample); + CHECK(curveIsMonotonic); + //cout << " passed " << endl; + + + } + + SmoothSegmentedFunction andersonTpCurve = SmoothSegmentedFunction(); + TorqueMuscleFunctionFactory:: + createAnderson2007PassiveTorqueAngleCurve( + scale, + c1, + b1, + k1, + b2, + k2, + "test_passiveTorqueAngleCurve", + andersonTpCurve); + + if(FLAG_PLOT_CURVES){ + andersonTpCurve.printCurveToCSVFile( + FILE_PATH, + "anderson2007PassiveTorqueAngleCurve", + andersonTpCurve.getCurveDomain()[0]-0.1, + andersonTpCurve.getCurveDomain()[1]+0.1); + } + +} + +TEST_CASE(__FILE__"_Anderson2007ActiveTorqueVelocityCurve", "") +{ + double subjectWeight = 75.0*9.81; + double subjectHeight = 1.75; + double scale = subjectHeight*subjectWeight; + + //These parameters are taken from table 3 for hip extension for + //men between the ages of 18-25 + double c1 = 0.161; //normalized maximum hip joint torque + double c2 = 0.958; // pi/(theta_max - theta_min) + double c3 = 0.932; //theta_max_iso_torque + double c4 = 1.578; //omega_1: angular velocity at 75% tq_iso_max + double c5 = 3.190; //omega_2: angular velocity at 50% tq_iso_max + double c6 = 0.242; //E, where eccentric slope = (1+E)*concentricSlope + //Passive torque angle curve parameters + double b1 =-1.210; // torque_passive = b1*exp(k1*theta) + double k1 =-6.351; // +b2*exp(k2*theta) + double b2 = 0.476; + double k2 = 5.910; + + //cout <= minEccentricMultiplier); + CHECK(maxTv <= maxEccentricMultiplier); + + CHECK(abs(andersonTvCurve.calcDerivative + (angularVelocityMax/angularVelocityMax,1)) wmaxC){ + CHECK( abs( tv.calcValue(w) ) < TOL_SMALL); + CHECK( abs( tv.calcDerivative(w,1) ) < TOL_SMALL); + CHECK( abs(tvX.calcDerivative(w,1) + - slopeAtConcentricOmegaMax ) < TOL_BIG); + }else if(w > 0 && w <= wmaxC){ + tvHill = (b*fiso-a*w)/(b+w); + errHill = abs(tv.calcValue(w)-tvHill); + //printf("%i. Err %f, ",i,errHill); + CHECK( errHill < 0.02); + errHill = abs(tvX.calcValue(w)-tvHill); + //printf(" Err %f\n",errHill); + CHECK( errHill < 0.02); + }else if(w < 0 & w > wmaxE){ + CHECK(tv.calcValue(w) > 1.0); + }else if(w < wmaxE){ + CHECK(abs( tv.calcValue(w) - tvAtEccentricOmegaMax ) < TOL_SMALL); + CHECK(abs( tv.calcDerivative(w,1) - 0.0 ) < TOL_SMALL); + //CHECK(abs( tvX.calcValue(w) - tvAtEccentricOmegaMax ) ); + CHECK(abs( tvX.calcDerivative(w,1) + - slopeAtEccentricOmegaMax ) < TOL_SMALL ); + } + } + + RigidBodyDynamics::Math::VectorNd curveDomain = tv.getCurveDomain(); + + double angularVelocityMin = curveDomain[0]; + double angularVelocityMax = curveDomain[1]; + + + RigidBodyDynamics::Math::MatrixNd tvCurveSample + = tv.calcSampledCurve( 6, + angularVelocityMin-0.1, + angularVelocityMax+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + tv, + tvCurveSample, + TOL_DX); + + CHECK(areCurveDerivativesGood); + + bool curveIsContinuous = isCurveC2Continuous( tv, + tvCurveSample, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(tvCurveSample); + CHECK(curveIsMonotonic); + + if(FLAG_PLOT_CURVES){ + tv.printCurveToCSVFile( + FILE_PATH, + "millard2016TorqueVelocityCurve", + -1.1, + 1.1); + } + +} + +//============================================================================== +TEST_CASE(__FILE__"_PassiveTorqueAngleCurve", "") +{ + SmoothSegmentedFunction tp = SmoothSegmentedFunction(); + SmoothSegmentedFunction tpX = SmoothSegmentedFunction(); + double angleAtZeroTorque0 = 0; + double angleAtOneNormTorque0 = -M_PI; + + double angleAtZeroTorque1 = 0; + double angleAtOneNormTorque1 = M_PI; + + double stiffnessAtOneNormTorque1 = + 5.6/(angleAtOneNormTorque1-angleAtZeroTorque1); + double stiffnessAtLowTorque1 = + 0.05*stiffnessAtOneNormTorque1; + double curviness1 = 0.75; + + std::string curveName0("tpTest0"); + std::string curveName1("tpTest1"); + + + TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( + angleAtZeroTorque0, + angleAtOneNormTorque0, + curveName0, + tp); + + TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( + angleAtZeroTorque1, + angleAtOneNormTorque1, + stiffnessAtLowTorque1, + stiffnessAtOneNormTorque1, + curviness1, + curveName1, + tpX); + + CHECK( abs(tp.calcValue(angleAtZeroTorque0)) < TOL_SMALL); + CHECK( abs(tp.calcValue(angleAtOneNormTorque0)-1.0) < TOL_SMALL); + + CHECK( abs(tpX.calcValue(angleAtZeroTorque1)) < TOL_SMALL); + CHECK( abs(tpX.calcValue(angleAtOneNormTorque1) - 1.0) < TOL_SMALL); + CHECK( abs(tpX.calcDerivative(angleAtZeroTorque1,1)) < TOL_SMALL); + CHECK( abs(tpX.calcDerivative(angleAtOneNormTorque1,1) + -stiffnessAtOneNormTorque1) < TOL_SMALL); + + RigidBodyDynamics::Math::VectorNd curveDomain0 = tp.getCurveDomain(); + RigidBodyDynamics::Math::VectorNd curveDomain1 = tpX.getCurveDomain(); + + RigidBodyDynamics::Math::MatrixNd tpSample0 + = tp.calcSampledCurve( 6, + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + + RigidBodyDynamics::Math::MatrixNd tpSample1 + = tpX.calcSampledCurve( 6, + curveDomain1[0]-0.1, + curveDomain1[1]+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + tp, + tpSample0, + TOL_DX); + + CHECK(areCurveDerivativesGood); + + areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + tpX, + tpSample1, + TOL_DX); + + CHECK(areCurveDerivativesGood); + bool curveIsContinuous = isCurveC2Continuous( tp, + tpSample0, + TOL_BIG); + CHECK(curveIsContinuous); + curveIsContinuous = isCurveC2Continuous(tpX, + tpSample1, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(tpSample0); + CHECK(curveIsMonotonic); + + curveIsMonotonic = isCurveMontonic(tpSample1); + CHECK(curveIsMonotonic); + + + if(FLAG_PLOT_CURVES){ + tp.printCurveToCSVFile( + FILE_PATH, + "millard2016PassiveTorqueAngleCurve", + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + } +} + +//============================================================================== +TEST_CASE(__FILE__"_ActiveTorqueAngleCurve", "") +{ + SmoothSegmentedFunction ta = SmoothSegmentedFunction(); + SmoothSegmentedFunction taX = SmoothSegmentedFunction(); + double angleAtOneNormTorque = 1.5; + double angularStandardDeviation = 1.0; + double angularStandardDeviationSq = + angularStandardDeviation*angularStandardDeviation; + + double minSlopeAtShoulders = 0.2; + double minValueAtShoulders = 0.1; + double xTrans = sqrt(-log(minValueAtShoulders)*2*angularStandardDeviationSq); + double delta = abs(xTrans+angleAtOneNormTorque); + double curviness = 0.75; + + std::string curveName0("tpTest0"); + std::string curveName1("tpTest1"); + + + TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( + angleAtOneNormTorque, + angularStandardDeviation, + curveName0, + ta); + + TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( + angleAtOneNormTorque, + angularStandardDeviation, + minSlopeAtShoulders, + minValueAtShoulders, + curviness, + curveName1, + taX); + + CHECK(abs(ta.calcValue(angleAtOneNormTorque)-1.0) < TOL_SMALL); + + CHECK(abs(ta.calcValue(angleAtOneNormTorque + +10.0*angularStandardDeviation)) < TOL_SMALL); + CHECK(abs(ta.calcValue(angleAtOneNormTorque + -10.0*angularStandardDeviation)) < TOL_SMALL); + + CHECK(abs(taX.calcValue(angleAtOneNormTorque)-1.0) < TOL_SMALL); + + double err = abs(taX.calcDerivative(angleAtOneNormTorque + delta,1) + + minSlopeAtShoulders); + CHECK(err < TOL_SMALL); + err = abs(taX.calcDerivative(angleAtOneNormTorque - delta,1) + - minSlopeAtShoulders); + CHECK(err < TOL_SMALL); + + + RigidBodyDynamics::Math::VectorNd curveDomain0 = ta.getCurveDomain(); + RigidBodyDynamics::Math::VectorNd curveDomain1 = taX.getCurveDomain(); + + RigidBodyDynamics::Math::MatrixNd taSample0 + = ta.calcSampledCurve( 6, + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + + RigidBodyDynamics::Math::MatrixNd taSample1 + = taX.calcSampledCurve( 6, + curveDomain1[0]-0.1, + curveDomain1[1]+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + ta, + taSample0, + TOL_DX); + + + CHECK(areCurveDerivativesGood); + + areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + taX, + taSample1, + TOL_DX); + + CHECK(areCurveDerivativesGood); + bool curveIsContinuous = isCurveC2Continuous( ta, + taSample0, + TOL_BIG); + CHECK(curveIsContinuous); + curveIsContinuous = isCurveC2Continuous(taX, + taSample1, + TOL_BIG); + CHECK(curveIsContinuous); + + if(FLAG_PLOT_CURVES){ + ta.printCurveToCSVFile( + FILE_PATH, + "millard2016ActiveTorqueAngleCurve", + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + } + +} + +//============================================================================== +TEST_CASE(__FILE__"_TendonTorqueAngleCurve", "") +{ + SmoothSegmentedFunction tt = SmoothSegmentedFunction(); + SmoothSegmentedFunction ttX = SmoothSegmentedFunction(); + + double angularStretchAtOneNormTorque = M_PI/2.0; + double stiffnessAtOneNormTorque = 2.5/angularStretchAtOneNormTorque; + double normTorqueAtToeEnd = 2.0/3.0; + double curviness = 0.5; + + std::string curveName0("ttTest0"); + std::string curveName1("ttTest1"); + + + TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( + angularStretchAtOneNormTorque, + curveName0, + tt); + + TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( + angularStretchAtOneNormTorque, + stiffnessAtOneNormTorque, + normTorqueAtToeEnd, + curviness, + curveName1, + ttX); + + CHECK(abs(tt.calcValue(0)) < TOL_SMALL); + CHECK(abs(ttX.calcValue(0)) < TOL_SMALL); + + CHECK(abs(tt.calcValue(angularStretchAtOneNormTorque)-1.0) < TOL_SMALL); + CHECK(abs(ttX.calcValue(angularStretchAtOneNormTorque)-1.0) < TOL_SMALL); + + CHECK(abs(ttX.calcDerivative(angularStretchAtOneNormTorque,1) + - stiffnessAtOneNormTorque) < TOL_SMALL); + + RigidBodyDynamics::Math::VectorNd curveDomain0 = tt.getCurveDomain(); + RigidBodyDynamics::Math::VectorNd curveDomain1 = ttX.getCurveDomain(); + + RigidBodyDynamics::Math::MatrixNd ttSample0 + = tt.calcSampledCurve( 6, + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + + RigidBodyDynamics::Math::MatrixNd ttSample1 + = ttX.calcSampledCurve( 6, + curveDomain1[0]-0.1, + curveDomain1[1]+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + tt, + ttSample0, + TOL_DX); + + CHECK(areCurveDerivativesGood); + + areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + ttX, + ttSample1, + TOL_DX); + + CHECK(areCurveDerivativesGood); + bool curveIsContinuous = isCurveC2Continuous( tt, + ttSample0, + TOL_BIG); + CHECK(curveIsContinuous); + curveIsContinuous = isCurveC2Continuous(ttX, + ttSample1, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(ttSample0); + CHECK(curveIsMonotonic); + + curveIsMonotonic = isCurveMontonic(ttSample1); + CHECK(curveIsMonotonic); + + + if(FLAG_PLOT_CURVES){ + tt.printCurveToCSVFile( + FILE_PATH, + "millard2016ActiveTorqueAngleCurve", + curveDomain0[0]-0.1, + angularStretchAtOneNormTorque); + } + +} + +//============================================================================== +TEST_CASE(__FILE__"_DampingBlendingCurve", "") +{ + SmoothSegmentedFunction negOne = SmoothSegmentedFunction(); + SmoothSegmentedFunction posOne = SmoothSegmentedFunction(); + + std::string curveName0("neg1"); + std::string curveName1("pos1"); + + + TorqueMuscleFunctionFactory::createDampingBlendingCurve( + -1.0,curveName0,negOne); + + TorqueMuscleFunctionFactory::createDampingBlendingCurve( + 1.0,curveName0,posOne); + + + CHECK(abs(negOne.calcValue(0)) < TOL_SMALL); + CHECK(abs(posOne.calcValue(0)) < TOL_SMALL); + + CHECK(abs(negOne.calcValue(-1.0)-1.0) < TOL_SMALL); + CHECK(abs(posOne.calcValue(1.0)-1.0) < TOL_SMALL); + + CHECK(abs(negOne.calcDerivative( 0,1)) < TOL_SMALL); + CHECK(abs(negOne.calcDerivative(-1,1)) < TOL_SMALL); + + CHECK(abs(posOne.calcDerivative( 0,1)) < TOL_SMALL); + CHECK(abs(posOne.calcDerivative( 1,1)) < TOL_SMALL); + + RigidBodyDynamics::Math::VectorNd curveDomainNegOne = negOne.getCurveDomain(); + RigidBodyDynamics::Math::VectorNd curveDomainPosOne = posOne.getCurveDomain(); + + RigidBodyDynamics::Math::MatrixNd sampleNegOne + = negOne.calcSampledCurve( 6, + curveDomainNegOne[0]-0.1, + curveDomainNegOne[1]+0.1); + + RigidBodyDynamics::Math::MatrixNd samplePosOne + = posOne.calcSampledCurve( 6, + curveDomainPosOne[0]-0.1, + curveDomainPosOne[1]+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + negOne, + sampleNegOne, + TOL_DX); + + CHECK(areCurveDerivativesGood); + + areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + posOne, + samplePosOne, + TOL_DX); + + CHECK(areCurveDerivativesGood); + bool curveIsContinuous = isCurveC2Continuous( negOne, + sampleNegOne, + TOL_BIG); + CHECK(curveIsContinuous); + curveIsContinuous = isCurveC2Continuous(posOne, + samplePosOne, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(sampleNegOne); + CHECK(curveIsMonotonic); + + curveIsMonotonic = isCurveMontonic(samplePosOne); + CHECK(curveIsMonotonic); + + + if(FLAG_PLOT_CURVES){ + negOne.printCurveToCSVFile( + FILE_PATH, + "millard2016DampingBlendingCurve", + curveDomainNegOne[0]-0.1, + curveDomainNegOne[1]+0.1); + } + +} diff --git a/addons/urdfreader/CMakeLists.txt b/addons/urdfreader/CMakeLists.txt index 9ec3b49..9feeb61 100644 --- a/addons/urdfreader/CMakeLists.txt +++ b/addons/urdfreader/CMakeLists.txt @@ -8,7 +8,7 @@ SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES INCLUDE_DIRECTORIES ( ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl - ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/ + ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/urdfparser/include ) SET ( URDFREADER_SOURCES @@ -24,39 +24,17 @@ ELSE () ENDIF () IF (RBDL_USE_ROS_URDF_LIBRARY) - # find_package(Boost REQUIRED COMPONENTS system) SET (URDFREADER_DEPENDENCIES - rbdl ${urdf_LIBRARIES} - # ${Boost_SYSTEM_LIBRARY} ) ELSE() + ADD_SUBDIRECTORY(thirdparty) + SET(CMAKE_CXX_STANDARD 17) SET (URDFREADER_SOURCES ${URDFREADER_SOURCES} - thirdparty/urdf/urdfdom/urdf_parser/src/check_urdf.cpp - thirdparty/urdf/urdfdom/urdf_parser/src/pose.cpp - thirdparty/urdf/urdfdom/urdf_parser/src/model.cpp - thirdparty/urdf/urdfdom/urdf_parser/src/link.cpp - thirdparty/urdf/urdfdom/urdf_parser/src/joint.cpp - thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h - thirdparty/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h - thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h - thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h - thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h - thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h - thirdparty/tinyxml/tinystr.cpp - thirdparty/tinyxml/tinyxml.cpp - thirdparty/tinyxml/tinyxmlerror.cpp - thirdparty/tinyxml/tinyxmlparser.cpp - thirdparty/urdf/boost_replacement/lexical_cast.h - thirdparty/urdf/boost_replacement/shared_ptr.h - thirdparty/urdf/boost_replacement/printf_console.cpp - thirdparty/urdf/boost_replacement/printf_console.h - thirdparty/urdf/boost_replacement/string_split.cpp - thirdparty/urdf/boost_replacement/string_split.h ) SET (URDFREADER_DEPENDENCIES - rbdl + urdfparser ) ENDIF() @@ -72,6 +50,7 @@ IF (RBDL_BUILD_STATIC) TARGET_LINK_LIBRARIES (rbdl_urdfreader-static rbdl-static + ${URDFREADER_DEPENDENCIES} ) TARGET_LINK_LIBRARIES (rbdl_urdfreader_util @@ -81,7 +60,7 @@ IF (RBDL_BUILD_STATIC) INSTALL (TARGETS rbdl_urdfreader-static rbdl_urdfreader_util RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} ) ELSE (RBDL_BUILD_STATIC) ADD_LIBRARY ( rbdl_urdfreader SHARED ${URDFREADER_SOURCES} ) @@ -91,6 +70,7 @@ ELSE (RBDL_BUILD_STATIC) ) TARGET_LINK_LIBRARIES (rbdl_urdfreader + rbdl ${URDFREADER_DEPENDENCIES} ) @@ -104,6 +84,16 @@ ELSE (RBDL_BUILD_STATIC) ) ENDIF (RBDL_BUILD_STATIC) +IF (RUN_AUTOMATIC_TESTS) + IF (URDF_BUILD_TEST) + ADD_CUSTOM_COMMAND (TARGET rbdl_urdfreader + POST_BUILD + COMMAND ./thirdparty/urdfparser/test_library + COMMENT "Running automated tests..." + ) + ENDIF(URDF_BUILD_TEST) +ENDIF(RUN_AUTOMATIC_TESTS) + FILE ( GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" ) diff --git a/addons/urdfreader/README.md b/addons/urdfreader/README.md index eb8f4d8..91f0853 100644 --- a/addons/urdfreader/README.md +++ b/addons/urdfreader/README.md @@ -1,50 +1,17 @@ urdfreader - load models from (URDF Unified Robot Description Format) files -Copyright (c) 2012-2018 Martin Felis +Copyright (c) 2020-2021 Felix Richter -Requirements -============ +RBDL - URDF Reader Addon +======================== +*This addon has been heavily updated from the original rbdl version!* -This addon depends on urdfdom to load and access the model data in the URDF -files. +This addons is used to import URDF models into RBDL. It uses the [URDF_Parser Library](https://github.com/orb-hd/URDF_Parser) to parse the models. -See https://github.com/ros/urdfdom for more details on how to -install urdfdom. +It is also possible to use the urdfdom distributed with ROS. To do this activate the ```RBDL_USE_ROS_URDF_LIBRARY``` cmake option during the compilation process! -Warning -======= - -This code is not properly tested as I do not have a proper urdf robot -model. If anyone has one and also some reference values that should come -out for the dynamics computations, please let me know. - -Licensing +Changelog ========= +04.12.20 - Replaced urdfdom with URDF_Parser library that has support for error handling and does not lead to crashes when loading models +18.11.19 - Add support for urdfdom distributed with ROS +before - Version shipped by original rbdl -This code is published under the zlib license, however some parts of the -CMake scripts are taken from other projects and are licensed under -different terms. - -Full license text: - -------- -urdfreader - load models from URDF (Unified Robot Description Format) files -Copyright (c) 2012-2018 Martin Felis - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any damages -arising from the use of this software. - -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it -freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - - 3. This notice may not be removed or altered from any source - distribution. diff --git a/addons/urdfreader/rbdl_urdfreader_util.cc b/addons/urdfreader/rbdl_urdfreader_util.cc index 3b3098f..e73d5e7 100644 --- a/addons/urdfreader/rbdl_urdfreader_util.cc +++ b/addons/urdfreader/rbdl_urdfreader_util.cc @@ -18,12 +18,18 @@ using namespace RigidBodyDynamics::Math; void usage (const char* argv_0) { cerr << "Usage: " << argv_0 << "[-v] [-m] [-d] " << endl; - cerr << " -v | --verbose enable additional output" << endl; - cerr << " -d | --dof-overview print an overview of the degress of freedom" << endl; - cerr << " -f | --floatbase set the first mobile body as floating base" << endl; - cerr << " -m | --model-hierarchy print the hierarchy of the model" << endl; - cerr << " -o | --body-origins print the origins of all bodies that have names" << endl; - cerr << " -c | --center_of_mass print center of mass for bodies and full model" << endl; + cerr << " -v | --verbose " + << "enable additional output" << endl; + cerr << " -d | --dof-overview " + << "print an overview of the degress of freedom" << endl; + cerr << " -f | --floatbase " + <<"set the first mobile body as floating base" << endl; + cerr << " -m | --model-hierarchy " + << "print the hierarchy of the model" << endl; + cerr << " -o | --body-origins " + << "print the origins of all bodies that have names" << endl; + cerr << " -c | --center_of_mass " + << "print center of mass for bodies and full model" << endl; cerr << " -h | --help print this help" << endl; exit (1); } @@ -63,7 +69,8 @@ int main (int argc, char *argv[]) { RigidBodyDynamics::Model model; - if (!RigidBodyDynamics::Addons::URDFReadFromFile(filename.c_str(), &model, floatbase, verbose)) { + if (!RigidBodyDynamics::Addons::URDFReadFromFile(filename.c_str(), &model, + floatbase, verbose)) { cerr << "Loading of urdf model failed!" << endl; return -1; } @@ -96,13 +103,16 @@ int main (int argc, char *argv[]) { SpatialRigidBodyInertia rbi_base = model.X_base[i].apply(model.I[i]); Vector3d body_com = rbi_base.h / rbi_base.m; - cout << setw(12) << model.GetBodyName (i) << ": " << setw(10) << body_com.transpose() << endl; + cout << setw(12) << model.GetBodyName (i) << ": " << setw(10) + << body_com.transpose() << endl; } Vector3d model_com; double mass; - RigidBodyDynamics::Utils::CalcCenterOfMass (model, q_zero, qdot_zero, NULL, mass, model_com); - cout << setw(14) << "Model COM: " << setw(10) << model_com.transpose() << endl; + RigidBodyDynamics::Utils::CalcCenterOfMass (model, q_zero, qdot_zero, + NULL, mass, model_com); + cout << setw(14) << "Model COM: " << setw(10) + << model_com.transpose() << endl; cout << setw(14) << "Model mass: " << mass << endl; } diff --git a/addons/urdfreader/thirdparty/CMakeLists.txt b/addons/urdfreader/thirdparty/CMakeLists.txt new file mode 100644 index 0000000..a57f5a7 --- /dev/null +++ b/addons/urdfreader/thirdparty/CMakeLists.txt @@ -0,0 +1,7 @@ +FIND_FILE(URDF_MODULE CMakeLists.txt PATHS ${CMAKE_CURRENT_SOURCE_DIR}/urdfparser NO_DEFAULT_PATH) +IF (URDF_MODULE STREQUAL URDF_MODULE-NOTFOUND) + MESSAGE(FATAL_ERROR "The urdfreader module was not initalized correctly! Please make sure that you recursively cloned rbdl. For more information take a closer look at the README!") +ENDIF(URDF_MODULE STREQUAL URDF_MODULE-NOTFOUND) +ADD_SUBDIRECTORY( + urdfparser +) diff --git a/addons/urdfreader/thirdparty/README.md b/addons/urdfreader/thirdparty/README.md deleted file mode 100644 index cd7a0d9..0000000 --- a/addons/urdfreader/thirdparty/README.md +++ /dev/null @@ -1,5 +0,0 @@ -Code from the urdf directory was taken from the Bullet 3 source code -repository https://github.com/bulletphysics/bullet3 from the directory -examples/ThirdPartyLibs/urdf. - -The tinyxml code was obtained from http://sourceforge.net/projects/tinyxml/ diff --git a/addons/urdfreader/thirdparty/tinyxml/changes.txt b/addons/urdfreader/thirdparty/tinyxml/changes.txt deleted file mode 100644 index 15b51bd..0000000 --- a/addons/urdfreader/thirdparty/tinyxml/changes.txt +++ /dev/null @@ -1,299 +0,0 @@ -Changes in version 1.0.1: -- Fixed comment tags which were outputing as ' include. Thanks - to Steve Lhomme for that. - -Changes in version 2.0.0 BETA -- Made the ToXXX() casts safe if 'this' is null. - When "LoadFile" is called with a filename, the value will correctly get set. - Thanks to Brian Yoder. -- Fixed bug where isalpha() and isalnum() would get called with a negative value for - high ascii numbers. Thanks to Alesky Aksenov. -- Fixed some errors codes that were not getting set. -- Made methods "const" that were not. -- Added a switch to enable or disable the ignoring of white space. ( TiXmlDocument::SetIgnoreWhiteSpace() ) -- Greater standardization and code re-use in the parser. -- Added a stream out operator. -- Added a stream in operator. -- Entity support, of predefined entites. &#x entities are untouched by input or output. -- Improved text out formatting. -- Fixed ReplaceChild bug, thanks to Tao Chen. - -Changes in version 2.0.1 -- Fixed hanging on loading a 0 length file. Thanks to Jeff Scozzafava. -- Fixed crashing on InsertBeforeChild and InsertAfterChild. Also possibility of bad links being - created by same function. Thanks to Frank De prins. -- Added missing licence text. Thanks to Lars Willemsens. -- Added include, at the suggestion of Steve Walters. - -Changes in version 2.1.0 -- Yves Berquin brings us the STL switch. The forum on SourceForge, and various emails to - me, have long debated all out STL vs. no STL at all. And now you can have it both ways. - TinyXml will compile either way. - -Changes in version 2.1.1 -- Compilation warnings. - -Changes in version 2.1.2 -- Uneeded code is not compiled in the STL case. -- Changed headers so that STL can be turned on or off in tinyxml.h - -Changes in version 2.1.3 -- Fixed non-const reference in API; now uses a pointer. -- Copy constructor of TiXmlString not checking for assignment to self. -- Nimrod Cohen found a truly evil bug in the STL implementation that occurs - when a string is converted to a c_str and then assigned to self. Search for - STL_STRING_BUG for a full description. I'm asserting this is a Microsoft STL - bug, since &string and string.c_str() should never be the same. Nevertheless, - the code works around it. -- Urivan Saaib pointed out a compiler conflict, where the C headers define - the isblank macro, which was wiping out the TiXmlString::isblank() method. - The method was unused and has been removed. - -Changes in version 2.1.4 -- Reworked the entity code. Entities were not correctly surving round trip input and output. - Will now automatically create entities for high ascii in output. - -Changes in version 2.1.5 -- Bug fix by kylotan : infinite loop on some input (tinyxmlparser.cpp rev 1.27) -- Contributed by Ivica Aracic (bytelord) : 1 new VC++ project to compile versions as static libraries (tinyxml_lib.dsp), - and an example usage in xmltest.dsp - (Patch request ID 678605) -- A suggestion by Ronald Fenner Jr (dormlock) to add #include and for Apple's Project Builder - (Patch request ID 697642) -- A patch from ohommes that allows to parse correctly dots in element names and attribute names - (Patch request 602600 and kylotan 701728) -- A patch from hermitgeek ( James ) and wasteland for improper error reporting -- Reviewed by Lee, with the following changes: - - Got sick of fighting the STL/non-STL thing in the windows build. Broke - them out as seperate projects. - - I have too long not included the dsw. Added. - - TinyXmlText had a protected Print. Odd. - - Made LinkEndChild public, with docs and appropriate warnings. - - Updated the docs. - -2.2.0 -- Fixed an uninitialized pointer in the TiXmlAttributes -- Fixed STL compilation problem in MinGW (and gcc 3?) - thanks Brian Yoder for finding this one -- Fixed a syntax error in TiXmlDeclaration - thanks Brian Yoder -- Fletcher Dunn proposed and submitted new error handling that tracked the row and column. Lee - modified it to not have performance impact. -- General cleanup suggestions from Fletcher Dunn. -- In error handling, general errors will no longer clear the error state of specific ones. -- Fix error in documentation : comments starting with ">) has now - been fixed. - -2.5.2 -- Lieven, and others, pointed out a missing const-cast that upset the Open Watcom compiler. - Should now be fixed. -- ErrorRow and ErrorCol should have been const, and weren't. Fixed thanks to Dmitry Polutov. - -2.5.3 -- zloe_zlo identified a missing string specialization for QueryValueAttribute() [ 1695429 ]. Worked - on this bug, but not sure how to fix it in a safe, cross-compiler way. -- increased warning level to 4 and turned on detect 64 bit portability issues for VC2005. - May address [ 1677737 ] VS2005: /Wp64 warnings -- grosheck identified several problems with the Document copy. Many thanks for [ 1660367 ] -- Nice catch, and suggested fix, be Gilad Novik on the Printer dropping entities. - "[ 1600650 ] Bug when printing xml text" is now fixed. -- A subtle fix from Nicos Gollan in the tinystring initializer: - [ 1581449 ] Fix initialiser of TiXmlString::nullrep_ -- Great catch, although there isn't a submitter for the bug. [ 1475201 ] TinyXML parses entities in comments. - Comments should not, in fact, parse entities. Fixed the code path and added tests. -- We were not catching all the returns from ftell. Thanks to Bernard for catching that. - -2.5.4 -- A TiXMLDocument can't be a sub-node. Block this from happening in the 'replace'. Thanks Noam. -- [ 1714831 ] TiXmlBase::location is not copied by copy-ctors, fix reported and suggested by Nicola Civran. -- Fixed possible memory overrun in the comment reading code - thanks gcarlton77 - -2.5.5 -- Alex van der Wal spotted incorrect types (lf) being used in print and scan. robertnestor pointed out some problems with the simple solution. Types updated. -- Johannes Hillert pointed out some bug typos. -- Christian Mueller identified inconsistent error handling with Attributes. -- olivier barthelemy also reported a problem with double truncation, also related to the %lf issue. -- zaelsius came up with a great (and simple) suggestion to fix QueryValueAttribute truncating strings. -- added some null pointer checks suggested by hansenk -- Sami Väisänen found a (rare) buffer overrun that could occur in parsing. -- vi tri filed a bug that led to a refactoring of the attribute setting mess (as well as adding a missing SetDoubleAttribute() ) -- removed TIXML_ERROR_OUT_OF_MEMORY. TinyXML does not systematically address OOO, and the notion it does is misleading. -- vanneto, keithmarshall, others all reported the warning from IsWhiteSpace() usage. Cleaned this up - many thanks to everyone who reported this one. -- tibur found a bug in end tag parsing - - -2.6.2 -- Switched over to VC 2010 -- Fixed up all the build issues arising from that. (Lots of latent build problems.) -- Removed the old, now unmaintained and likely not working, build files. -- Fixed some static analysis issues reported by orbitcowboy from cppcheck. -- Bayard 95 sent in analysis from a different analyzer - fixes applied from that as well. -- Tim Kosse sent a patch fixing an infinite loop. -- Ma Anguo identified a doc issue. -- Eddie Cohen identified a missing qualifier resulting in a compilation error on some systems. -- Fixed a line ending bug. (What year is this? Can we all agree on a format for text files? Please? ...oh well.) - diff --git a/addons/urdfreader/thirdparty/tinyxml/readme.txt b/addons/urdfreader/thirdparty/tinyxml/readme.txt deleted file mode 100644 index 89d9e8d..0000000 --- a/addons/urdfreader/thirdparty/tinyxml/readme.txt +++ /dev/null @@ -1,530 +0,0 @@ -/** @mainpage - -

TinyXML

- -TinyXML is a simple, small, C++ XML parser that can be easily -integrated into other programs. - -

What it does.

- -In brief, TinyXML parses an XML document, and builds from that a -Document Object Model (DOM) that can be read, modified, and saved. - -XML stands for "eXtensible Markup Language." It allows you to create -your own document markups. Where HTML does a very good job of marking -documents for browsers, XML allows you to define any kind of document -markup, for example a document that describes a "to do" list for an -organizer application. XML is a very structured and convenient format. -All those random file formats created to store application data can -all be replaced with XML. One parser for everything. - -The best place for the complete, correct, and quite frankly hard to -read spec is at -http://www.w3.org/TR/2004/REC-xml-20040204/. An intro to XML -(that I really like) can be found at -http://skew.org/xml/tutorial. - -There are different ways to access and interact with XML data. -TinyXML uses a Document Object Model (DOM), meaning the XML data is parsed -into a C++ objects that can be browsed and manipulated, and then -written to disk or another output stream. You can also construct an XML document -from scratch with C++ objects and write this to disk or another output -stream. - -TinyXML is designed to be easy and fast to learn. It is two headers -and four cpp files. Simply add these to your project and off you go. -There is an example file - xmltest.cpp - to get you started. - -TinyXML is released under the ZLib license, -so you can use it in open source or commercial code. The details -of the license are at the top of every source file. - -TinyXML attempts to be a flexible parser, but with truly correct and -compliant XML output. TinyXML should compile on any reasonably C++ -compliant system. It does not rely on exceptions or RTTI. It can be -compiled with or without STL support. TinyXML fully supports -the UTF-8 encoding, and the first 64k character entities. - - -

What it doesn't do.

- -TinyXML doesn't parse or use DTDs (Document Type Definitions) or XSLs -(eXtensible Stylesheet Language.) There are other parsers out there -(check out www.sourceforge.org, search for XML) that are much more fully -featured. But they are also much bigger, take longer to set up in -your project, have a higher learning curve, and often have a more -restrictive license. If you are working with browsers or have more -complete XML needs, TinyXML is not the parser for you. - -The following DTD syntax will not parse at this time in TinyXML: - -@verbatim - - ]> -@endverbatim - -because TinyXML sees this as a !DOCTYPE node with an illegally -embedded !ELEMENT node. This may be addressed in the future. - -

Tutorials.

- -For the impatient, here is a tutorial to get you going. A great way to get started, -but it is worth your time to read this (very short) manual completely. - -- @subpage tutorial0 - -

Code Status.

- -TinyXML is mature, tested code. It is very stable. If you find -bugs, please file a bug report on the sourceforge web site -(www.sourceforge.net/projects/tinyxml). We'll get them straightened -out as soon as possible. - -There are some areas of improvement; please check sourceforge if you are -interested in working on TinyXML. - -

Related Projects

- -TinyXML projects you may find useful! (Descriptions provided by the projects.) - -
    -
  • TinyXPath (http://tinyxpath.sourceforge.net). TinyXPath is a small footprint - XPath syntax decoder, written in C++.
    • -
  • TinyXML++ (http://code.google.com/p/ticpp/). TinyXML++ is a completely new - interface to TinyXML that uses MANY of the C++ strengths. Templates, - exceptions, and much better error handling.
    • -
- -

Features

- -

Using STL

- -TinyXML can be compiled to use or not use STL. When using STL, TinyXML -uses the std::string class, and fully supports std::istream, std::ostream, -operator<<, and operator>>. Many API methods have both 'const char*' and -'const std::string&' forms. - -When STL support is compiled out, no STL files are included whatsoever. All -the string classes are implemented by TinyXML itself. API methods -all use the 'const char*' form for input. - -Use the compile time #define: - - TIXML_USE_STL - -to compile one version or the other. This can be passed by the compiler, -or set as the first line of "tinyxml.h". - -Note: If compiling the test code in Linux, setting the environment -variable TINYXML_USE_STL=YES/NO will control STL compilation. In the -Windows project file, STL and non STL targets are provided. In your project, -It's probably easiest to add the line "#define TIXML_USE_STL" as the first -line of tinyxml.h. - -

UTF-8

- -TinyXML supports UTF-8 allowing to manipulate XML files in any language. TinyXML -also supports "legacy mode" - the encoding used before UTF-8 support and -probably best described as "extended ascii". - -Normally, TinyXML will try to detect the correct encoding and use it. However, -by setting the value of TIXML_DEFAULT_ENCODING in the header file, TinyXML -can be forced to always use one encoding. - -TinyXML will assume Legacy Mode until one of the following occurs: -
    -
  1. If the non-standard but common "UTF-8 lead bytes" (0xef 0xbb 0xbf) - begin the file or data stream, TinyXML will read it as UTF-8.
    2. -
  2. If the declaration tag is read, and it has an encoding="UTF-8", then - TinyXML will read it as UTF-8.
    3. -
  3. If the declaration tag is read, and it has no encoding specified, then TinyXML will - read it as UTF-8.
    4. -
  4. If the declaration tag is read, and it has an encoding="something else", then TinyXML - will read it as Legacy Mode. In legacy mode, TinyXML will work as it did before. It's - not clear what that mode does exactly, but old content should keep working.
    5. -
  5. Until one of the above criteria is met, TinyXML runs in Legacy Mode.
    6. -
- -What happens if the encoding is incorrectly set or detected? TinyXML will try -to read and pass through text seen as improperly encoded. You may get some strange results or -mangled characters. You may want to force TinyXML to the correct mode. - -You may force TinyXML to Legacy Mode by using LoadFile( TIXML_ENCODING_LEGACY ) or -LoadFile( filename, TIXML_ENCODING_LEGACY ). You may force it to use legacy mode all -the time by setting TIXML_DEFAULT_ENCODING = TIXML_ENCODING_LEGACY. Likewise, you may -force it to TIXML_ENCODING_UTF8 with the same technique. - -For English users, using English XML, UTF-8 is the same as low-ASCII. You -don't need to be aware of UTF-8 or change your code in any way. You can think -of UTF-8 as a "superset" of ASCII. - -UTF-8 is not a double byte format - but it is a standard encoding of Unicode! -TinyXML does not use or directly support wchar, TCHAR, or Microsoft's _UNICODE at this time. -It is common to see the term "Unicode" improperly refer to UTF-16, a wide byte encoding -of unicode. This is a source of confusion. - -For "high-ascii" languages - everything not English, pretty much - TinyXML can -handle all languages, at the same time, as long as the XML is encoded -in UTF-8. That can be a little tricky, older programs and operating systems -tend to use the "default" or "traditional" code page. Many apps (and almost all -modern ones) can output UTF-8, but older or stubborn (or just broken) ones -still output text in the default code page. - -For example, Japanese systems traditionally use SHIFT-JIS encoding. -Text encoded as SHIFT-JIS can not be read by TinyXML. -A good text editor can import SHIFT-JIS and then save as UTF-8. - -The Skew.org link does a great -job covering the encoding issue. - -The test file "utf8test.xml" is an XML containing English, Spanish, Russian, -and Simplified Chinese. (Hopefully they are translated correctly). The file -"utf8test.gif" is a screen capture of the XML file, rendered in IE. Note that -if you don't have the correct fonts (Simplified Chinese or Russian) on your -system, you won't see output that matches the GIF file even if you can parse -it correctly. Also note that (at least on my Windows machine) console output -is in a Western code page, so that Print() or printf() cannot correctly display -the file. This is not a bug in TinyXML - just an OS issue. No data is lost or -destroyed by TinyXML. The console just doesn't render UTF-8. - - -

Entities

-TinyXML recognizes the pre-defined "character entities", meaning special -characters. Namely: - -@verbatim - & & - < < - > > - " " - ' ' -@endverbatim - -These are recognized when the XML document is read, and translated to there -UTF-8 equivalents. For instance, text with the XML of: - -@verbatim - Far & Away -@endverbatim - -will have the Value() of "Far & Away" when queried from the TiXmlText object, -and will be written back to the XML stream/file as an ampersand. Older versions -of TinyXML "preserved" character entities, but the newer versions will translate -them into characters. - -Additionally, any character can be specified by its Unicode code point: -The syntax " " or " " are both to the non-breaking space characher. - -

Printing

-TinyXML can print output in several different ways that all have strengths and limitations. - -- Print( FILE* ). Output to a std-C stream, which includes all C files as well as stdout. - - "Pretty prints", but you don't have control over printing options. - - The output is streamed directly to the FILE object, so there is no memory overhead - in the TinyXML code. - - used by Print() and SaveFile() - -- operator<<. Output to a c++ stream. - - Integrates with standart C++ iostreams. - - Outputs in "network printing" mode without line breaks. Good for network transmission - and moving XML between C++ objects, but hard for a human to read. - -- TiXmlPrinter. Output to a std::string or memory buffer. - - API is less concise - - Future printing options will be put here. - - Printing may change slightly in future versions as it is refined and expanded. - -

Streams

-With TIXML_USE_STL on TinyXML supports C++ streams (operator <<,>>) streams as well -as C (FILE*) streams. There are some differences that you may need to be aware of. - -C style output: - - based on FILE* - - the Print() and SaveFile() methods - - Generates formatted output, with plenty of white space, intended to be as - human-readable as possible. They are very fast, and tolerant of ill formed - XML documents. For example, an XML document that contains 2 root elements - and 2 declarations, will still print. - -C style input: - - based on FILE* - - the Parse() and LoadFile() methods - - A fast, tolerant read. Use whenever you don't need the C++ streams. - -C++ style output: - - based on std::ostream - - operator<< - - Generates condensed output, intended for network transmission rather than - readability. Depending on your system's implementation of the ostream class, - these may be somewhat slower. (Or may not.) Not tolerant of ill formed XML: - a document should contain the correct one root element. Additional root level - elements will not be streamed out. - -C++ style input: - - based on std::istream - - operator>> - - Reads XML from a stream, making it useful for network transmission. The tricky - part is knowing when the XML document is complete, since there will almost - certainly be other data in the stream. TinyXML will assume the XML data is - complete after it reads the root element. Put another way, documents that - are ill-constructed with more than one root element will not read correctly. - Also note that operator>> is somewhat slower than Parse, due to both - implementation of the STL and limitations of TinyXML. - -

White space

-The world simply does not agree on whether white space should be kept, or condensed. -For example, pretend the '_' is a space, and look at "Hello____world". HTML, and -at least some XML parsers, will interpret this as "Hello_world". They condense white -space. Some XML parsers do not, and will leave it as "Hello____world". (Remember -to keep pretending the _ is a space.) Others suggest that __Hello___world__ should become -Hello___world. - -It's an issue that hasn't been resolved to my satisfaction. TinyXML supports the -first 2 approaches. Call TiXmlBase::SetCondenseWhiteSpace( bool ) to set the desired behavior. -The default is to condense white space. - -If you change the default, you should call TiXmlBase::SetCondenseWhiteSpace( bool ) -before making any calls to Parse XML data, and I don't recommend changing it after -it has been set. - - -

Handles

- -Where browsing an XML document in a robust way, it is important to check -for null returns from method calls. An error safe implementation can -generate a lot of code like: - -@verbatim -TiXmlElement* root = document.FirstChildElement( "Document" ); -if ( root ) -{ - TiXmlElement* element = root->FirstChildElement( "Element" ); - if ( element ) - { - TiXmlElement* child = element->FirstChildElement( "Child" ); - if ( child ) - { - TiXmlElement* child2 = child->NextSiblingElement( "Child" ); - if ( child2 ) - { - // Finally do something useful. -@endverbatim - -Handles have been introduced to clean this up. Using the TiXmlHandle class, -the previous code reduces to: - -@verbatim -TiXmlHandle docHandle( &document ); -TiXmlElement* child2 = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", 1 ).ToElement(); -if ( child2 ) -{ - // do something useful -@endverbatim - -Which is much easier to deal with. See TiXmlHandle for more information. - - -

Row and Column tracking

-Being able to track nodes and attributes back to their origin location -in source files can be very important for some applications. Additionally, -knowing where parsing errors occured in the original source can be very -time saving. - -TinyXML can tracks the row and column origin of all nodes and attributes -in a text file. The TiXmlBase::Row() and TiXmlBase::Column() methods return -the origin of the node in the source text. The correct tabs can be -configured in TiXmlDocument::SetTabSize(). - - -

Using and Installing

- -To Compile and Run xmltest: - -A Linux Makefile and a Windows Visual C++ .dsw file is provided. -Simply compile and run. It will write the file demotest.xml to your -disk and generate output on the screen. It also tests walking the -DOM by printing out the number of nodes found using different -techniques. - -The Linux makefile is very generic and runs on many systems - it -is currently tested on mingw and -MacOSX. You do not need to run 'make depend'. The dependecies have been -hard coded. - -

Windows project file for VC6

-
    -
  • tinyxml: tinyxml library, non-STL
    • -
  • tinyxmlSTL: tinyxml library, STL
    • -
  • tinyXmlTest: test app, non-STL
    • -
  • tinyXmlTestSTL: test app, STL
    • -
- -

Makefile

-At the top of the makefile you can set: - -PROFILE, DEBUG, and TINYXML_USE_STL. Details (such that they are) are in -the makefile. - -In the tinyxml directory, type "make clean" then "make". The executable -file 'xmltest' will be created. - - - -

To Use in an Application:

- -Add tinyxml.cpp, tinyxml.h, tinyxmlerror.cpp, tinyxmlparser.cpp, tinystr.cpp, and tinystr.h to your -project or make file. That's it! It should compile on any reasonably -compliant C++ system. You do not need to enable exceptions or -RTTI for TinyXML. - - -

How TinyXML works.

- -An example is probably the best way to go. Take: -@verbatim - - - - Go to the Toy store! - Do bills - -@endverbatim - -Its not much of a To Do list, but it will do. To read this file -(say "demo.xml") you would create a document, and parse it in: -@verbatim - TiXmlDocument doc( "demo.xml" ); - doc.LoadFile(); -@endverbatim - -And its ready to go. Now lets look at some lines and how they -relate to the DOM. - -@verbatim - -@endverbatim - - The first line is a declaration, and gets turned into the - TiXmlDeclaration class. It will be the first child of the - document node. - - This is the only directive/special tag parsed by TinyXML. - Generally directive tags are stored in TiXmlUnknown so the - commands wont be lost when it is saved back to disk. - -@verbatim - -@endverbatim - - A comment. Will become a TiXmlComment object. - -@verbatim - -@endverbatim - - The "ToDo" tag defines a TiXmlElement object. This one does not have - any attributes, but does contain 2 other elements. - -@verbatim - -@endverbatim - - Creates another TiXmlElement which is a child of the "ToDo" element. - This element has 1 attribute, with the name "priority" and the value - "1". - -@verbatim -Go to the -@endverbatim - - A TiXmlText. This is a leaf node and cannot contain other nodes. - It is a child of the "Item" TiXmlElement. - -@verbatim - -@endverbatim - - - Another TiXmlElement, this one a child of the "Item" element. - -Etc. - -Looking at the entire object tree, you end up with: -@verbatim -TiXmlDocument "demo.xml" - TiXmlDeclaration "version='1.0'" "standalone=no" - TiXmlComment " Our to do list data" - TiXmlElement "ToDo" - TiXmlElement "Item" Attribtutes: priority = 1 - TiXmlText "Go to the " - TiXmlElement "bold" - TiXmlText "Toy store!" - TiXmlElement "Item" Attributes: priority=2 - TiXmlText "Do bills" -@endverbatim - -

Documentation

- -The documentation is build with Doxygen, using the 'dox' -configuration file. - -

License

- -TinyXML is released under the zlib license: - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any -damages arising from the use of this software. - -Permission is granted to anyone to use this software for any -purpose, including commercial applications, and to alter it and -redistribute it freely, subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must -not claim that you wrote the original software. If you use this -software in a product, an acknowledgment in the product documentation -would be appreciated but is not required. - -2. Altered source versions must be plainly marked as such, and -must not be misrepresented as being the original software. - -3. This notice may not be removed or altered from any source -distribution. - -

References

- -The World Wide Web Consortium is the definitive standard body for -XML, and their web pages contain huge amounts of information. - -The definitive spec: -http://www.w3.org/TR/2004/REC-xml-20040204/ - -I also recommend "XML Pocket Reference" by Robert Eckstein and published by -OReilly...the book that got the whole thing started. - -

Contributors, Contacts, and a Brief History

- -Thanks very much to everyone who sends suggestions, bugs, ideas, and -encouragement. It all helps, and makes this project fun. A special thanks -to the contributors on the web pages that keep it lively. - -So many people have sent in bugs and ideas, that rather than list here -we try to give credit due in the "changes.txt" file. - -TinyXML was originally written by Lee Thomason. (Often the "I" still -in the documentation.) Lee reviews changes and releases new versions, -with the help of Yves Berquin, Andrew Ellerton, and the tinyXml community. - -We appreciate your suggestions, and would love to know if you -use TinyXML. Hopefully you will enjoy it and find it useful. -Please post questions, comments, file bugs, or contact us at: - -www.sourceforge.net/projects/tinyxml - -Lee Thomason, Yves Berquin, Andrew Ellerton -*/ diff --git a/addons/urdfreader/thirdparty/tinyxml/tinystr.cpp b/addons/urdfreader/thirdparty/tinyxml/tinystr.cpp deleted file mode 100644 index 0665768..0000000 --- a/addons/urdfreader/thirdparty/tinyxml/tinystr.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* -www.sourceforge.net/projects/tinyxml - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any -damages arising from the use of this software. - -Permission is granted to anyone to use this software for any -purpose, including commercial applications, and to alter it and -redistribute it freely, subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must -not claim that you wrote the original software. If you use this -software in a product, an acknowledgment in the product documentation -would be appreciated but is not required. - -2. Altered source versions must be plainly marked as such, and -must not be misrepresented as being the original software. - -3. This notice may not be removed or altered from any source -distribution. -*/ - - -#ifndef TIXML_USE_STL - -#include "tinystr.h" - -// Error value for find primitive -const TiXmlString::size_type TiXmlString::npos = static_cast< TiXmlString::size_type >(-1); - - -// Null rep. -TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } }; - - -void TiXmlString::reserve (size_type cap) -{ - if (cap > capacity()) - { - TiXmlString tmp; - tmp.init(length(), cap); - memcpy(tmp.start(), data(), length()); - swap(tmp); - } -} - - -TiXmlString& TiXmlString::assign(const char* str, size_type len) -{ - size_type cap = capacity(); - if (len > cap || cap > 3*(len + 8)) - { - TiXmlString tmp; - tmp.init(len); - memcpy(tmp.start(), str, len); - swap(tmp); - } - else - { - memmove(start(), str, len); - set_size(len); - } - return *this; -} - - -TiXmlString& TiXmlString::append(const char* str, size_type len) -{ - size_type newsize = length() + len; - if (newsize > capacity()) - { - reserve (newsize + capacity()); - } - memmove(finish(), str, len); - set_size(newsize); - return *this; -} - - -TiXmlString operator + (const TiXmlString & a, const TiXmlString & b) -{ - TiXmlString tmp; - tmp.reserve(a.length() + b.length()); - tmp += a; - tmp += b; - return tmp; -} - -TiXmlString operator + (const TiXmlString & a, const char* b) -{ - TiXmlString tmp; - TiXmlString::size_type b_len = static_cast( strlen(b) ); - tmp.reserve(a.length() + b_len); - tmp += a; - tmp.append(b, b_len); - return tmp; -} - -TiXmlString operator + (const char* a, const TiXmlString & b) -{ - TiXmlString tmp; - TiXmlString::size_type a_len = static_cast( strlen(a) ); - tmp.reserve(a_len + b.length()); - tmp.append(a, a_len); - tmp += b; - return tmp; -} - - -#endif // TIXML_USE_STL diff --git a/addons/urdfreader/thirdparty/tinyxml/tinystr.h b/addons/urdfreader/thirdparty/tinyxml/tinystr.h deleted file mode 100644 index 89cca33..0000000 --- a/addons/urdfreader/thirdparty/tinyxml/tinystr.h +++ /dev/null @@ -1,305 +0,0 @@ -/* -www.sourceforge.net/projects/tinyxml - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any -damages arising from the use of this software. - -Permission is granted to anyone to use this software for any -purpose, including commercial applications, and to alter it and -redistribute it freely, subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must -not claim that you wrote the original software. If you use this -software in a product, an acknowledgment in the product documentation -would be appreciated but is not required. - -2. Altered source versions must be plainly marked as such, and -must not be misrepresented as being the original software. - -3. This notice may not be removed or altered from any source -distribution. -*/ - - -#ifndef TIXML_USE_STL - -#ifndef TIXML_STRING_INCLUDED -#define TIXML_STRING_INCLUDED - -#include -#include - -/* The support for explicit isn't that universal, and it isn't really - required - it is used to check that the TiXmlString class isn't incorrectly - used. Be nice to old compilers and macro it here: -*/ -#if defined(_MSC_VER) && (_MSC_VER >= 1200 ) - // Microsoft visual studio, version 6 and higher. - #define TIXML_EXPLICIT explicit -#elif defined(__GNUC__) && (__GNUC__ >= 3 ) - // GCC version 3 and higher.s - #define TIXML_EXPLICIT explicit -#else - #define TIXML_EXPLICIT -#endif - - -/* - TiXmlString is an emulation of a subset of the std::string template. - Its purpose is to allow compiling TinyXML on compilers with no or poor STL support. - Only the member functions relevant to the TinyXML project have been implemented. - The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase - a string and there's no more room, we allocate a buffer twice as big as we need. -*/ -class TiXmlString -{ - public : - // The size type used - typedef size_t size_type; - - // Error value for find primitive - static const size_type npos; // = -1; - - - // TiXmlString empty constructor - TiXmlString () : rep_(&nullrep_) - { - } - - // TiXmlString copy constructor - TiXmlString ( const TiXmlString & copy) : rep_(0) - { - init(copy.length()); - memcpy(start(), copy.data(), length()); - } - - // TiXmlString constructor, based on a string - TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0) - { - init( static_cast( strlen(copy) )); - memcpy(start(), copy, length()); - } - - // TiXmlString constructor, based on a string - TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0) - { - init(len); - memcpy(start(), str, len); - } - - // TiXmlString destructor - ~TiXmlString () - { - quit(); - } - - TiXmlString& operator = (const char * copy) - { - return assign( copy, (size_type)strlen(copy)); - } - - TiXmlString& operator = (const TiXmlString & copy) - { - return assign(copy.start(), copy.length()); - } - - - // += operator. Maps to append - TiXmlString& operator += (const char * suffix) - { - return append(suffix, static_cast( strlen(suffix) )); - } - - // += operator. Maps to append - TiXmlString& operator += (char single) - { - return append(&single, 1); - } - - // += operator. Maps to append - TiXmlString& operator += (const TiXmlString & suffix) - { - return append(suffix.data(), suffix.length()); - } - - - // Convert a TiXmlString into a null-terminated char * - const char * c_str () const { return rep_->str; } - - // Convert a TiXmlString into a char * (need not be null terminated). - const char * data () const { return rep_->str; } - - // Return the length of a TiXmlString - size_type length () const { return rep_->size; } - - // Alias for length() - size_type size () const { return rep_->size; } - - // Checks if a TiXmlString is empty - bool empty () const { return rep_->size == 0; } - - // Return capacity of string - size_type capacity () const { return rep_->capacity; } - - - // single char extraction - const char& at (size_type index) const - { - assert( index < length() ); - return rep_->str[ index ]; - } - - // [] operator - char& operator [] (size_type index) const - { - assert( index < length() ); - return rep_->str[ index ]; - } - - // find a char in a string. Return TiXmlString::npos if not found - size_type find (char lookup) const - { - return find(lookup, 0); - } - - // find a char in a string from an offset. Return TiXmlString::npos if not found - size_type find (char tofind, size_type offset) const - { - if (offset >= length()) return npos; - - for (const char* p = c_str() + offset; *p != '\0'; ++p) - { - if (*p == tofind) return static_cast< size_type >( p - c_str() ); - } - return npos; - } - - void clear () - { - //Lee: - //The original was just too strange, though correct: - // TiXmlString().swap(*this); - //Instead use the quit & re-init: - quit(); - init(0,0); - } - - /* Function to reserve a big amount of data when we know we'll need it. Be aware that this - function DOES NOT clear the content of the TiXmlString if any exists. - */ - void reserve (size_type cap); - - TiXmlString& assign (const char* str, size_type len); - - TiXmlString& append (const char* str, size_type len); - - void swap (TiXmlString& other) - { - Rep* r = rep_; - rep_ = other.rep_; - other.rep_ = r; - } - - private: - - void init(size_type sz) { init(sz, sz); } - void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; } - char* start() const { return rep_->str; } - char* finish() const { return rep_->str + rep_->size; } - - struct Rep - { - size_type size, capacity; - char str[1]; - }; - - void init(size_type sz, size_type cap) - { - if (cap) - { - // Lee: the original form: - // rep_ = static_cast(operator new(sizeof(Rep) + cap)); - // doesn't work in some cases of new being overloaded. Switching - // to the normal allocation, although use an 'int' for systems - // that are overly picky about structure alignment. - const size_type bytesNeeded = sizeof(Rep) + cap; - const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int ); - rep_ = reinterpret_cast( new int[ intsNeeded ] ); - - rep_->str[ rep_->size = sz ] = '\0'; - rep_->capacity = cap; - } - else - { - rep_ = &nullrep_; - } - } - - void quit() - { - if (rep_ != &nullrep_) - { - // The rep_ is really an array of ints. (see the allocator, above). - // Cast it back before delete, so the compiler won't incorrectly call destructors. - delete [] ( reinterpret_cast( rep_ ) ); - } - } - - Rep * rep_; - static Rep nullrep_; - -} ; - - -inline bool operator == (const TiXmlString & a, const TiXmlString & b) -{ - return ( a.length() == b.length() ) // optimization on some platforms - && ( strcmp(a.c_str(), b.c_str()) == 0 ); // actual compare -} -inline bool operator < (const TiXmlString & a, const TiXmlString & b) -{ - return strcmp(a.c_str(), b.c_str()) < 0; -} - -inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); } -inline bool operator > (const TiXmlString & a, const TiXmlString & b) { return b < a; } -inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); } -inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); } - -inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; } -inline bool operator == (const char* a, const TiXmlString & b) { return b == a; } -inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); } -inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); } - -TiXmlString operator + (const TiXmlString & a, const TiXmlString & b); -TiXmlString operator + (const TiXmlString & a, const char* b); -TiXmlString operator + (const char* a, const TiXmlString & b); - - -/* - TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString. - Only the operators that we need for TinyXML have been developped. -*/ -class TiXmlOutStream : public TiXmlString -{ -public : - - // TiXmlOutStream << operator. - TiXmlOutStream & operator << (const TiXmlString & in) - { - *this += in; - return *this; - } - - // TiXmlOutStream << operator. - TiXmlOutStream & operator << (const char * in) - { - *this += in; - return *this; - } - -} ; - -#endif // TIXML_STRING_INCLUDED -#endif // TIXML_USE_STL diff --git a/addons/urdfreader/thirdparty/tinyxml/tinyxml.cpp b/addons/urdfreader/thirdparty/tinyxml/tinyxml.cpp deleted file mode 100644 index 9c161df..0000000 --- a/addons/urdfreader/thirdparty/tinyxml/tinyxml.cpp +++ /dev/null @@ -1,1886 +0,0 @@ -/* -www.sourceforge.net/projects/tinyxml -Original code by Lee Thomason (www.grinninglizard.com) - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any -damages arising from the use of this software. - -Permission is granted to anyone to use this software for any -purpose, including commercial applications, and to alter it and -redistribute it freely, subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must -not claim that you wrote the original software. If you use this -software in a product, an acknowledgment in the product documentation -would be appreciated but is not required. - -2. Altered source versions must be plainly marked as such, and -must not be misrepresented as being the original software. - -3. This notice may not be removed or altered from any source -distribution. -*/ - -#include - -#ifdef TIXML_USE_STL -#include -#include -#endif - -#include "tinyxml.h" - -FILE* TiXmlFOpen( const char* filename, const char* mode ); - -bool TiXmlBase::condenseWhiteSpace = true; - -// Microsoft compiler security -FILE* TiXmlFOpen( const char* filename, const char* mode ) -{ - #if defined(_MSC_VER) && (_MSC_VER >= 1400 ) - FILE* fp = 0; - errno_t err = fopen_s( &fp, filename, mode ); - if ( !err && fp ) - return fp; - return 0; - #else - return fopen( filename, mode ); - #endif -} - -void TiXmlBase::EncodeString( const TIXML_STRING& str, TIXML_STRING* outString ) -{ - int i=0; - - while( i<(int)str.length() ) - { - unsigned char c = (unsigned char) str[i]; - - if ( c == '&' - && i < ( (int)str.length() - 2 ) - && str[i+1] == '#' - && str[i+2] == 'x' ) - { - // Hexadecimal character reference. - // Pass through unchanged. - // © -- copyright symbol, for example. - // - // The -1 is a bug fix from Rob Laveaux. It keeps - // an overflow from happening if there is no ';'. - // There are actually 2 ways to exit this loop - - // while fails (error case) and break (semicolon found). - // However, there is no mechanism (currently) for - // this function to return an error. - while ( i<(int)str.length()-1 ) - { - outString->append( str.c_str() + i, 1 ); - ++i; - if ( str[i] == ';' ) - break; - } - } - else if ( c == '&' ) - { - outString->append( entity[0].str, entity[0].strLength ); - ++i; - } - else if ( c == '<' ) - { - outString->append( entity[1].str, entity[1].strLength ); - ++i; - } - else if ( c == '>' ) - { - outString->append( entity[2].str, entity[2].strLength ); - ++i; - } - else if ( c == '\"' ) - { - outString->append( entity[3].str, entity[3].strLength ); - ++i; - } - else if ( c == '\'' ) - { - outString->append( entity[4].str, entity[4].strLength ); - ++i; - } - else if ( c < 32 ) - { - // Easy pass at non-alpha/numeric/symbol - // Below 32 is symbolic. - char buf[ 32 ]; - - #if defined(TIXML_SNPRINTF) - TIXML_SNPRINTF( buf, sizeof(buf), "&#x%02X;", (unsigned) ( c & 0xff ) ); - #else - sprintf( buf, "&#x%02X;", (unsigned) ( c & 0xff ) ); - #endif - - //*ME: warning C4267: convert 'size_t' to 'int' - //*ME: Int-Cast to make compiler happy ... - outString->append( buf, (int)strlen( buf ) ); - ++i; - } - else - { - //char realc = (char) c; - //outString->append( &realc, 1 ); - *outString += (char) c; // somewhat more efficient function call. - ++i; - } - } -} - - -TiXmlNode::TiXmlNode( NodeType _type ) : TiXmlBase() -{ - parent = 0; - type = _type; - firstChild = 0; - lastChild = 0; - prev = 0; - next = 0; -} - - -TiXmlNode::~TiXmlNode() -{ - TiXmlNode* node = firstChild; - TiXmlNode* temp = 0; - - while ( node ) - { - temp = node; - node = node->next; - delete temp; - } -} - - -void TiXmlNode::CopyTo( TiXmlNode* target ) const -{ - target->SetValue (value.c_str() ); - target->userData = userData; - target->location = location; -} - - -void TiXmlNode::Clear() -{ - TiXmlNode* node = firstChild; - TiXmlNode* temp = 0; - - while ( node ) - { - temp = node; - node = node->next; - delete temp; - } - - firstChild = 0; - lastChild = 0; -} - - -TiXmlNode* TiXmlNode::LinkEndChild( TiXmlNode* node ) -{ - assert( node->parent == 0 || node->parent == this ); - assert( node->GetDocument() == 0 || node->GetDocument() == this->GetDocument() ); - - if ( node->Type() == TiXmlNode::TINYXML_DOCUMENT ) - { - delete node; - if ( GetDocument() ) - GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - node->parent = this; - - node->prev = lastChild; - node->next = 0; - - if ( lastChild ) - lastChild->next = node; - else - firstChild = node; // it was an empty list. - - lastChild = node; - return node; -} - - -TiXmlNode* TiXmlNode::InsertEndChild( const TiXmlNode& addThis ) -{ - if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) - { - if ( GetDocument() ) - GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - TiXmlNode* node = addThis.Clone(); - if ( !node ) - return 0; - - return LinkEndChild( node ); -} - - -TiXmlNode* TiXmlNode::InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis ) -{ - if ( !beforeThis || beforeThis->parent != this ) { - return 0; - } - if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) - { - if ( GetDocument() ) - GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - TiXmlNode* node = addThis.Clone(); - if ( !node ) - return 0; - node->parent = this; - - node->next = beforeThis; - node->prev = beforeThis->prev; - if ( beforeThis->prev ) - { - beforeThis->prev->next = node; - } - else - { - assert( firstChild == beforeThis ); - firstChild = node; - } - beforeThis->prev = node; - return node; -} - - -TiXmlNode* TiXmlNode::InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis ) -{ - if ( !afterThis || afterThis->parent != this ) { - return 0; - } - if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) - { - if ( GetDocument() ) - GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - TiXmlNode* node = addThis.Clone(); - if ( !node ) - return 0; - node->parent = this; - - node->prev = afterThis; - node->next = afterThis->next; - if ( afterThis->next ) - { - afterThis->next->prev = node; - } - else - { - assert( lastChild == afterThis ); - lastChild = node; - } - afterThis->next = node; - return node; -} - - -TiXmlNode* TiXmlNode::ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis ) -{ - if ( !replaceThis ) - return 0; - - if ( replaceThis->parent != this ) - return 0; - - if ( withThis.ToDocument() ) { - // A document can never be a child. Thanks to Noam. - TiXmlDocument* document = GetDocument(); - if ( document ) - document->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - TiXmlNode* node = withThis.Clone(); - if ( !node ) - return 0; - - node->next = replaceThis->next; - node->prev = replaceThis->prev; - - if ( replaceThis->next ) - replaceThis->next->prev = node; - else - lastChild = node; - - if ( replaceThis->prev ) - replaceThis->prev->next = node; - else - firstChild = node; - - delete replaceThis; - node->parent = this; - return node; -} - - -bool TiXmlNode::RemoveChild( TiXmlNode* removeThis ) -{ - if ( !removeThis ) { - return false; - } - - if ( removeThis->parent != this ) - { - assert( 0 ); - return false; - } - - if ( removeThis->next ) - removeThis->next->prev = removeThis->prev; - else - lastChild = removeThis->prev; - - if ( removeThis->prev ) - removeThis->prev->next = removeThis->next; - else - firstChild = removeThis->next; - - delete removeThis; - return true; -} - -const TiXmlNode* TiXmlNode::FirstChild( const char * _value ) const -{ - const TiXmlNode* node; - for ( node = firstChild; node; node = node->next ) - { - if ( strcmp( node->Value(), _value ) == 0 ) - return node; - } - return 0; -} - - -const TiXmlNode* TiXmlNode::LastChild( const char * _value ) const -{ - const TiXmlNode* node; - for ( node = lastChild; node; node = node->prev ) - { - if ( strcmp( node->Value(), _value ) == 0 ) - return node; - } - return 0; -} - - -const TiXmlNode* TiXmlNode::IterateChildren( const TiXmlNode* previous ) const -{ - if ( !previous ) - { - return FirstChild(); - } - else - { - assert( previous->parent == this ); - return previous->NextSibling(); - } -} - - -const TiXmlNode* TiXmlNode::IterateChildren( const char * val, const TiXmlNode* previous ) const -{ - if ( !previous ) - { - return FirstChild( val ); - } - else - { - assert( previous->parent == this ); - return previous->NextSibling( val ); - } -} - - -const TiXmlNode* TiXmlNode::NextSibling( const char * _value ) const -{ - const TiXmlNode* node; - for ( node = next; node; node = node->next ) - { - if ( strcmp( node->Value(), _value ) == 0 ) - return node; - } - return 0; -} - - -const TiXmlNode* TiXmlNode::PreviousSibling( const char * _value ) const -{ - const TiXmlNode* node; - for ( node = prev; node; node = node->prev ) - { - if ( strcmp( node->Value(), _value ) == 0 ) - return node; - } - return 0; -} - - -void TiXmlElement::RemoveAttribute( const char * name ) -{ - #ifdef TIXML_USE_STL - TIXML_STRING str( name ); - TiXmlAttribute* node = attributeSet.Find( str ); - #else - TiXmlAttribute* node = attributeSet.Find( name ); - #endif - if ( node ) - { - attributeSet.Remove( node ); - delete node; - } -} - -const TiXmlElement* TiXmlNode::FirstChildElement() const -{ - const TiXmlNode* node; - - for ( node = FirstChild(); - node; - node = node->NextSibling() ) - { - if ( node->ToElement() ) - return node->ToElement(); - } - return 0; -} - - -const TiXmlElement* TiXmlNode::FirstChildElement( const char * _value ) const -{ - const TiXmlNode* node; - - for ( node = FirstChild( _value ); - node; - node = node->NextSibling( _value ) ) - { - if ( node->ToElement() ) - return node->ToElement(); - } - return 0; -} - - -const TiXmlElement* TiXmlNode::NextSiblingElement() const -{ - const TiXmlNode* node; - - for ( node = NextSibling(); - node; - node = node->NextSibling() ) - { - if ( node->ToElement() ) - return node->ToElement(); - } - return 0; -} - - -const TiXmlElement* TiXmlNode::NextSiblingElement( const char * _value ) const -{ - const TiXmlNode* node; - - for ( node = NextSibling( _value ); - node; - node = node->NextSibling( _value ) ) - { - if ( node->ToElement() ) - return node->ToElement(); - } - return 0; -} - - -const TiXmlDocument* TiXmlNode::GetDocument() const -{ - const TiXmlNode* node; - - for( node = this; node; node = node->parent ) - { - if ( node->ToDocument() ) - return node->ToDocument(); - } - return 0; -} - - -TiXmlElement::TiXmlElement (const char * _value) - : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) -{ - firstChild = lastChild = 0; - value = _value; -} - - -#ifdef TIXML_USE_STL -TiXmlElement::TiXmlElement( const std::string& _value ) - : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) -{ - firstChild = lastChild = 0; - value = _value; -} -#endif - - -TiXmlElement::TiXmlElement( const TiXmlElement& copy) - : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) -{ - firstChild = lastChild = 0; - copy.CopyTo( this ); -} - - -TiXmlElement& TiXmlElement::operator=( const TiXmlElement& base ) -{ - ClearThis(); - base.CopyTo( this ); - return *this; -} - - -TiXmlElement::~TiXmlElement() -{ - ClearThis(); -} - - -void TiXmlElement::ClearThis() -{ - Clear(); - while( attributeSet.First() ) - { - TiXmlAttribute* node = attributeSet.First(); - attributeSet.Remove( node ); - delete node; - } -} - - -const char* TiXmlElement::Attribute( const char* name ) const -{ - const TiXmlAttribute* node = attributeSet.Find( name ); - if ( node ) - return node->Value(); - return 0; -} - - -#ifdef TIXML_USE_STL -const std::string* TiXmlElement::Attribute( const std::string& name ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( attrib ) - return &attrib->ValueStr(); - return 0; -} -#endif - - -const char* TiXmlElement::Attribute( const char* name, int* i ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - const char* result = 0; - - if ( attrib ) { - result = attrib->Value(); - if ( i ) { - attrib->QueryIntValue( i ); - } - } - return result; -} - - -#ifdef TIXML_USE_STL -const std::string* TiXmlElement::Attribute( const std::string& name, int* i ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - const std::string* result = 0; - - if ( attrib ) { - result = &attrib->ValueStr(); - if ( i ) { - attrib->QueryIntValue( i ); - } - } - return result; -} -#endif - - -const char* TiXmlElement::Attribute( const char* name, double* d ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - const char* result = 0; - - if ( attrib ) { - result = attrib->Value(); - if ( d ) { - attrib->QueryDoubleValue( d ); - } - } - return result; -} - - -#ifdef TIXML_USE_STL -const std::string* TiXmlElement::Attribute( const std::string& name, double* d ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - const std::string* result = 0; - - if ( attrib ) { - result = &attrib->ValueStr(); - if ( d ) { - attrib->QueryDoubleValue( d ); - } - } - return result; -} -#endif - - -int TiXmlElement::QueryIntAttribute( const char* name, int* ival ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( !attrib ) - return TIXML_NO_ATTRIBUTE; - return attrib->QueryIntValue( ival ); -} - - -int TiXmlElement::QueryUnsignedAttribute( const char* name, unsigned* value ) const -{ - const TiXmlAttribute* node = attributeSet.Find( name ); - if ( !node ) - return TIXML_NO_ATTRIBUTE; - - int ival = 0; - int result = node->QueryIntValue( &ival ); - *value = (unsigned)ival; - return result; -} - - -int TiXmlElement::QueryBoolAttribute( const char* name, bool* bval ) const -{ - const TiXmlAttribute* node = attributeSet.Find( name ); - if ( !node ) - return TIXML_NO_ATTRIBUTE; - - int result = TIXML_WRONG_TYPE; - if ( StringEqual( node->Value(), "true", true, TIXML_ENCODING_UNKNOWN ) - || StringEqual( node->Value(), "yes", true, TIXML_ENCODING_UNKNOWN ) - || StringEqual( node->Value(), "1", true, TIXML_ENCODING_UNKNOWN ) ) - { - *bval = true; - result = TIXML_SUCCESS; - } - else if ( StringEqual( node->Value(), "false", true, TIXML_ENCODING_UNKNOWN ) - || StringEqual( node->Value(), "no", true, TIXML_ENCODING_UNKNOWN ) - || StringEqual( node->Value(), "0", true, TIXML_ENCODING_UNKNOWN ) ) - { - *bval = false; - result = TIXML_SUCCESS; - } - return result; -} - - - -#ifdef TIXML_USE_STL -int TiXmlElement::QueryIntAttribute( const std::string& name, int* ival ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( !attrib ) - return TIXML_NO_ATTRIBUTE; - return attrib->QueryIntValue( ival ); -} -#endif - - -int TiXmlElement::QueryDoubleAttribute( const char* name, double* dval ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( !attrib ) - return TIXML_NO_ATTRIBUTE; - return attrib->QueryDoubleValue( dval ); -} - - -#ifdef TIXML_USE_STL -int TiXmlElement::QueryDoubleAttribute( const std::string& name, double* dval ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( !attrib ) - return TIXML_NO_ATTRIBUTE; - return attrib->QueryDoubleValue( dval ); -} -#endif - - -void TiXmlElement::SetAttribute( const char * name, int val ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); - if ( attrib ) { - attrib->SetIntValue( val ); - } -} - - -#ifdef TIXML_USE_STL -void TiXmlElement::SetAttribute( const std::string& name, int val ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); - if ( attrib ) { - attrib->SetIntValue( val ); - } -} -#endif - - -void TiXmlElement::SetDoubleAttribute( const char * name, double val ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); - if ( attrib ) { - attrib->SetDoubleValue( val ); - } -} - - -#ifdef TIXML_USE_STL -void TiXmlElement::SetDoubleAttribute( const std::string& name, double val ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); - if ( attrib ) { - attrib->SetDoubleValue( val ); - } -} -#endif - - -void TiXmlElement::SetAttribute( const char * cname, const char * cvalue ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( cname ); - if ( attrib ) { - attrib->SetValue( cvalue ); - } -} - - -#ifdef TIXML_USE_STL -void TiXmlElement::SetAttribute( const std::string& _name, const std::string& _value ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( _name ); - if ( attrib ) { - attrib->SetValue( _value ); - } -} -#endif - - -void TiXmlElement::Print( FILE* cfile, int depth ) const -{ - int i; - assert( cfile ); - for ( i=0; iNext() ) - { - fprintf( cfile, " " ); - attrib->Print( cfile, depth ); - } - - // There are 3 different formatting approaches: - // 1) An element without children is printed as a node - // 2) An element with only a text child is printed as text - // 3) An element with children is printed on multiple lines. - TiXmlNode* node; - if ( !firstChild ) - { - fprintf( cfile, " />" ); - } - else if ( firstChild == lastChild && firstChild->ToText() ) - { - fprintf( cfile, ">" ); - firstChild->Print( cfile, depth + 1 ); - fprintf( cfile, "", value.c_str() ); - } - else - { - fprintf( cfile, ">" ); - - for ( node = firstChild; node; node=node->NextSibling() ) - { - if ( !node->ToText() ) - { - fprintf( cfile, "\n" ); - } - node->Print( cfile, depth+1 ); - } - fprintf( cfile, "\n" ); - for( i=0; i", value.c_str() ); - } -} - - -void TiXmlElement::CopyTo( TiXmlElement* target ) const -{ - // superclass: - TiXmlNode::CopyTo( target ); - - // Element class: - // Clone the attributes, then clone the children. - const TiXmlAttribute* attribute = 0; - for( attribute = attributeSet.First(); - attribute; - attribute = attribute->Next() ) - { - target->SetAttribute( attribute->Name(), attribute->Value() ); - } - - TiXmlNode* node = 0; - for ( node = firstChild; node; node = node->NextSibling() ) - { - target->LinkEndChild( node->Clone() ); - } -} - -bool TiXmlElement::Accept( TiXmlVisitor* visitor ) const -{ - if ( visitor->VisitEnter( *this, attributeSet.First() ) ) - { - for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) - { - if ( !node->Accept( visitor ) ) - break; - } - } - return visitor->VisitExit( *this ); -} - - -TiXmlNode* TiXmlElement::Clone() const -{ - TiXmlElement* clone = new TiXmlElement( Value() ); - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -const char* TiXmlElement::GetText() const -{ - const TiXmlNode* child = this->FirstChild(); - if ( child ) { - const TiXmlText* childText = child->ToText(); - if ( childText ) { - return childText->Value(); - } - } - return 0; -} - - -TiXmlDocument::TiXmlDocument() : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) -{ - tabsize = 4; - useMicrosoftBOM = false; - ClearError(); -} - -TiXmlDocument::TiXmlDocument( const char * documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) -{ - tabsize = 4; - useMicrosoftBOM = false; - value = documentName; - ClearError(); -} - - -#ifdef TIXML_USE_STL -TiXmlDocument::TiXmlDocument( const std::string& documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) -{ - tabsize = 4; - useMicrosoftBOM = false; - value = documentName; - ClearError(); -} -#endif - - -TiXmlDocument::TiXmlDocument( const TiXmlDocument& copy ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) -{ - copy.CopyTo( this ); -} - - -TiXmlDocument& TiXmlDocument::operator=( const TiXmlDocument& copy ) -{ - Clear(); - copy.CopyTo( this ); - return *this; -} - - -bool TiXmlDocument::LoadFile( TiXmlEncoding encoding ) -{ - return LoadFile( Value(), encoding ); -} - - -bool TiXmlDocument::SaveFile() const -{ - return SaveFile( Value() ); -} - -bool TiXmlDocument::LoadFile( const char* _filename, TiXmlEncoding encoding ) -{ - TIXML_STRING filename( _filename ); - value = filename; - - // reading in binary mode so that tinyxml can normalize the EOL - FILE* file = TiXmlFOpen( value.c_str (), "rb" ); - - if ( file ) - { - bool result = LoadFile( file, encoding ); - fclose( file ); - return result; - } - else - { - SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); - return false; - } -} - -bool TiXmlDocument::LoadFile( FILE* file, TiXmlEncoding encoding ) -{ - if ( !file ) - { - SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); - return false; - } - - // Delete the existing data: - Clear(); - location.Clear(); - - // Get the file size, so we can pre-allocate the string. HUGE speed impact. - long length = 0; - fseek( file, 0, SEEK_END ); - length = ftell( file ); - fseek( file, 0, SEEK_SET ); - - // Strange case, but good to handle up front. - if ( length <= 0 ) - { - SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return false; - } - - // Subtle bug here. TinyXml did use fgets. But from the XML spec: - // 2.11 End-of-Line Handling - // - // - // ...the XML processor MUST behave as if it normalized all line breaks in external - // parsed entities (including the document entity) on input, before parsing, by translating - // both the two-character sequence #xD #xA and any #xD that is not followed by #xA to - // a single #xA character. - // - // - // It is not clear fgets does that, and certainly isn't clear it works cross platform. - // Generally, you expect fgets to translate from the convention of the OS to the c/unix - // convention, and not work generally. - - /* - while( fgets( buf, sizeof(buf), file ) ) - { - data += buf; - } - */ - - char* buf = new char[ length+1 ]; - buf[0] = 0; - - if ( fread( buf, length, 1, file ) != 1 ) { - delete [] buf; - SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); - return false; - } - - // Process the buffer in place to normalize new lines. (See comment above.) - // Copies from the 'p' to 'q' pointer, where p can advance faster if - // a newline-carriage return is hit. - // - // Wikipedia: - // Systems based on ASCII or a compatible character set use either LF (Line feed, '\n', 0x0A, 10 in decimal) or - // CR (Carriage return, '\r', 0x0D, 13 in decimal) individually, or CR followed by LF (CR+LF, 0x0D 0x0A)... - // * LF: Multics, Unix and Unix-like systems (GNU/Linux, AIX, Xenix, Mac OS X, FreeBSD, etc.), BeOS, Amiga, RISC OS, and others - // * CR+LF: DEC RT-11 and most other early non-Unix, non-IBM OSes, CP/M, MP/M, DOS, OS/2, Microsoft Windows, Symbian OS - // * CR: Commodore 8-bit machines, Apple II family, Mac OS up to version 9 and OS-9 - - const char* p = buf; // the read head - char* q = buf; // the write head - const char CR = 0x0d; - const char LF = 0x0a; - - buf[length] = 0; - while( *p ) { - assert( p < (buf+length) ); - assert( q <= (buf+length) ); - assert( q <= p ); - - if ( *p == CR ) { - *q++ = LF; - p++; - if ( *p == LF ) { // check for CR+LF (and skip LF) - p++; - } - } - else { - *q++ = *p++; - } - } - assert( q <= (buf+length) ); - *q = 0; - - Parse( buf, 0, encoding ); - - delete [] buf; - return !Error(); -} - - -bool TiXmlDocument::SaveFile( const char * filename ) const -{ - // The old c stuff lives on... - FILE* fp = TiXmlFOpen( filename, "w" ); - if ( fp ) - { - bool result = SaveFile( fp ); - fclose( fp ); - return result; - } - return false; -} - - -bool TiXmlDocument::SaveFile( FILE* fp ) const -{ - if ( useMicrosoftBOM ) - { - const unsigned char TIXML_UTF_LEAD_0 = 0xefU; - const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; - const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; - - fputc( TIXML_UTF_LEAD_0, fp ); - fputc( TIXML_UTF_LEAD_1, fp ); - fputc( TIXML_UTF_LEAD_2, fp ); - } - Print( fp, 0 ); - return (ferror(fp) == 0); -} - - -void TiXmlDocument::CopyTo( TiXmlDocument* target ) const -{ - TiXmlNode::CopyTo( target ); - - target->error = error; - target->errorId = errorId; - target->errorDesc = errorDesc; - target->tabsize = tabsize; - target->errorLocation = errorLocation; - target->useMicrosoftBOM = useMicrosoftBOM; - - TiXmlNode* node = 0; - for ( node = firstChild; node; node = node->NextSibling() ) - { - target->LinkEndChild( node->Clone() ); - } -} - - -TiXmlNode* TiXmlDocument::Clone() const -{ - TiXmlDocument* clone = new TiXmlDocument(); - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -void TiXmlDocument::Print( FILE* cfile, int depth ) const -{ - assert( cfile ); - for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) - { - node->Print( cfile, depth ); - fprintf( cfile, "\n" ); - } -} - - -bool TiXmlDocument::Accept( TiXmlVisitor* visitor ) const -{ - if ( visitor->VisitEnter( *this ) ) - { - for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) - { - if ( !node->Accept( visitor ) ) - break; - } - } - return visitor->VisitExit( *this ); -} - - -const TiXmlAttribute* TiXmlAttribute::Next() const -{ - // We are using knowledge of the sentinel. The sentinel - // have a value or name. - if ( next->value.empty() && next->name.empty() ) - return 0; - return next; -} - -/* -TiXmlAttribute* TiXmlAttribute::Next() -{ - // We are using knowledge of the sentinel. The sentinel - // have a value or name. - if ( next->value.empty() && next->name.empty() ) - return 0; - return next; -} -*/ - -const TiXmlAttribute* TiXmlAttribute::Previous() const -{ - // We are using knowledge of the sentinel. The sentinel - // have a value or name. - if ( prev->value.empty() && prev->name.empty() ) - return 0; - return prev; -} - -/* -TiXmlAttribute* TiXmlAttribute::Previous() -{ - // We are using knowledge of the sentinel. The sentinel - // have a value or name. - if ( prev->value.empty() && prev->name.empty() ) - return 0; - return prev; -} -*/ - -void TiXmlAttribute::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const -{ - TIXML_STRING n, v; - - EncodeString( name, &n ); - EncodeString( value, &v ); - - if (value.find ('\"') == TIXML_STRING::npos) { - if ( cfile ) { - fprintf (cfile, "%s=\"%s\"", n.c_str(), v.c_str() ); - } - if ( str ) { - (*str) += n; (*str) += "=\""; (*str) += v; (*str) += "\""; - } - } - else { - if ( cfile ) { - fprintf (cfile, "%s='%s'", n.c_str(), v.c_str() ); - } - if ( str ) { - (*str) += n; (*str) += "='"; (*str) += v; (*str) += "'"; - } - } -} - - -int TiXmlAttribute::QueryIntValue( int* ival ) const -{ - if ( TIXML_SSCANF( value.c_str(), "%d", ival ) == 1 ) - return TIXML_SUCCESS; - return TIXML_WRONG_TYPE; -} - -int TiXmlAttribute::QueryDoubleValue( double* dval ) const -{ - if ( TIXML_SSCANF( value.c_str(), "%lf", dval ) == 1 ) - return TIXML_SUCCESS; - return TIXML_WRONG_TYPE; -} - -void TiXmlAttribute::SetIntValue( int _value ) -{ - char buf [64]; - #if defined(TIXML_SNPRINTF) - TIXML_SNPRINTF(buf, sizeof(buf), "%d", _value); - #else - sprintf (buf, "%d", _value); - #endif - SetValue (buf); -} - -void TiXmlAttribute::SetDoubleValue( double _value ) -{ - char buf [256]; - #if defined(TIXML_SNPRINTF) - TIXML_SNPRINTF( buf, sizeof(buf), "%g", _value); - #else - sprintf (buf, "%g", _value); - #endif - SetValue (buf); -} - -int TiXmlAttribute::IntValue() const -{ - return atoi (value.c_str ()); -} - -double TiXmlAttribute::DoubleValue() const -{ - return atof (value.c_str ()); -} - - -TiXmlComment::TiXmlComment( const TiXmlComment& copy ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) -{ - copy.CopyTo( this ); -} - - -TiXmlComment& TiXmlComment::operator=( const TiXmlComment& base ) -{ - Clear(); - base.CopyTo( this ); - return *this; -} - - -void TiXmlComment::Print( FILE* cfile, int depth ) const -{ - assert( cfile ); - for ( int i=0; i", value.c_str() ); -} - - -void TiXmlComment::CopyTo( TiXmlComment* target ) const -{ - TiXmlNode::CopyTo( target ); -} - - -bool TiXmlComment::Accept( TiXmlVisitor* visitor ) const -{ - return visitor->Visit( *this ); -} - - -TiXmlNode* TiXmlComment::Clone() const -{ - TiXmlComment* clone = new TiXmlComment(); - - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -void TiXmlText::Print( FILE* cfile, int depth ) const -{ - assert( cfile ); - if ( cdata ) - { - int i; - fprintf( cfile, "\n" ); - for ( i=0; i\n", value.c_str() ); // unformatted output - } - else - { - TIXML_STRING buffer; - EncodeString( value, &buffer ); - fprintf( cfile, "%s", buffer.c_str() ); - } -} - - -void TiXmlText::CopyTo( TiXmlText* target ) const -{ - TiXmlNode::CopyTo( target ); - target->cdata = cdata; -} - - -bool TiXmlText::Accept( TiXmlVisitor* visitor ) const -{ - return visitor->Visit( *this ); -} - - -TiXmlNode* TiXmlText::Clone() const -{ - TiXmlText* clone = 0; - clone = new TiXmlText( "" ); - - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -TiXmlDeclaration::TiXmlDeclaration( const char * _version, - const char * _encoding, - const char * _standalone ) - : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) -{ - version = _version; - encoding = _encoding; - standalone = _standalone; -} - - -#ifdef TIXML_USE_STL -TiXmlDeclaration::TiXmlDeclaration( const std::string& _version, - const std::string& _encoding, - const std::string& _standalone ) - : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) -{ - version = _version; - encoding = _encoding; - standalone = _standalone; -} -#endif - - -TiXmlDeclaration::TiXmlDeclaration( const TiXmlDeclaration& copy ) - : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) -{ - copy.CopyTo( this ); -} - - -TiXmlDeclaration& TiXmlDeclaration::operator=( const TiXmlDeclaration& copy ) -{ - Clear(); - copy.CopyTo( this ); - return *this; -} - - -void TiXmlDeclaration::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const -{ - if ( cfile ) fprintf( cfile, "" ); - if ( str ) (*str) += "?>"; -} - - -void TiXmlDeclaration::CopyTo( TiXmlDeclaration* target ) const -{ - TiXmlNode::CopyTo( target ); - - target->version = version; - target->encoding = encoding; - target->standalone = standalone; -} - - -bool TiXmlDeclaration::Accept( TiXmlVisitor* visitor ) const -{ - return visitor->Visit( *this ); -} - - -TiXmlNode* TiXmlDeclaration::Clone() const -{ - TiXmlDeclaration* clone = new TiXmlDeclaration(); - - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -void TiXmlUnknown::Print( FILE* cfile, int depth ) const -{ - for ( int i=0; i", value.c_str() ); -} - - -void TiXmlUnknown::CopyTo( TiXmlUnknown* target ) const -{ - TiXmlNode::CopyTo( target ); -} - - -bool TiXmlUnknown::Accept( TiXmlVisitor* visitor ) const -{ - return visitor->Visit( *this ); -} - - -TiXmlNode* TiXmlUnknown::Clone() const -{ - TiXmlUnknown* clone = new TiXmlUnknown(); - - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -TiXmlAttributeSet::TiXmlAttributeSet() -{ - sentinel.next = &sentinel; - sentinel.prev = &sentinel; -} - - -TiXmlAttributeSet::~TiXmlAttributeSet() -{ - assert( sentinel.next == &sentinel ); - assert( sentinel.prev == &sentinel ); -} - - -void TiXmlAttributeSet::Add( TiXmlAttribute* addMe ) -{ - #ifdef TIXML_USE_STL - assert( !Find( TIXML_STRING( addMe->Name() ) ) ); // Shouldn't be multiply adding to the set. - #else - assert( !Find( addMe->Name() ) ); // Shouldn't be multiply adding to the set. - #endif - - addMe->next = &sentinel; - addMe->prev = sentinel.prev; - - sentinel.prev->next = addMe; - sentinel.prev = addMe; -} - -void TiXmlAttributeSet::Remove( TiXmlAttribute* removeMe ) -{ - TiXmlAttribute* node; - - for( node = sentinel.next; node != &sentinel; node = node->next ) - { - if ( node == removeMe ) - { - node->prev->next = node->next; - node->next->prev = node->prev; - node->next = 0; - node->prev = 0; - return; - } - } - assert( 0 ); // we tried to remove a non-linked attribute. -} - - -#ifdef TIXML_USE_STL -TiXmlAttribute* TiXmlAttributeSet::Find( const std::string& name ) const -{ - for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next ) - { - if ( node->name == name ) - return node; - } - return 0; -} - -TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const std::string& _name ) -{ - TiXmlAttribute* attrib = Find( _name ); - if ( !attrib ) { - attrib = new TiXmlAttribute(); - Add( attrib ); - attrib->SetName( _name ); - } - return attrib; -} -#endif - - -TiXmlAttribute* TiXmlAttributeSet::Find( const char* name ) const -{ - for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next ) - { - if ( strcmp( node->name.c_str(), name ) == 0 ) - return node; - } - return 0; -} - - -TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const char* _name ) -{ - TiXmlAttribute* attrib = Find( _name ); - if ( !attrib ) { - attrib = new TiXmlAttribute(); - Add( attrib ); - attrib->SetName( _name ); - } - return attrib; -} - - -#ifdef TIXML_USE_STL -std::istream& operator>> (std::istream & in, TiXmlNode & base) -{ - TIXML_STRING tag; - tag.reserve( 8 * 1000 ); - base.StreamIn( &in, &tag ); - - base.Parse( tag.c_str(), 0, TIXML_DEFAULT_ENCODING ); - return in; -} -#endif - - -#ifdef TIXML_USE_STL -std::ostream& operator<< (std::ostream & out, const TiXmlNode & base) -{ - TiXmlPrinter printer; - printer.SetStreamPrinting(); - base.Accept( &printer ); - out << printer.Str(); - - return out; -} - - -std::string& operator<< (std::string& out, const TiXmlNode& base ) -{ - TiXmlPrinter printer; - printer.SetStreamPrinting(); - base.Accept( &printer ); - out.append( printer.Str() ); - - return out; -} -#endif - - -TiXmlHandle TiXmlHandle::FirstChild() const -{ - if ( node ) - { - TiXmlNode* child = node->FirstChild(); - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::FirstChild( const char * value ) const -{ - if ( node ) - { - TiXmlNode* child = node->FirstChild( value ); - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::FirstChildElement() const -{ - if ( node ) - { - TiXmlElement* child = node->FirstChildElement(); - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::FirstChildElement( const char * value ) const -{ - if ( node ) - { - TiXmlElement* child = node->FirstChildElement( value ); - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::Child( int count ) const -{ - if ( node ) - { - int i; - TiXmlNode* child = node->FirstChild(); - for ( i=0; - child && iNextSibling(), ++i ) - { - // nothing - } - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::Child( const char* value, int count ) const -{ - if ( node ) - { - int i; - TiXmlNode* child = node->FirstChild( value ); - for ( i=0; - child && iNextSibling( value ), ++i ) - { - // nothing - } - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::ChildElement( int count ) const -{ - if ( node ) - { - int i; - TiXmlElement* child = node->FirstChildElement(); - for ( i=0; - child && iNextSiblingElement(), ++i ) - { - // nothing - } - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::ChildElement( const char* value, int count ) const -{ - if ( node ) - { - int i; - TiXmlElement* child = node->FirstChildElement( value ); - for ( i=0; - child && iNextSiblingElement( value ), ++i ) - { - // nothing - } - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -bool TiXmlPrinter::VisitEnter( const TiXmlDocument& ) -{ - return true; -} - -bool TiXmlPrinter::VisitExit( const TiXmlDocument& ) -{ - return true; -} - -bool TiXmlPrinter::VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute ) -{ - DoIndent(); - buffer += "<"; - buffer += element.Value(); - - for( const TiXmlAttribute* attrib = firstAttribute; attrib; attrib = attrib->Next() ) - { - buffer += " "; - attrib->Print( 0, 0, &buffer ); - } - - if ( !element.FirstChild() ) - { - buffer += " />"; - DoLineBreak(); - } - else - { - buffer += ">"; - if ( element.FirstChild()->ToText() - && element.LastChild() == element.FirstChild() - && element.FirstChild()->ToText()->CDATA() == false ) - { - simpleTextPrint = true; - // no DoLineBreak()! - } - else - { - DoLineBreak(); - } - } - ++depth; - return true; -} - - -bool TiXmlPrinter::VisitExit( const TiXmlElement& element ) -{ - --depth; - if ( !element.FirstChild() ) - { - // nothing. - } - else - { - if ( simpleTextPrint ) - { - simpleTextPrint = false; - } - else - { - DoIndent(); - } - buffer += ""; - DoLineBreak(); - } - return true; -} - - -bool TiXmlPrinter::Visit( const TiXmlText& text ) -{ - if ( text.CDATA() ) - { - DoIndent(); - buffer += ""; - DoLineBreak(); - } - else if ( simpleTextPrint ) - { - TIXML_STRING str; - TiXmlBase::EncodeString( text.ValueTStr(), &str ); - buffer += str; - } - else - { - DoIndent(); - TIXML_STRING str; - TiXmlBase::EncodeString( text.ValueTStr(), &str ); - buffer += str; - DoLineBreak(); - } - return true; -} - - -bool TiXmlPrinter::Visit( const TiXmlDeclaration& declaration ) -{ - DoIndent(); - declaration.Print( 0, 0, &buffer ); - DoLineBreak(); - return true; -} - - -bool TiXmlPrinter::Visit( const TiXmlComment& comment ) -{ - DoIndent(); - buffer += ""; - DoLineBreak(); - return true; -} - - -bool TiXmlPrinter::Visit( const TiXmlUnknown& unknown ) -{ - DoIndent(); - buffer += "<"; - buffer += unknown.Value(); - buffer += ">"; - DoLineBreak(); - return true; -} - diff --git a/addons/urdfreader/thirdparty/tinyxml/tinyxml.h b/addons/urdfreader/thirdparty/tinyxml/tinyxml.h deleted file mode 100644 index a3589e5..0000000 --- a/addons/urdfreader/thirdparty/tinyxml/tinyxml.h +++ /dev/null @@ -1,1805 +0,0 @@ -/* -www.sourceforge.net/projects/tinyxml -Original code by Lee Thomason (www.grinninglizard.com) - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any -damages arising from the use of this software. - -Permission is granted to anyone to use this software for any -purpose, including commercial applications, and to alter it and -redistribute it freely, subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must -not claim that you wrote the original software. If you use this -software in a product, an acknowledgment in the product documentation -would be appreciated but is not required. - -2. Altered source versions must be plainly marked as such, and -must not be misrepresented as being the original software. - -3. This notice may not be removed or altered from any source -distribution. -*/ - - -#ifndef TINYXML_INCLUDED -#define TINYXML_INCLUDED - -#ifdef _MSC_VER -#pragma warning( push ) -#pragma warning( disable : 4530 ) -#pragma warning( disable : 4786 ) -#endif - -#include -#include -#include -#include -#include - -// Help out windows: -#if defined( _DEBUG ) && !defined( DEBUG ) -#define DEBUG -#endif - -#ifdef TIXML_USE_STL - #include - #include - #include - #define TIXML_STRING std::string -#else - #include "tinystr.h" - #define TIXML_STRING TiXmlString -#endif - -// Deprecated library function hell. Compilers want to use the -// new safe versions. This probably doesn't fully address the problem, -// but it gets closer. There are too many compilers for me to fully -// test. If you get compilation troubles, undefine TIXML_SAFE -#define TIXML_SAFE - -#ifdef TIXML_SAFE - #if defined(_MSC_VER) && (_MSC_VER >= 1400 ) - // Microsoft visual studio, version 2005 and higher. - #define TIXML_SNPRINTF _snprintf_s - #define TIXML_SSCANF sscanf_s - #elif defined(_MSC_VER) && (_MSC_VER >= 1200 ) - // Microsoft visual studio, version 6 and higher. - //#pragma message( "Using _sn* functions." ) - #define TIXML_SNPRINTF _snprintf - #define TIXML_SSCANF sscanf - #elif defined(__GNUC__) && (__GNUC__ >= 3 ) - // GCC version 3 and higher.s - //#warning( "Using sn* functions." ) - #define TIXML_SNPRINTF snprintf - #define TIXML_SSCANF sscanf - #else - #define TIXML_SNPRINTF snprintf - #define TIXML_SSCANF sscanf - #endif -#endif - -class TiXmlDocument; -class TiXmlElement; -class TiXmlComment; -class TiXmlUnknown; -class TiXmlAttribute; -class TiXmlText; -class TiXmlDeclaration; -class TiXmlParsingData; - -const int TIXML_MAJOR_VERSION = 2; -const int TIXML_MINOR_VERSION = 6; -const int TIXML_PATCH_VERSION = 2; - -/* Internal structure for tracking location of items - in the XML file. -*/ -struct TiXmlCursor -{ - TiXmlCursor() { Clear(); } - void Clear() { row = col = -1; } - - int row; // 0 based. - int col; // 0 based. -}; - - -/** - Implements the interface to the "Visitor pattern" (see the Accept() method.) - If you call the Accept() method, it requires being passed a TiXmlVisitor - class to handle callbacks. For nodes that contain other nodes (Document, Element) - you will get called with a VisitEnter/VisitExit pair. Nodes that are always leaves - are simply called with Visit(). - - If you return 'true' from a Visit method, recursive parsing will continue. If you return - false, no children of this node or its sibilings will be Visited. - - All flavors of Visit methods have a default implementation that returns 'true' (continue - visiting). You need to only override methods that are interesting to you. - - Generally Accept() is called on the TiXmlDocument, although all nodes suppert Visiting. - - You should never change the document from a callback. - - @sa TiXmlNode::Accept() -*/ -class TiXmlVisitor -{ -public: - virtual ~TiXmlVisitor() {} - - /// Visit a document. - virtual bool VisitEnter( const TiXmlDocument& /*doc*/ ) { return true; } - /// Visit a document. - virtual bool VisitExit( const TiXmlDocument& /*doc*/ ) { return true; } - - /// Visit an element. - virtual bool VisitEnter( const TiXmlElement& /*element*/, const TiXmlAttribute* /*firstAttribute*/ ) { return true; } - /// Visit an element. - virtual bool VisitExit( const TiXmlElement& /*element*/ ) { return true; } - - /// Visit a declaration - virtual bool Visit( const TiXmlDeclaration& /*declaration*/ ) { return true; } - /// Visit a text node - virtual bool Visit( const TiXmlText& /*text*/ ) { return true; } - /// Visit a comment node - virtual bool Visit( const TiXmlComment& /*comment*/ ) { return true; } - /// Visit an unknown node - virtual bool Visit( const TiXmlUnknown& /*unknown*/ ) { return true; } -}; - -// Only used by Attribute::Query functions -enum -{ - TIXML_SUCCESS, - TIXML_NO_ATTRIBUTE, - TIXML_WRONG_TYPE -}; - - -// Used by the parsing routines. -enum TiXmlEncoding -{ - TIXML_ENCODING_UNKNOWN, - TIXML_ENCODING_UTF8, - TIXML_ENCODING_LEGACY -}; - -const TiXmlEncoding TIXML_DEFAULT_ENCODING = TIXML_ENCODING_UNKNOWN; - -/** TiXmlBase is a base class for every class in TinyXml. - It does little except to establish that TinyXml classes - can be printed and provide some utility functions. - - In XML, the document and elements can contain - other elements and other types of nodes. - - @verbatim - A Document can contain: Element (container or leaf) - Comment (leaf) - Unknown (leaf) - Declaration( leaf ) - - An Element can contain: Element (container or leaf) - Text (leaf) - Attributes (not on tree) - Comment (leaf) - Unknown (leaf) - - A Decleration contains: Attributes (not on tree) - @endverbatim -*/ -class TiXmlBase -{ - friend class TiXmlNode; - friend class TiXmlElement; - friend class TiXmlDocument; - -public: - TiXmlBase() : userData(0) {} - virtual ~TiXmlBase() {} - - /** All TinyXml classes can print themselves to a filestream - or the string class (TiXmlString in non-STL mode, std::string - in STL mode.) Either or both cfile and str can be null. - - This is a formatted print, and will insert - tabs and newlines. - - (For an unformatted stream, use the << operator.) - */ - virtual void Print( FILE* cfile, int depth ) const = 0; - - /** The world does not agree on whether white space should be kept or - not. In order to make everyone happy, these global, static functions - are provided to set whether or not TinyXml will condense all white space - into a single space or not. The default is to condense. Note changing this - value is not thread safe. - */ - static void SetCondenseWhiteSpace( bool condense ) { condenseWhiteSpace = condense; } - - /// Return the current white space setting. - static bool IsWhiteSpaceCondensed() { return condenseWhiteSpace; } - - /** Return the position, in the original source file, of this node or attribute. - The row and column are 1-based. (That is the first row and first column is - 1,1). If the returns values are 0 or less, then the parser does not have - a row and column value. - - Generally, the row and column value will be set when the TiXmlDocument::Load(), - TiXmlDocument::LoadFile(), or any TiXmlNode::Parse() is called. It will NOT be set - when the DOM was created from operator>>. - - The values reflect the initial load. Once the DOM is modified programmatically - (by adding or changing nodes and attributes) the new values will NOT update to - reflect changes in the document. - - There is a minor performance cost to computing the row and column. Computation - can be disabled if TiXmlDocument::SetTabSize() is called with 0 as the value. - - @sa TiXmlDocument::SetTabSize() - */ - int Row() const { return location.row + 1; } - int Column() const { return location.col + 1; } ///< See Row() - - void SetUserData( void* user ) { userData = user; } ///< Set a pointer to arbitrary user data. - void* GetUserData() { return userData; } ///< Get a pointer to arbitrary user data. - const void* GetUserData() const { return userData; } ///< Get a pointer to arbitrary user data. - - // Table that returs, for a given lead byte, the total number of bytes - // in the UTF-8 sequence. - static const int utf8ByteTable[256]; - - virtual const char* Parse( const char* p, - TiXmlParsingData* data, - TiXmlEncoding encoding /*= TIXML_ENCODING_UNKNOWN */ ) = 0; - - /** Expands entities in a string. Note this should not contian the tag's '<', '>', etc, - or they will be transformed into entities! - */ - static void EncodeString( const TIXML_STRING& str, TIXML_STRING* out ); - - enum - { - TIXML_NO_ERROR = 0, - TIXML_ERROR, - TIXML_ERROR_OPENING_FILE, - TIXML_ERROR_PARSING_ELEMENT, - TIXML_ERROR_FAILED_TO_READ_ELEMENT_NAME, - TIXML_ERROR_READING_ELEMENT_VALUE, - TIXML_ERROR_READING_ATTRIBUTES, - TIXML_ERROR_PARSING_EMPTY, - TIXML_ERROR_READING_END_TAG, - TIXML_ERROR_PARSING_UNKNOWN, - TIXML_ERROR_PARSING_COMMENT, - TIXML_ERROR_PARSING_DECLARATION, - TIXML_ERROR_DOCUMENT_EMPTY, - TIXML_ERROR_EMBEDDED_NULL, - TIXML_ERROR_PARSING_CDATA, - TIXML_ERROR_DOCUMENT_TOP_ONLY, - - TIXML_ERROR_STRING_COUNT - }; - -protected: - - static const char* SkipWhiteSpace( const char*, TiXmlEncoding encoding ); - - inline static bool IsWhiteSpace( char c ) - { - return ( isspace( (unsigned char) c ) || c == '\n' || c == '\r' ); - } - inline static bool IsWhiteSpace( int c ) - { - if ( c < 256 ) - return IsWhiteSpace( (char) c ); - return false; // Again, only truly correct for English/Latin...but usually works. - } - - #ifdef TIXML_USE_STL - static bool StreamWhiteSpace( std::istream * in, TIXML_STRING * tag ); - static bool StreamTo( std::istream * in, int character, TIXML_STRING * tag ); - #endif - - /* Reads an XML name into the string provided. Returns - a pointer just past the last character of the name, - or 0 if the function has an error. - */ - static const char* ReadName( const char* p, TIXML_STRING* name, TiXmlEncoding encoding ); - - /* Reads text. Returns a pointer past the given end tag. - Wickedly complex options, but it keeps the (sensitive) code in one place. - */ - static const char* ReadText( const char* in, // where to start - TIXML_STRING* text, // the string read - bool ignoreWhiteSpace, // whether to keep the white space - const char* endTag, // what ends this text - bool ignoreCase, // whether to ignore case in the end tag - TiXmlEncoding encoding ); // the current encoding - - // If an entity has been found, transform it into a character. - static const char* GetEntity( const char* in, char* value, int* length, TiXmlEncoding encoding ); - - // Get a character, while interpreting entities. - // The length can be from 0 to 4 bytes. - inline static const char* GetChar( const char* p, char* _value, int* length, TiXmlEncoding encoding ) - { - assert( p ); - if ( encoding == TIXML_ENCODING_UTF8 ) - { - *length = utf8ByteTable[ *((const unsigned char*)p) ]; - assert( *length >= 0 && *length < 5 ); - } - else - { - *length = 1; - } - - if ( *length == 1 ) - { - if ( *p == '&' ) - return GetEntity( p, _value, length, encoding ); - *_value = *p; - return p+1; - } - else if ( *length ) - { - //strncpy( _value, p, *length ); // lots of compilers don't like this function (unsafe), - // and the null terminator isn't needed - for( int i=0; p[i] && i<*length; ++i ) { - _value[i] = p[i]; - } - return p + (*length); - } - else - { - // Not valid text. - return 0; - } - } - - // Return true if the next characters in the stream are any of the endTag sequences. - // Ignore case only works for english, and should only be relied on when comparing - // to English words: StringEqual( p, "version", true ) is fine. - static bool StringEqual( const char* p, - const char* endTag, - bool ignoreCase, - TiXmlEncoding encoding ); - - static const char* errorString[ TIXML_ERROR_STRING_COUNT ]; - - TiXmlCursor location; - - /// Field containing a generic user pointer - void* userData; - - // None of these methods are reliable for any language except English. - // Good for approximation, not great for accuracy. - static int IsAlpha( unsigned char anyByte, TiXmlEncoding encoding ); - static int IsAlphaNum( unsigned char anyByte, TiXmlEncoding encoding ); - inline static int ToLower( int v, TiXmlEncoding encoding ) - { - if ( encoding == TIXML_ENCODING_UTF8 ) - { - if ( v < 128 ) return tolower( v ); - return v; - } - else - { - return tolower( v ); - } - } - static void ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ); - -private: - TiXmlBase( const TiXmlBase& ); // not implemented. - void operator=( const TiXmlBase& base ); // not allowed. - - struct Entity - { - const char* str; - unsigned int strLength; - char chr; - }; - enum - { - NUM_ENTITY = 5, - MAX_ENTITY_LENGTH = 6 - - }; - static Entity entity[ NUM_ENTITY ]; - static bool condenseWhiteSpace; -}; - - -/** The parent class for everything in the Document Object Model. - (Except for attributes). - Nodes have siblings, a parent, and children. A node can be - in a document, or stand on its own. The type of a TiXmlNode - can be queried, and it can be cast to its more defined type. -*/ -class TiXmlNode : public TiXmlBase -{ - friend class TiXmlDocument; - friend class TiXmlElement; - -public: - #ifdef TIXML_USE_STL - - /** An input stream operator, for every class. Tolerant of newlines and - formatting, but doesn't expect them. - */ - friend std::istream& operator >> (std::istream& in, TiXmlNode& base); - - /** An output stream operator, for every class. Note that this outputs - without any newlines or formatting, as opposed to Print(), which - includes tabs and new lines. - - The operator<< and operator>> are not completely symmetric. Writing - a node to a stream is very well defined. You'll get a nice stream - of output, without any extra whitespace or newlines. - - But reading is not as well defined. (As it always is.) If you create - a TiXmlElement (for example) and read that from an input stream, - the text needs to define an element or junk will result. This is - true of all input streams, but it's worth keeping in mind. - - A TiXmlDocument will read nodes until it reads a root element, and - all the children of that root element. - */ - friend std::ostream& operator<< (std::ostream& out, const TiXmlNode& base); - - /// Appends the XML node or attribute to a std::string. - friend std::string& operator<< (std::string& out, const TiXmlNode& base ); - - #endif - - /** The types of XML nodes supported by TinyXml. (All the - unsupported types are picked up by UNKNOWN.) - */ - enum NodeType - { - TINYXML_DOCUMENT, - TINYXML_ELEMENT, - TINYXML_COMMENT, - TINYXML_UNKNOWN, - TINYXML_TEXT, - TINYXML_DECLARATION, - TINYXML_TYPECOUNT - }; - - virtual ~TiXmlNode(); - - /** The meaning of 'value' changes for the specific type of - TiXmlNode. - @verbatim - Document: filename of the xml file - Element: name of the element - Comment: the comment text - Unknown: the tag contents - Text: the text string - @endverbatim - - The subclasses will wrap this function. - */ - const char *Value() const { return value.c_str (); } - - #ifdef TIXML_USE_STL - /** Return Value() as a std::string. If you only use STL, - this is more efficient than calling Value(). - Only available in STL mode. - */ - const std::string& ValueStr() const { return value; } - #endif - - const TIXML_STRING& ValueTStr() const { return value; } - - /** Changes the value of the node. Defined as: - @verbatim - Document: filename of the xml file - Element: name of the element - Comment: the comment text - Unknown: the tag contents - Text: the text string - @endverbatim - */ - void SetValue(const char * _value) { value = _value;} - - #ifdef TIXML_USE_STL - /// STL std::string form. - void SetValue( const std::string& _value ) { value = _value; } - #endif - - /// Delete all the children of this node. Does not affect 'this'. - void Clear(); - - /// One step up the DOM. - TiXmlNode* Parent() { return parent; } - const TiXmlNode* Parent() const { return parent; } - - const TiXmlNode* FirstChild() const { return firstChild; } ///< The first child of this node. Will be null if there are no children. - TiXmlNode* FirstChild() { return firstChild; } - const TiXmlNode* FirstChild( const char * value ) const; ///< The first child of this node with the matching 'value'. Will be null if none found. - /// The first child of this node with the matching 'value'. Will be null if none found. - TiXmlNode* FirstChild( const char * _value ) { - // Call through to the const version - safe since nothing is changed. Exiting syntax: cast this to a const (always safe) - // call the method, cast the return back to non-const. - return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->FirstChild( _value )); - } - const TiXmlNode* LastChild() const { return lastChild; } /// The last child of this node. Will be null if there are no children. - TiXmlNode* LastChild() { return lastChild; } - - const TiXmlNode* LastChild( const char * value ) const; /// The last child of this node matching 'value'. Will be null if there are no children. - TiXmlNode* LastChild( const char * _value ) { - return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->LastChild( _value )); - } - - #ifdef TIXML_USE_STL - const TiXmlNode* FirstChild( const std::string& _value ) const { return FirstChild (_value.c_str ()); } ///< STL std::string form. - TiXmlNode* FirstChild( const std::string& _value ) { return FirstChild (_value.c_str ()); } ///< STL std::string form. - const TiXmlNode* LastChild( const std::string& _value ) const { return LastChild (_value.c_str ()); } ///< STL std::string form. - TiXmlNode* LastChild( const std::string& _value ) { return LastChild (_value.c_str ()); } ///< STL std::string form. - #endif - - /** An alternate way to walk the children of a node. - One way to iterate over nodes is: - @verbatim - for( child = parent->FirstChild(); child; child = child->NextSibling() ) - @endverbatim - - IterateChildren does the same thing with the syntax: - @verbatim - child = 0; - while( child = parent->IterateChildren( child ) ) - @endverbatim - - IterateChildren takes the previous child as input and finds - the next one. If the previous child is null, it returns the - first. IterateChildren will return null when done. - */ - const TiXmlNode* IterateChildren( const TiXmlNode* previous ) const; - TiXmlNode* IterateChildren( const TiXmlNode* previous ) { - return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( previous ) ); - } - - /// This flavor of IterateChildren searches for children with a particular 'value' - const TiXmlNode* IterateChildren( const char * value, const TiXmlNode* previous ) const; - TiXmlNode* IterateChildren( const char * _value, const TiXmlNode* previous ) { - return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( _value, previous ) ); - } - - #ifdef TIXML_USE_STL - const TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) const { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form. - TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form. - #endif - - /** Add a new node related to this. Adds a child past the LastChild. - Returns a pointer to the new object or NULL if an error occured. - */ - TiXmlNode* InsertEndChild( const TiXmlNode& addThis ); - - - /** Add a new node related to this. Adds a child past the LastChild. - - NOTE: the node to be added is passed by pointer, and will be - henceforth owned (and deleted) by tinyXml. This method is efficient - and avoids an extra copy, but should be used with care as it - uses a different memory model than the other insert functions. - - @sa InsertEndChild - */ - TiXmlNode* LinkEndChild( TiXmlNode* addThis ); - - /** Add a new node related to this. Adds a child before the specified child. - Returns a pointer to the new object or NULL if an error occured. - */ - TiXmlNode* InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis ); - - /** Add a new node related to this. Adds a child after the specified child. - Returns a pointer to the new object or NULL if an error occured. - */ - TiXmlNode* InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis ); - - /** Replace a child of this node. - Returns a pointer to the new object or NULL if an error occured. - */ - TiXmlNode* ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis ); - - /// Delete a child of this node. - bool RemoveChild( TiXmlNode* removeThis ); - - /// Navigate to a sibling node. - const TiXmlNode* PreviousSibling() const { return prev; } - TiXmlNode* PreviousSibling() { return prev; } - - /// Navigate to a sibling node. - const TiXmlNode* PreviousSibling( const char * ) const; - TiXmlNode* PreviousSibling( const char *_prev ) { - return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->PreviousSibling( _prev ) ); - } - - #ifdef TIXML_USE_STL - const TiXmlNode* PreviousSibling( const std::string& _value ) const { return PreviousSibling (_value.c_str ()); } ///< STL std::string form. - TiXmlNode* PreviousSibling( const std::string& _value ) { return PreviousSibling (_value.c_str ()); } ///< STL std::string form. - const TiXmlNode* NextSibling( const std::string& _value) const { return NextSibling (_value.c_str ()); } ///< STL std::string form. - TiXmlNode* NextSibling( const std::string& _value) { return NextSibling (_value.c_str ()); } ///< STL std::string form. - #endif - - /// Navigate to a sibling node. - const TiXmlNode* NextSibling() const { return next; } - TiXmlNode* NextSibling() { return next; } - - /// Navigate to a sibling node with the given 'value'. - const TiXmlNode* NextSibling( const char * ) const; - TiXmlNode* NextSibling( const char* _next ) { - return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->NextSibling( _next ) ); - } - - /** Convenience function to get through elements. - Calls NextSibling and ToElement. Will skip all non-Element - nodes. Returns 0 if there is not another element. - */ - const TiXmlElement* NextSiblingElement() const; - TiXmlElement* NextSiblingElement() { - return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement() ); - } - - /** Convenience function to get through elements. - Calls NextSibling and ToElement. Will skip all non-Element - nodes. Returns 0 if there is not another element. - */ - const TiXmlElement* NextSiblingElement( const char * ) const; - TiXmlElement* NextSiblingElement( const char *_next ) { - return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement( _next ) ); - } - - #ifdef TIXML_USE_STL - const TiXmlElement* NextSiblingElement( const std::string& _value) const { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form. - TiXmlElement* NextSiblingElement( const std::string& _value) { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form. - #endif - - /// Convenience function to get through elements. - const TiXmlElement* FirstChildElement() const; - TiXmlElement* FirstChildElement() { - return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement() ); - } - - /// Convenience function to get through elements. - const TiXmlElement* FirstChildElement( const char * _value ) const; - TiXmlElement* FirstChildElement( const char * _value ) { - return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement( _value ) ); - } - - #ifdef TIXML_USE_STL - const TiXmlElement* FirstChildElement( const std::string& _value ) const { return FirstChildElement (_value.c_str ()); } ///< STL std::string form. - TiXmlElement* FirstChildElement( const std::string& _value ) { return FirstChildElement (_value.c_str ()); } ///< STL std::string form. - #endif - - /** Query the type (as an enumerated value, above) of this node. - The possible types are: TINYXML_DOCUMENT, TINYXML_ELEMENT, TINYXML_COMMENT, - TINYXML_UNKNOWN, TINYXML_TEXT, and TINYXML_DECLARATION. - */ - int Type() const { return type; } - - /** Return a pointer to the Document this node lives in. - Returns null if not in a document. - */ - const TiXmlDocument* GetDocument() const; - TiXmlDocument* GetDocument() { - return const_cast< TiXmlDocument* >( (const_cast< const TiXmlNode* >(this))->GetDocument() ); - } - - /// Returns true if this node has no children. - bool NoChildren() const { return !firstChild; } - - virtual const TiXmlDocument* ToDocument() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual const TiXmlElement* ToElement() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual const TiXmlComment* ToComment() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual const TiXmlUnknown* ToUnknown() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual const TiXmlText* ToText() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual const TiXmlDeclaration* ToDeclaration() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - - virtual TiXmlDocument* ToDocument() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual TiXmlElement* ToElement() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual TiXmlComment* ToComment() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual TiXmlUnknown* ToUnknown() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual TiXmlText* ToText() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual TiXmlDeclaration* ToDeclaration() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - - /** Create an exact duplicate of this node and return it. The memory must be deleted - by the caller. - */ - virtual TiXmlNode* Clone() const = 0; - - /** Accept a hierchical visit the nodes in the TinyXML DOM. Every node in the - XML tree will be conditionally visited and the host will be called back - via the TiXmlVisitor interface. - - This is essentially a SAX interface for TinyXML. (Note however it doesn't re-parse - the XML for the callbacks, so the performance of TinyXML is unchanged by using this - interface versus any other.) - - The interface has been based on ideas from: - - - http://www.saxproject.org/ - - http://c2.com/cgi/wiki?HierarchicalVisitorPattern - - Which are both good references for "visiting". - - An example of using Accept(): - @verbatim - TiXmlPrinter printer; - tinyxmlDoc.Accept( &printer ); - const char* xmlcstr = printer.CStr(); - @endverbatim - */ - virtual bool Accept( TiXmlVisitor* visitor ) const = 0; - -protected: - TiXmlNode( NodeType _type ); - - // Copy to the allocated object. Shared functionality between Clone, Copy constructor, - // and the assignment operator. - void CopyTo( TiXmlNode* target ) const; - - #ifdef TIXML_USE_STL - // The real work of the input operator. - virtual void StreamIn( std::istream* in, TIXML_STRING* tag ) = 0; - #endif - - // Figure out what is at *p, and parse it. Returns null if it is not an xml node. - TiXmlNode* Identify( const char* start, TiXmlEncoding encoding ); - - TiXmlNode* parent; - NodeType type; - - TiXmlNode* firstChild; - TiXmlNode* lastChild; - - TIXML_STRING value; - - TiXmlNode* prev; - TiXmlNode* next; - -private: - TiXmlNode( const TiXmlNode& ); // not implemented. - void operator=( const TiXmlNode& base ); // not allowed. -}; - - -/** An attribute is a name-value pair. Elements have an arbitrary - number of attributes, each with a unique name. - - @note The attributes are not TiXmlNodes, since they are not - part of the tinyXML document object model. There are other - suggested ways to look at this problem. -*/ -class TiXmlAttribute : public TiXmlBase -{ - friend class TiXmlAttributeSet; - -public: - /// Construct an empty attribute. - TiXmlAttribute() : TiXmlBase() - { - document = 0; - prev = next = 0; - } - - #ifdef TIXML_USE_STL - /// std::string constructor. - TiXmlAttribute( const std::string& _name, const std::string& _value ) - { - name = _name; - value = _value; - document = 0; - prev = next = 0; - } - #endif - - /// Construct an attribute with a name and value. - TiXmlAttribute( const char * _name, const char * _value ) - { - name = _name; - value = _value; - document = 0; - prev = next = 0; - } - - const char* Name() const { return name.c_str(); } ///< Return the name of this attribute. - const char* Value() const { return value.c_str(); } ///< Return the value of this attribute. - #ifdef TIXML_USE_STL - const std::string& ValueStr() const { return value; } ///< Return the value of this attribute. - #endif - int IntValue() const; ///< Return the value of this attribute, converted to an integer. - double DoubleValue() const; ///< Return the value of this attribute, converted to a double. - - // Get the tinyxml string representation - const TIXML_STRING& NameTStr() const { return name; } - - /** QueryIntValue examines the value string. It is an alternative to the - IntValue() method with richer error checking. - If the value is an integer, it is stored in 'value' and - the call returns TIXML_SUCCESS. If it is not - an integer, it returns TIXML_WRONG_TYPE. - - A specialized but useful call. Note that for success it returns 0, - which is the opposite of almost all other TinyXml calls. - */ - int QueryIntValue( int* _value ) const; - /// QueryDoubleValue examines the value string. See QueryIntValue(). - int QueryDoubleValue( double* _value ) const; - - void SetName( const char* _name ) { name = _name; } ///< Set the name of this attribute. - void SetValue( const char* _value ) { value = _value; } ///< Set the value. - - void SetIntValue( int _value ); ///< Set the value from an integer. - void SetDoubleValue( double _value ); ///< Set the value from a double. - - #ifdef TIXML_USE_STL - /// STL std::string form. - void SetName( const std::string& _name ) { name = _name; } - /// STL std::string form. - void SetValue( const std::string& _value ) { value = _value; } - #endif - - /// Get the next sibling attribute in the DOM. Returns null at end. - const TiXmlAttribute* Next() const; - TiXmlAttribute* Next() { - return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Next() ); - } - - /// Get the previous sibling attribute in the DOM. Returns null at beginning. - const TiXmlAttribute* Previous() const; - TiXmlAttribute* Previous() { - return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Previous() ); - } - - bool operator==( const TiXmlAttribute& rhs ) const { return rhs.name == name; } - bool operator<( const TiXmlAttribute& rhs ) const { return name < rhs.name; } - bool operator>( const TiXmlAttribute& rhs ) const { return name > rhs.name; } - - /* Attribute parsing starts: first letter of the name - returns: the next char after the value end quote - */ - virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); - - // Prints this Attribute to a FILE stream. - virtual void Print( FILE* cfile, int depth ) const { - Print( cfile, depth, 0 ); - } - void Print( FILE* cfile, int depth, TIXML_STRING* str ) const; - - // [internal use] - // Set the document pointer so the attribute can report errors. - void SetDocument( TiXmlDocument* doc ) { document = doc; } - -private: - TiXmlAttribute( const TiXmlAttribute& ); // not implemented. - void operator=( const TiXmlAttribute& base ); // not allowed. - - TiXmlDocument* document; // A pointer back to a document, for error reporting. - TIXML_STRING name; - TIXML_STRING value; - TiXmlAttribute* prev; - TiXmlAttribute* next; -}; - - -/* A class used to manage a group of attributes. - It is only used internally, both by the ELEMENT and the DECLARATION. - - The set can be changed transparent to the Element and Declaration - classes that use it, but NOT transparent to the Attribute - which has to implement a next() and previous() method. Which makes - it a bit problematic and prevents the use of STL. - - This version is implemented with circular lists because: - - I like circular lists - - it demonstrates some independence from the (typical) doubly linked list. -*/ -class TiXmlAttributeSet -{ -public: - TiXmlAttributeSet(); - ~TiXmlAttributeSet(); - - void Add( TiXmlAttribute* attribute ); - void Remove( TiXmlAttribute* attribute ); - - const TiXmlAttribute* First() const { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; } - TiXmlAttribute* First() { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; } - const TiXmlAttribute* Last() const { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; } - TiXmlAttribute* Last() { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; } - - TiXmlAttribute* Find( const char* _name ) const; - TiXmlAttribute* FindOrCreate( const char* _name ); - -# ifdef TIXML_USE_STL - TiXmlAttribute* Find( const std::string& _name ) const; - TiXmlAttribute* FindOrCreate( const std::string& _name ); -# endif - - -private: - //*ME: Because of hidden/disabled copy-construktor in TiXmlAttribute (sentinel-element), - //*ME: this class must be also use a hidden/disabled copy-constructor !!! - TiXmlAttributeSet( const TiXmlAttributeSet& ); // not allowed - void operator=( const TiXmlAttributeSet& ); // not allowed (as TiXmlAttribute) - - TiXmlAttribute sentinel; -}; - - -/** The element is a container class. It has a value, the element name, - and can contain other elements, text, comments, and unknowns. - Elements also contain an arbitrary number of attributes. -*/ -class TiXmlElement : public TiXmlNode -{ -public: - /// Construct an element. - TiXmlElement (const char * in_value); - - #ifdef TIXML_USE_STL - /// std::string constructor. - TiXmlElement( const std::string& _value ); - #endif - - TiXmlElement( const TiXmlElement& ); - - TiXmlElement& operator=( const TiXmlElement& base ); - - virtual ~TiXmlElement(); - - /** Given an attribute name, Attribute() returns the value - for the attribute of that name, or null if none exists. - */ - const char* Attribute( const char* name ) const; - - /** Given an attribute name, Attribute() returns the value - for the attribute of that name, or null if none exists. - If the attribute exists and can be converted to an integer, - the integer value will be put in the return 'i', if 'i' - is non-null. - */ - const char* Attribute( const char* name, int* i ) const; - - /** Given an attribute name, Attribute() returns the value - for the attribute of that name, or null if none exists. - If the attribute exists and can be converted to an double, - the double value will be put in the return 'd', if 'd' - is non-null. - */ - const char* Attribute( const char* name, double* d ) const; - - /** QueryIntAttribute examines the attribute - it is an alternative to the - Attribute() method with richer error checking. - If the attribute is an integer, it is stored in 'value' and - the call returns TIXML_SUCCESS. If it is not - an integer, it returns TIXML_WRONG_TYPE. If the attribute - does not exist, then TIXML_NO_ATTRIBUTE is returned. - */ - int QueryIntAttribute( const char* name, int* _value ) const; - /// QueryUnsignedAttribute examines the attribute - see QueryIntAttribute(). - int QueryUnsignedAttribute( const char* name, unsigned* _value ) const; - /** QueryBoolAttribute examines the attribute - see QueryIntAttribute(). - Note that '1', 'true', or 'yes' are considered true, while '0', 'false' - and 'no' are considered false. - */ - int QueryBoolAttribute( const char* name, bool* _value ) const; - /// QueryDoubleAttribute examines the attribute - see QueryIntAttribute(). - int QueryDoubleAttribute( const char* name, double* _value ) const; - /// QueryFloatAttribute examines the attribute - see QueryIntAttribute(). - int QueryFloatAttribute( const char* name, float* _value ) const { - double d; - int result = QueryDoubleAttribute( name, &d ); - if ( result == TIXML_SUCCESS ) { - *_value = (float)d; - } - return result; - } - - #ifdef TIXML_USE_STL - /// QueryStringAttribute examines the attribute - see QueryIntAttribute(). - int QueryStringAttribute( const char* name, std::string* _value ) const { - const char* cstr = Attribute( name ); - if ( cstr ) { - *_value = std::string( cstr ); - return TIXML_SUCCESS; - } - return TIXML_NO_ATTRIBUTE; - } - - /** Template form of the attribute query which will try to read the - attribute into the specified type. Very easy, very powerful, but - be careful to make sure to call this with the correct type. - - NOTE: This method doesn't work correctly for 'string' types that contain spaces. - - @return TIXML_SUCCESS, TIXML_WRONG_TYPE, or TIXML_NO_ATTRIBUTE - */ - template< typename T > int QueryValueAttribute( const std::string& name, T* outValue ) const - { - const TiXmlAttribute* node = attributeSet.Find( name ); - if ( !node ) - return TIXML_NO_ATTRIBUTE; - - std::stringstream sstream( node->ValueStr() ); - sstream >> *outValue; - if ( !sstream.fail() ) - return TIXML_SUCCESS; - return TIXML_WRONG_TYPE; - } - - int QueryValueAttribute( const std::string& name, std::string* outValue ) const - { - const TiXmlAttribute* node = attributeSet.Find( name ); - if ( !node ) - return TIXML_NO_ATTRIBUTE; - *outValue = node->ValueStr(); - return TIXML_SUCCESS; - } - #endif - - /** Sets an attribute of name to a given value. The attribute - will be created if it does not exist, or changed if it does. - */ - void SetAttribute( const char* name, const char * _value ); - - #ifdef TIXML_USE_STL - const std::string* Attribute( const std::string& name ) const; - const std::string* Attribute( const std::string& name, int* i ) const; - const std::string* Attribute( const std::string& name, double* d ) const; - int QueryIntAttribute( const std::string& name, int* _value ) const; - int QueryDoubleAttribute( const std::string& name, double* _value ) const; - - /// STL std::string form. - void SetAttribute( const std::string& name, const std::string& _value ); - ///< STL std::string form. - void SetAttribute( const std::string& name, int _value ); - ///< STL std::string form. - void SetDoubleAttribute( const std::string& name, double value ); - #endif - - /** Sets an attribute of name to a given value. The attribute - will be created if it does not exist, or changed if it does. - */ - void SetAttribute( const char * name, int value ); - - /** Sets an attribute of name to a given value. The attribute - will be created if it does not exist, or changed if it does. - */ - void SetDoubleAttribute( const char * name, double value ); - - /** Deletes an attribute with the given name. - */ - void RemoveAttribute( const char * name ); - #ifdef TIXML_USE_STL - void RemoveAttribute( const std::string& name ) { RemoveAttribute (name.c_str ()); } ///< STL std::string form. - #endif - - const TiXmlAttribute* FirstAttribute() const { return attributeSet.First(); } ///< Access the first attribute in this element. - TiXmlAttribute* FirstAttribute() { return attributeSet.First(); } - const TiXmlAttribute* LastAttribute() const { return attributeSet.Last(); } ///< Access the last attribute in this element. - TiXmlAttribute* LastAttribute() { return attributeSet.Last(); } - - /** Convenience function for easy access to the text inside an element. Although easy - and concise, GetText() is limited compared to getting the TiXmlText child - and accessing it directly. - - If the first child of 'this' is a TiXmlText, the GetText() - returns the character string of the Text node, else null is returned. - - This is a convenient method for getting the text of simple contained text: - @verbatim - This is text - const char* str = fooElement->GetText(); - @endverbatim - - 'str' will be a pointer to "This is text". - - Note that this function can be misleading. If the element foo was created from - this XML: - @verbatim - This is text - @endverbatim - - then the value of str would be null. The first child node isn't a text node, it is - another element. From this XML: - @verbatim - This is text - @endverbatim - GetText() will return "This is ". - - WARNING: GetText() accesses a child node - don't become confused with the - similarly named TiXmlHandle::Text() and TiXmlNode::ToText() which are - safe type casts on the referenced node. - */ - const char* GetText() const; - - /// Creates a new Element and returns it - the returned element is a copy. - virtual TiXmlNode* Clone() const; - // Print the Element to a FILE stream. - virtual void Print( FILE* cfile, int depth ) const; - - /* Attribtue parsing starts: next char past '<' - returns: next char past '>' - */ - virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); - - virtual const TiXmlElement* ToElement() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - virtual TiXmlElement* ToElement() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - - /** Walk the XML tree visiting this node and all of its children. - */ - virtual bool Accept( TiXmlVisitor* visitor ) const; - -protected: - - void CopyTo( TiXmlElement* target ) const; - void ClearThis(); // like clear, but initializes 'this' object as well - - // Used to be public [internal use] - #ifdef TIXML_USE_STL - virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); - #endif - /* [internal use] - Reads the "value" of the element -- another element, or text. - This should terminate with the current end tag. - */ - const char* ReadValue( const char* in, TiXmlParsingData* prevData, TiXmlEncoding encoding ); - -private: - TiXmlAttributeSet attributeSet; -}; - - -/** An XML comment. -*/ -class TiXmlComment : public TiXmlNode -{ -public: - /// Constructs an empty comment. - TiXmlComment() : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) {} - /// Construct a comment from text. - TiXmlComment( const char* _value ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) { - SetValue( _value ); - } - TiXmlComment( const TiXmlComment& ); - TiXmlComment& operator=( const TiXmlComment& base ); - - virtual ~TiXmlComment() {} - - /// Returns a copy of this Comment. - virtual TiXmlNode* Clone() const; - // Write this Comment to a FILE stream. - virtual void Print( FILE* cfile, int depth ) const; - - /* Attribtue parsing starts: at the ! of the !-- - returns: next char past '>' - */ - virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); - - virtual const TiXmlComment* ToComment() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - virtual TiXmlComment* ToComment() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - - /** Walk the XML tree visiting this node and all of its children. - */ - virtual bool Accept( TiXmlVisitor* visitor ) const; - -protected: - void CopyTo( TiXmlComment* target ) const; - - // used to be public - #ifdef TIXML_USE_STL - virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); - #endif -// virtual void StreamOut( TIXML_OSTREAM * out ) const; - -private: - -}; - - -/** XML text. A text node can have 2 ways to output the next. "normal" output - and CDATA. It will default to the mode it was parsed from the XML file and - you generally want to leave it alone, but you can change the output mode with - SetCDATA() and query it with CDATA(). -*/ -class TiXmlText : public TiXmlNode -{ - friend class TiXmlElement; -public: - /** Constructor for text element. By default, it is treated as - normal, encoded text. If you want it be output as a CDATA text - element, set the parameter _cdata to 'true' - */ - TiXmlText (const char * initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT) - { - SetValue( initValue ); - cdata = false; - } - virtual ~TiXmlText() {} - - #ifdef TIXML_USE_STL - /// Constructor. - TiXmlText( const std::string& initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT) - { - SetValue( initValue ); - cdata = false; - } - #endif - - TiXmlText( const TiXmlText& copy ) : TiXmlNode( TiXmlNode::TINYXML_TEXT ) { copy.CopyTo( this ); } - TiXmlText& operator=( const TiXmlText& base ) { base.CopyTo( this ); return *this; } - - // Write this text object to a FILE stream. - virtual void Print( FILE* cfile, int depth ) const; - - /// Queries whether this represents text using a CDATA section. - bool CDATA() const { return cdata; } - /// Turns on or off a CDATA representation of text. - void SetCDATA( bool _cdata ) { cdata = _cdata; } - - virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); - - virtual const TiXmlText* ToText() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - virtual TiXmlText* ToText() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - - /** Walk the XML tree visiting this node and all of its children. - */ - virtual bool Accept( TiXmlVisitor* content ) const; - -protected : - /// [internal use] Creates a new Element and returns it. - virtual TiXmlNode* Clone() const; - void CopyTo( TiXmlText* target ) const; - - bool Blank() const; // returns true if all white space and new lines - // [internal use] - #ifdef TIXML_USE_STL - virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); - #endif - -private: - bool cdata; // true if this should be input and output as a CDATA style text element -}; - - -/** In correct XML the declaration is the first entry in the file. - @verbatim - - @endverbatim - - TinyXml will happily read or write files without a declaration, - however. There are 3 possible attributes to the declaration: - version, encoding, and standalone. - - Note: In this version of the code, the attributes are - handled as special cases, not generic attributes, simply - because there can only be at most 3 and they are always the same. -*/ -class TiXmlDeclaration : public TiXmlNode -{ -public: - /// Construct an empty declaration. - TiXmlDeclaration() : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) {} - -#ifdef TIXML_USE_STL - /// Constructor. - TiXmlDeclaration( const std::string& _version, - const std::string& _encoding, - const std::string& _standalone ); -#endif - - /// Construct. - TiXmlDeclaration( const char* _version, - const char* _encoding, - const char* _standalone ); - - TiXmlDeclaration( const TiXmlDeclaration& copy ); - TiXmlDeclaration& operator=( const TiXmlDeclaration& copy ); - - virtual ~TiXmlDeclaration() {} - - /// Version. Will return an empty string if none was found. - const char *Version() const { return version.c_str (); } - /// Encoding. Will return an empty string if none was found. - const char *Encoding() const { return encoding.c_str (); } - /// Is this a standalone document? - const char *Standalone() const { return standalone.c_str (); } - - /// Creates a copy of this Declaration and returns it. - virtual TiXmlNode* Clone() const; - // Print this declaration to a FILE stream. - virtual void Print( FILE* cfile, int depth, TIXML_STRING* str ) const; - virtual void Print( FILE* cfile, int depth ) const { - Print( cfile, depth, 0 ); - } - - virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); - - virtual const TiXmlDeclaration* ToDeclaration() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - virtual TiXmlDeclaration* ToDeclaration() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - - /** Walk the XML tree visiting this node and all of its children. - */ - virtual bool Accept( TiXmlVisitor* visitor ) const; - -protected: - void CopyTo( TiXmlDeclaration* target ) const; - // used to be public - #ifdef TIXML_USE_STL - virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); - #endif - -private: - - TIXML_STRING version; - TIXML_STRING encoding; - TIXML_STRING standalone; -}; - - -/** Any tag that tinyXml doesn't recognize is saved as an - unknown. It is a tag of text, but should not be modified. - It will be written back to the XML, unchanged, when the file - is saved. - - DTD tags get thrown into TiXmlUnknowns. -*/ -class TiXmlUnknown : public TiXmlNode -{ -public: - TiXmlUnknown() : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) {} - virtual ~TiXmlUnknown() {} - - TiXmlUnknown( const TiXmlUnknown& copy ) : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) { copy.CopyTo( this ); } - TiXmlUnknown& operator=( const TiXmlUnknown& copy ) { copy.CopyTo( this ); return *this; } - - /// Creates a copy of this Unknown and returns it. - virtual TiXmlNode* Clone() const; - // Print this Unknown to a FILE stream. - virtual void Print( FILE* cfile, int depth ) const; - - virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); - - virtual const TiXmlUnknown* ToUnknown() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - virtual TiXmlUnknown* ToUnknown() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - - /** Walk the XML tree visiting this node and all of its children. - */ - virtual bool Accept( TiXmlVisitor* content ) const; - -protected: - void CopyTo( TiXmlUnknown* target ) const; - - #ifdef TIXML_USE_STL - virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); - #endif - -private: - -}; - - -/** Always the top level node. A document binds together all the - XML pieces. It can be saved, loaded, and printed to the screen. - The 'value' of a document node is the xml file name. -*/ -class TiXmlDocument : public TiXmlNode -{ -public: - /// Create an empty document, that has no name. - TiXmlDocument(); - /// Create a document with a name. The name of the document is also the filename of the xml. - TiXmlDocument( const char * documentName ); - - #ifdef TIXML_USE_STL - /// Constructor. - TiXmlDocument( const std::string& documentName ); - #endif - - TiXmlDocument( const TiXmlDocument& copy ); - TiXmlDocument& operator=( const TiXmlDocument& copy ); - - virtual ~TiXmlDocument() {} - - /** Load a file using the current document value. - Returns true if successful. Will delete any existing - document data before loading. - */ - bool LoadFile( TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); - /// Save a file using the current document value. Returns true if successful. - bool SaveFile() const; - /// Load a file using the given filename. Returns true if successful. - bool LoadFile( const char * filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); - /// Save a file using the given filename. Returns true if successful. - bool SaveFile( const char * filename ) const; - /** Load a file using the given FILE*. Returns true if successful. Note that this method - doesn't stream - the entire object pointed at by the FILE* - will be interpreted as an XML file. TinyXML doesn't stream in XML from the current - file location. Streaming may be added in the future. - */ - bool LoadFile( FILE*, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); - /// Save a file using the given FILE*. Returns true if successful. - bool SaveFile( FILE* ) const; - - #ifdef TIXML_USE_STL - bool LoadFile( const std::string& filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ) ///< STL std::string version. - { - return LoadFile( filename.c_str(), encoding ); - } - bool SaveFile( const std::string& filename ) const ///< STL std::string version. - { - return SaveFile( filename.c_str() ); - } - #endif - - /** Parse the given null terminated block of xml data. Passing in an encoding to this - method (either TIXML_ENCODING_LEGACY or TIXML_ENCODING_UTF8 will force TinyXml - to use that encoding, regardless of what TinyXml might otherwise try to detect. - */ - virtual const char* Parse( const char* p, TiXmlParsingData* data = 0, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); - - /** Get the root element -- the only top level element -- of the document. - In well formed XML, there should only be one. TinyXml is tolerant of - multiple elements at the document level. - */ - const TiXmlElement* RootElement() const { return FirstChildElement(); } - TiXmlElement* RootElement() { return FirstChildElement(); } - - /** If an error occurs, Error will be set to true. Also, - - The ErrorId() will contain the integer identifier of the error (not generally useful) - - The ErrorDesc() method will return the name of the error. (very useful) - - The ErrorRow() and ErrorCol() will return the location of the error (if known) - */ - bool Error() const { return error; } - - /// Contains a textual (english) description of the error if one occurs. - const char * ErrorDesc() const { return errorDesc.c_str (); } - - /** Generally, you probably want the error string ( ErrorDesc() ). But if you - prefer the ErrorId, this function will fetch it. - */ - int ErrorId() const { return errorId; } - - /** Returns the location (if known) of the error. The first column is column 1, - and the first row is row 1. A value of 0 means the row and column wasn't applicable - (memory errors, for example, have no row/column) or the parser lost the error. (An - error in the error reporting, in that case.) - - @sa SetTabSize, Row, Column - */ - int ErrorRow() const { return errorLocation.row+1; } - int ErrorCol() const { return errorLocation.col+1; } ///< The column where the error occured. See ErrorRow() - - /** SetTabSize() allows the error reporting functions (ErrorRow() and ErrorCol()) - to report the correct values for row and column. It does not change the output - or input in any way. - - By calling this method, with a tab size - greater than 0, the row and column of each node and attribute is stored - when the file is loaded. Very useful for tracking the DOM back in to - the source file. - - The tab size is required for calculating the location of nodes. If not - set, the default of 4 is used. The tabsize is set per document. Setting - the tabsize to 0 disables row/column tracking. - - Note that row and column tracking is not supported when using operator>>. - - The tab size needs to be enabled before the parse or load. Correct usage: - @verbatim - TiXmlDocument doc; - doc.SetTabSize( 8 ); - doc.Load( "myfile.xml" ); - @endverbatim - - @sa Row, Column - */ - void SetTabSize( int _tabsize ) { tabsize = _tabsize; } - - int TabSize() const { return tabsize; } - - /** If you have handled the error, it can be reset with this call. The error - state is automatically cleared if you Parse a new XML block. - */ - void ClearError() { error = false; - errorId = 0; - errorDesc = ""; - errorLocation.row = errorLocation.col = 0; - //errorLocation.last = 0; - } - - /** Write the document to standard out using formatted printing ("pretty print"). */ - void Print() const { Print( stdout, 0 ); } - - /* Write the document to a string using formatted printing ("pretty print"). This - will allocate a character array (new char[]) and return it as a pointer. The - calling code pust call delete[] on the return char* to avoid a memory leak. - */ - //char* PrintToMemory() const; - - /// Print this Document to a FILE stream. - virtual void Print( FILE* cfile, int depth = 0 ) const; - // [internal use] - void SetError( int err, const char* errorLocation, TiXmlParsingData* prevData, TiXmlEncoding encoding ); - - virtual const TiXmlDocument* ToDocument() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - virtual TiXmlDocument* ToDocument() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - - /** Walk the XML tree visiting this node and all of its children. - */ - virtual bool Accept( TiXmlVisitor* content ) const; - -protected : - // [internal use] - virtual TiXmlNode* Clone() const; - #ifdef TIXML_USE_STL - virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); - #endif - -private: - void CopyTo( TiXmlDocument* target ) const; - - bool error; - int errorId; - TIXML_STRING errorDesc; - int tabsize; - TiXmlCursor errorLocation; - bool useMicrosoftBOM; // the UTF-8 BOM were found when read. Note this, and try to write. -}; - - -/** - A TiXmlHandle is a class that wraps a node pointer with null checks; this is - an incredibly useful thing. Note that TiXmlHandle is not part of the TinyXml - DOM structure. It is a separate utility class. - - Take an example: - @verbatim - - - - - - - @endverbatim - - Assuming you want the value of "attributeB" in the 2nd "Child" element, it's very - easy to write a *lot* of code that looks like: - - @verbatim - TiXmlElement* root = document.FirstChildElement( "Document" ); - if ( root ) - { - TiXmlElement* element = root->FirstChildElement( "Element" ); - if ( element ) - { - TiXmlElement* child = element->FirstChildElement( "Child" ); - if ( child ) - { - TiXmlElement* child2 = child->NextSiblingElement( "Child" ); - if ( child2 ) - { - // Finally do something useful. - @endverbatim - - And that doesn't even cover "else" cases. TiXmlHandle addresses the verbosity - of such code. A TiXmlHandle checks for null pointers so it is perfectly safe - and correct to use: - - @verbatim - TiXmlHandle docHandle( &document ); - TiXmlElement* child2 = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", 1 ).ToElement(); - if ( child2 ) - { - // do something useful - @endverbatim - - Which is MUCH more concise and useful. - - It is also safe to copy handles - internally they are nothing more than node pointers. - @verbatim - TiXmlHandle handleCopy = handle; - @endverbatim - - What they should not be used for is iteration: - - @verbatim - int i=0; - while ( true ) - { - TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", i ).ToElement(); - if ( !child ) - break; - // do something - ++i; - } - @endverbatim - - It seems reasonable, but it is in fact two embedded while loops. The Child method is - a linear walk to find the element, so this code would iterate much more than it needs - to. Instead, prefer: - - @verbatim - TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).FirstChild( "Child" ).ToElement(); - - for( child; child; child=child->NextSiblingElement() ) - { - // do something - } - @endverbatim -*/ -class TiXmlHandle -{ -public: - /// Create a handle from any node (at any depth of the tree.) This can be a null pointer. - TiXmlHandle( TiXmlNode* _node ) { this->node = _node; } - /// Copy constructor - TiXmlHandle( const TiXmlHandle& ref ) { this->node = ref.node; } - TiXmlHandle operator=( const TiXmlHandle& ref ) { if ( &ref != this ) this->node = ref.node; return *this; } - - /// Return a handle to the first child node. - TiXmlHandle FirstChild() const; - /// Return a handle to the first child node with the given name. - TiXmlHandle FirstChild( const char * value ) const; - /// Return a handle to the first child element. - TiXmlHandle FirstChildElement() const; - /// Return a handle to the first child element with the given name. - TiXmlHandle FirstChildElement( const char * value ) const; - - /** Return a handle to the "index" child with the given name. - The first child is 0, the second 1, etc. - */ - TiXmlHandle Child( const char* value, int index ) const; - /** Return a handle to the "index" child. - The first child is 0, the second 1, etc. - */ - TiXmlHandle Child( int index ) const; - /** Return a handle to the "index" child element with the given name. - The first child element is 0, the second 1, etc. Note that only TiXmlElements - are indexed: other types are not counted. - */ - TiXmlHandle ChildElement( const char* value, int index ) const; - /** Return a handle to the "index" child element. - The first child element is 0, the second 1, etc. Note that only TiXmlElements - are indexed: other types are not counted. - */ - TiXmlHandle ChildElement( int index ) const; - - #ifdef TIXML_USE_STL - TiXmlHandle FirstChild( const std::string& _value ) const { return FirstChild( _value.c_str() ); } - TiXmlHandle FirstChildElement( const std::string& _value ) const { return FirstChildElement( _value.c_str() ); } - - TiXmlHandle Child( const std::string& _value, int index ) const { return Child( _value.c_str(), index ); } - TiXmlHandle ChildElement( const std::string& _value, int index ) const { return ChildElement( _value.c_str(), index ); } - #endif - - /** Return the handle as a TiXmlNode. This may return null. - */ - TiXmlNode* ToNode() const { return node; } - /** Return the handle as a TiXmlElement. This may return null. - */ - TiXmlElement* ToElement() const { return ( ( node && node->ToElement() ) ? node->ToElement() : 0 ); } - /** Return the handle as a TiXmlText. This may return null. - */ - TiXmlText* ToText() const { return ( ( node && node->ToText() ) ? node->ToText() : 0 ); } - /** Return the handle as a TiXmlUnknown. This may return null. - */ - TiXmlUnknown* ToUnknown() const { return ( ( node && node->ToUnknown() ) ? node->ToUnknown() : 0 ); } - - /** @deprecated use ToNode. - Return the handle as a TiXmlNode. This may return null. - */ - TiXmlNode* Node() const { return ToNode(); } - /** @deprecated use ToElement. - Return the handle as a TiXmlElement. This may return null. - */ - TiXmlElement* Element() const { return ToElement(); } - /** @deprecated use ToText() - Return the handle as a TiXmlText. This may return null. - */ - TiXmlText* Text() const { return ToText(); } - /** @deprecated use ToUnknown() - Return the handle as a TiXmlUnknown. This may return null. - */ - TiXmlUnknown* Unknown() const { return ToUnknown(); } - -private: - TiXmlNode* node; -}; - - -/** Print to memory functionality. The TiXmlPrinter is useful when you need to: - - -# Print to memory (especially in non-STL mode) - -# Control formatting (line endings, etc.) - - When constructed, the TiXmlPrinter is in its default "pretty printing" mode. - Before calling Accept() you can call methods to control the printing - of the XML document. After TiXmlNode::Accept() is called, the printed document can - be accessed via the CStr(), Str(), and Size() methods. - - TiXmlPrinter uses the Visitor API. - @verbatim - TiXmlPrinter printer; - printer.SetIndent( "\t" ); - - doc.Accept( &printer ); - fprintf( stdout, "%s", printer.CStr() ); - @endverbatim -*/ -class TiXmlPrinter : public TiXmlVisitor -{ -public: - TiXmlPrinter() : depth( 0 ), simpleTextPrint( false ), - buffer(), indent( " " ), lineBreak( "\n" ) {} - - virtual bool VisitEnter( const TiXmlDocument& doc ); - virtual bool VisitExit( const TiXmlDocument& doc ); - - virtual bool VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute ); - virtual bool VisitExit( const TiXmlElement& element ); - - virtual bool Visit( const TiXmlDeclaration& declaration ); - virtual bool Visit( const TiXmlText& text ); - virtual bool Visit( const TiXmlComment& comment ); - virtual bool Visit( const TiXmlUnknown& unknown ); - - /** Set the indent characters for printing. By default 4 spaces - but tab (\t) is also useful, or null/empty string for no indentation. - */ - void SetIndent( const char* _indent ) { indent = _indent ? _indent : "" ; } - /// Query the indention string. - const char* Indent() { return indent.c_str(); } - /** Set the line breaking string. By default set to newline (\n). - Some operating systems prefer other characters, or can be - set to the null/empty string for no indenation. - */ - void SetLineBreak( const char* _lineBreak ) { lineBreak = _lineBreak ? _lineBreak : ""; } - /// Query the current line breaking string. - const char* LineBreak() { return lineBreak.c_str(); } - - /** Switch over to "stream printing" which is the most dense formatting without - linebreaks. Common when the XML is needed for network transmission. - */ - void SetStreamPrinting() { indent = ""; - lineBreak = ""; - } - /// Return the result. - const char* CStr() { return buffer.c_str(); } - /// Return the length of the result string. - size_t Size() { return buffer.size(); } - - #ifdef TIXML_USE_STL - /// Return the result. - const std::string& Str() { return buffer; } - #endif - -private: - void DoIndent() { - for( int i=0; i -#include - -#include "tinyxml.h" - -//#define DEBUG_PARSER -#if defined( DEBUG_PARSER ) -# if defined( DEBUG ) && defined( _MSC_VER ) -# include -# define TIXML_LOG OutputDebugString -# else -# define TIXML_LOG printf -# endif -#endif - -// Note tha "PutString" hardcodes the same list. This -// is less flexible than it appears. Changing the entries -// or order will break putstring. -TiXmlBase::Entity TiXmlBase::entity[ TiXmlBase::NUM_ENTITY ] = -{ - { "&", 5, '&' }, - { "<", 4, '<' }, - { ">", 4, '>' }, - { """, 6, '\"' }, - { "'", 6, '\'' } -}; - -// Bunch of unicode info at: -// http://www.unicode.org/faq/utf_bom.html -// Including the basic of this table, which determines the #bytes in the -// sequence from the lead byte. 1 placed for invalid sequences -- -// although the result will be junk, pass it through as much as possible. -// Beware of the non-characters in UTF-8: -// ef bb bf (Microsoft "lead bytes") -// ef bf be -// ef bf bf - -const unsigned char TIXML_UTF_LEAD_0 = 0xefU; -const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; -const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; - -const int TiXmlBase::utf8ByteTable[256] = -{ - // 0 1 2 3 4 5 6 7 8 9 a b c d e f - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x00 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x10 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x20 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x30 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x40 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x50 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x60 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x70 End of ASCII range - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x80 0x80 to 0xc1 invalid - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x90 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xa0 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xb0 - 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xc0 0xc2 to 0xdf 2 byte - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xd0 - 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, // 0xe0 0xe0 to 0xef 3 byte - 4, 4, 4, 4, 4, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // 0xf0 0xf0 to 0xf4 4 byte, 0xf5 and higher invalid -}; - - -void TiXmlBase::ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ) -{ - const unsigned long BYTE_MASK = 0xBF; - const unsigned long BYTE_MARK = 0x80; - const unsigned long FIRST_BYTE_MARK[7] = { 0x00, 0x00, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC }; - - if (input < 0x80) - *length = 1; - else if ( input < 0x800 ) - *length = 2; - else if ( input < 0x10000 ) - *length = 3; - else if ( input < 0x200000 ) - *length = 4; - else - { *length = 0; return; } // This code won't covert this correctly anyway. - - output += *length; - - // Scary scary fall throughs. - switch (*length) - { - case 4: - --output; - *output = (char)((input | BYTE_MARK) & BYTE_MASK); - input >>= 6; - case 3: - --output; - *output = (char)((input | BYTE_MARK) & BYTE_MASK); - input >>= 6; - case 2: - --output; - *output = (char)((input | BYTE_MARK) & BYTE_MASK); - input >>= 6; - case 1: - --output; - *output = (char)(input | FIRST_BYTE_MARK[*length]); - } -} - - -/*static*/ int TiXmlBase::IsAlpha( unsigned char anyByte, TiXmlEncoding /*encoding*/ ) -{ - // This will only work for low-ascii, everything else is assumed to be a valid - // letter. I'm not sure this is the best approach, but it is quite tricky trying - // to figure out alhabetical vs. not across encoding. So take a very - // conservative approach. - -// if ( encoding == TIXML_ENCODING_UTF8 ) -// { - if ( anyByte < 127 ) - return isalpha( anyByte ); - else - return 1; // What else to do? The unicode set is huge...get the english ones right. -// } -// else -// { -// return isalpha( anyByte ); -// } -} - - -/*static*/ int TiXmlBase::IsAlphaNum( unsigned char anyByte, TiXmlEncoding /*encoding*/ ) -{ - // This will only work for low-ascii, everything else is assumed to be a valid - // letter. I'm not sure this is the best approach, but it is quite tricky trying - // to figure out alhabetical vs. not across encoding. So take a very - // conservative approach. - -// if ( encoding == TIXML_ENCODING_UTF8 ) -// { - if ( anyByte < 127 ) - return isalnum( anyByte ); - else - return 1; // What else to do? The unicode set is huge...get the english ones right. -// } -// else -// { -// return isalnum( anyByte ); -// } -} - - -class TiXmlParsingData -{ - friend class TiXmlDocument; - public: - void Stamp( const char* now, TiXmlEncoding encoding ); - - const TiXmlCursor& Cursor() const { return cursor; } - - private: - // Only used by the document! - TiXmlParsingData( const char* start, int _tabsize, int row, int col ) - { - assert( start ); - stamp = start; - tabsize = _tabsize; - cursor.row = row; - cursor.col = col; - } - - TiXmlCursor cursor; - const char* stamp; - int tabsize; -}; - - -void TiXmlParsingData::Stamp( const char* now, TiXmlEncoding encoding ) -{ - assert( now ); - - // Do nothing if the tabsize is 0. - if ( tabsize < 1 ) - { - return; - } - - // Get the current row, column. - int row = cursor.row; - int col = cursor.col; - const char* p = stamp; - assert( p ); - - while ( p < now ) - { - // Treat p as unsigned, so we have a happy compiler. - const unsigned char* pU = (const unsigned char*)p; - - // Code contributed by Fletcher Dunn: (modified by lee) - switch (*pU) { - case 0: - // We *should* never get here, but in case we do, don't - // advance past the terminating null character, ever - return; - - case '\r': - // bump down to the next line - ++row; - col = 0; - // Eat the character - ++p; - - // Check for \r\n sequence, and treat this as a single character - if (*p == '\n') { - ++p; - } - break; - - case '\n': - // bump down to the next line - ++row; - col = 0; - - // Eat the character - ++p; - - // Check for \n\r sequence, and treat this as a single - // character. (Yes, this bizarre thing does occur still - // on some arcane platforms...) - if (*p == '\r') { - ++p; - } - break; - - case '\t': - // Eat the character - ++p; - - // Skip to next tab stop - col = (col / tabsize + 1) * tabsize; - break; - - case TIXML_UTF_LEAD_0: - if ( encoding == TIXML_ENCODING_UTF8 ) - { - if ( *(p+1) && *(p+2) ) - { - // In these cases, don't advance the column. These are - // 0-width spaces. - if ( *(pU+1)==TIXML_UTF_LEAD_1 && *(pU+2)==TIXML_UTF_LEAD_2 ) - p += 3; - else if ( *(pU+1)==0xbfU && *(pU+2)==0xbeU ) - p += 3; - else if ( *(pU+1)==0xbfU && *(pU+2)==0xbfU ) - p += 3; - else - { p +=3; ++col; } // A normal character. - } - } - else - { - ++p; - ++col; - } - break; - - default: - if ( encoding == TIXML_ENCODING_UTF8 ) - { - // Eat the 1 to 4 byte utf8 character. - int step = TiXmlBase::utf8ByteTable[*((const unsigned char*)p)]; - if ( step == 0 ) - step = 1; // Error case from bad encoding, but handle gracefully. - p += step; - - // Just advance one column, of course. - ++col; - } - else - { - ++p; - ++col; - } - break; - } - } - cursor.row = row; - cursor.col = col; - assert( cursor.row >= -1 ); - assert( cursor.col >= -1 ); - stamp = p; - assert( stamp ); -} - - -const char* TiXmlBase::SkipWhiteSpace( const char* p, TiXmlEncoding encoding ) -{ - if ( !p || !*p ) - { - return 0; - } - if ( encoding == TIXML_ENCODING_UTF8 ) - { - while ( *p ) - { - const unsigned char* pU = (const unsigned char*)p; - - // Skip the stupid Microsoft UTF-8 Byte order marks - if ( *(pU+0)==TIXML_UTF_LEAD_0 - && *(pU+1)==TIXML_UTF_LEAD_1 - && *(pU+2)==TIXML_UTF_LEAD_2 ) - { - p += 3; - continue; - } - else if(*(pU+0)==TIXML_UTF_LEAD_0 - && *(pU+1)==0xbfU - && *(pU+2)==0xbeU ) - { - p += 3; - continue; - } - else if(*(pU+0)==TIXML_UTF_LEAD_0 - && *(pU+1)==0xbfU - && *(pU+2)==0xbfU ) - { - p += 3; - continue; - } - - if ( IsWhiteSpace( *p ) ) // Still using old rules for white space. - ++p; - else - break; - } - } - else - { - while ( *p && IsWhiteSpace( *p ) ) - ++p; - } - - return p; -} - -#ifdef TIXML_USE_STL -/*static*/ bool TiXmlBase::StreamWhiteSpace( std::istream * in, TIXML_STRING * tag ) -{ - for( ;; ) - { - if ( !in->good() ) return false; - - int c = in->peek(); - // At this scope, we can't get to a document. So fail silently. - if ( !IsWhiteSpace( c ) || c <= 0 ) - return true; - - *tag += (char) in->get(); - } -} - -/*static*/ bool TiXmlBase::StreamTo( std::istream * in, int character, TIXML_STRING * tag ) -{ - //assert( character > 0 && character < 128 ); // else it won't work in utf-8 - while ( in->good() ) - { - int c = in->peek(); - if ( c == character ) - return true; - if ( c <= 0 ) // Silent failure: can't get document at this scope - return false; - - in->get(); - *tag += (char) c; - } - return false; -} -#endif - -// One of TinyXML's more performance demanding functions. Try to keep the memory overhead down. The -// "assign" optimization removes over 10% of the execution time. -// -const char* TiXmlBase::ReadName( const char* p, TIXML_STRING * name, TiXmlEncoding encoding ) -{ - // Oddly, not supported on some comilers, - //name->clear(); - // So use this: - *name = ""; - assert( p ); - - // Names start with letters or underscores. - // Of course, in unicode, tinyxml has no idea what a letter *is*. The - // algorithm is generous. - // - // After that, they can be letters, underscores, numbers, - // hyphens, or colons. (Colons are valid ony for namespaces, - // but tinyxml can't tell namespaces from names.) - if ( p && *p - && ( IsAlpha( (unsigned char) *p, encoding ) || *p == '_' ) ) - { - const char* start = p; - while( p && *p - && ( IsAlphaNum( (unsigned char ) *p, encoding ) - || *p == '_' - || *p == '-' - || *p == '.' - || *p == ':' ) ) - { - //(*name) += *p; // expensive - ++p; - } - if ( p-start > 0 ) { - name->assign( start, p-start ); - } - return p; - } - return 0; -} - -const char* TiXmlBase::GetEntity( const char* p, char* value, int* length, TiXmlEncoding encoding ) -{ - // Presume an entity, and pull it out. - TIXML_STRING ent; - int i; - *length = 0; - - if ( *(p+1) && *(p+1) == '#' && *(p+2) ) - { - unsigned long ucs = 0; - ptrdiff_t delta = 0; - unsigned mult = 1; - - if ( *(p+2) == 'x' ) - { - // Hexadecimal. - if ( !*(p+3) ) return 0; - - const char* q = p+3; - q = strchr( q, ';' ); - - if ( !q || !*q ) return 0; - - delta = q-p; - --q; - - while ( *q != 'x' ) - { - if ( *q >= '0' && *q <= '9' ) - ucs += mult * (*q - '0'); - else if ( *q >= 'a' && *q <= 'f' ) - ucs += mult * (*q - 'a' + 10); - else if ( *q >= 'A' && *q <= 'F' ) - ucs += mult * (*q - 'A' + 10 ); - else - return 0; - mult *= 16; - --q; - } - } - else - { - // Decimal. - if ( !*(p+2) ) return 0; - - const char* q = p+2; - q = strchr( q, ';' ); - - if ( !q || !*q ) return 0; - - delta = q-p; - --q; - - while ( *q != '#' ) - { - if ( *q >= '0' && *q <= '9' ) - ucs += mult * (*q - '0'); - else - return 0; - mult *= 10; - --q; - } - } - if ( encoding == TIXML_ENCODING_UTF8 ) - { - // convert the UCS to UTF-8 - ConvertUTF32ToUTF8( ucs, value, length ); - } - else - { - *value = (char)ucs; - *length = 1; - } - return p + delta + 1; - } - - // Now try to match it. - for( i=0; iappend( cArr, len ); - } - } - else - { - bool whitespace = false; - - // Remove leading white space: - p = SkipWhiteSpace( p, encoding ); - while ( p && *p - && !StringEqual( p, endTag, caseInsensitive, encoding ) ) - { - if ( *p == '\r' || *p == '\n' ) - { - whitespace = true; - ++p; - } - else if ( IsWhiteSpace( *p ) ) - { - whitespace = true; - ++p; - } - else - { - // If we've found whitespace, add it before the - // new character. Any whitespace just becomes a space. - if ( whitespace ) - { - (*text) += ' '; - whitespace = false; - } - int len; - char cArr[4] = { 0, 0, 0, 0 }; - p = GetChar( p, cArr, &len, encoding ); - if ( len == 1 ) - (*text) += cArr[0]; // more efficient - else - text->append( cArr, len ); - } - } - } - if ( p && *p ) - p += strlen( endTag ); - return ( p && *p ) ? p : 0; -} - -#ifdef TIXML_USE_STL - -void TiXmlDocument::StreamIn( std::istream * in, TIXML_STRING * tag ) -{ - // The basic issue with a document is that we don't know what we're - // streaming. Read something presumed to be a tag (and hope), then - // identify it, and call the appropriate stream method on the tag. - // - // This "pre-streaming" will never read the closing ">" so the - // sub-tag can orient itself. - - if ( !StreamTo( in, '<', tag ) ) - { - SetError( TIXML_ERROR_PARSING_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return; - } - - while ( in->good() ) - { - int tagIndex = (int) tag->length(); - while ( in->good() && in->peek() != '>' ) - { - int c = in->get(); - if ( c <= 0 ) - { - SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); - break; - } - (*tag) += (char) c; - } - - if ( in->good() ) - { - // We now have something we presume to be a node of - // some sort. Identify it, and call the node to - // continue streaming. - TiXmlNode* node = Identify( tag->c_str() + tagIndex, TIXML_DEFAULT_ENCODING ); - - if ( node ) - { - node->StreamIn( in, tag ); - bool isElement = node->ToElement() != 0; - delete node; - node = 0; - - // If this is the root element, we're done. Parsing will be - // done by the >> operator. - if ( isElement ) - { - return; - } - } - else - { - SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN ); - return; - } - } - } - // We should have returned sooner. - SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN ); -} - -#endif - -const char* TiXmlDocument::Parse( const char* p, TiXmlParsingData* prevData, TiXmlEncoding encoding ) -{ - ClearError(); - - // Parse away, at the document level. Since a document - // contains nothing but other tags, most of what happens - // here is skipping white space. - if ( !p || !*p ) - { - SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - // Note that, for a document, this needs to come - // before the while space skip, so that parsing - // starts from the pointer we are given. - location.Clear(); - if ( prevData ) - { - location.row = prevData->cursor.row; - location.col = prevData->cursor.col; - } - else - { - location.row = 0; - location.col = 0; - } - TiXmlParsingData data( p, TabSize(), location.row, location.col ); - location = data.Cursor(); - - if ( encoding == TIXML_ENCODING_UNKNOWN ) - { - // Check for the Microsoft UTF-8 lead bytes. - const unsigned char* pU = (const unsigned char*)p; - if ( *(pU+0) && *(pU+0) == TIXML_UTF_LEAD_0 - && *(pU+1) && *(pU+1) == TIXML_UTF_LEAD_1 - && *(pU+2) && *(pU+2) == TIXML_UTF_LEAD_2 ) - { - encoding = TIXML_ENCODING_UTF8; - useMicrosoftBOM = true; - } - } - - p = SkipWhiteSpace( p, encoding ); - if ( !p ) - { - SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - while ( p && *p ) - { - TiXmlNode* node = Identify( p, encoding ); - if ( node ) - { - p = node->Parse( p, &data, encoding ); - LinkEndChild( node ); - } - else - { - break; - } - - // Did we get encoding info? - if ( encoding == TIXML_ENCODING_UNKNOWN - && node->ToDeclaration() ) - { - TiXmlDeclaration* dec = node->ToDeclaration(); - const char* enc = dec->Encoding(); - assert( enc ); - - if ( *enc == 0 ) - encoding = TIXML_ENCODING_UTF8; - else if ( StringEqual( enc, "UTF-8", true, TIXML_ENCODING_UNKNOWN ) ) - encoding = TIXML_ENCODING_UTF8; - else if ( StringEqual( enc, "UTF8", true, TIXML_ENCODING_UNKNOWN ) ) - encoding = TIXML_ENCODING_UTF8; // incorrect, but be nice - else - encoding = TIXML_ENCODING_LEGACY; - } - - p = SkipWhiteSpace( p, encoding ); - } - - // Was this empty? - if ( !firstChild ) { - SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, encoding ); - return 0; - } - - // All is well. - return p; -} - -void TiXmlDocument::SetError( int err, const char* pError, TiXmlParsingData* data, TiXmlEncoding encoding ) -{ - // The first error in a chain is more accurate - don't set again! - if ( error ) - return; - - assert( err > 0 && err < TIXML_ERROR_STRING_COUNT ); - error = true; - errorId = err; - errorDesc = errorString[ errorId ]; - - errorLocation.Clear(); - if ( pError && data ) - { - data->Stamp( pError, encoding ); - errorLocation = data->Cursor(); - } -} - - -TiXmlNode* TiXmlNode::Identify( const char* p, TiXmlEncoding encoding ) -{ - TiXmlNode* returnNode = 0; - - p = SkipWhiteSpace( p, encoding ); - if( !p || !*p || *p != '<' ) - { - return 0; - } - - p = SkipWhiteSpace( p, encoding ); - - if ( !p || !*p ) - { - return 0; - } - - // What is this thing? - // - Elements start with a letter or underscore, but xml is reserved. - // - Comments: "; - - if ( !StringEqual( p, startTag, false, encoding ) ) - { - if ( document ) - document->SetError( TIXML_ERROR_PARSING_COMMENT, p, data, encoding ); - return 0; - } - p += strlen( startTag ); - - // [ 1475201 ] TinyXML parses entities in comments - // Oops - ReadText doesn't work, because we don't want to parse the entities. - // p = ReadText( p, &value, false, endTag, false, encoding ); - // - // from the XML spec: - /* - [Definition: Comments may appear anywhere in a document outside other markup; in addition, - they may appear within the document type declaration at places allowed by the grammar. - They are not part of the document's character data; an XML processor MAY, but need not, - make it possible for an application to retrieve the text of comments. For compatibility, - the string "--" (double-hyphen) MUST NOT occur within comments.] Parameter entity - references MUST NOT be recognized within comments. - - An example of a comment: - - - */ - - value = ""; - // Keep all the white space. - while ( p && *p && !StringEqual( p, endTag, false, encoding ) ) - { - value.append( p, 1 ); - ++p; - } - if ( p && *p ) - p += strlen( endTag ); - - return p; -} - - -const char* TiXmlAttribute::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ) -{ - p = SkipWhiteSpace( p, encoding ); - if ( !p || !*p ) return 0; - - if ( data ) - { - data->Stamp( p, encoding ); - location = data->Cursor(); - } - // Read the name, the '=' and the value. - const char* pErr = p; - p = ReadName( p, &name, encoding ); - if ( !p || !*p ) - { - if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, pErr, data, encoding ); - return 0; - } - p = SkipWhiteSpace( p, encoding ); - if ( !p || !*p || *p != '=' ) - { - if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); - return 0; - } - - ++p; // skip '=' - p = SkipWhiteSpace( p, encoding ); - if ( !p || !*p ) - { - if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); - return 0; - } - - const char* end; - const char SINGLE_QUOTE = '\''; - const char DOUBLE_QUOTE = '\"'; - - if ( *p == SINGLE_QUOTE ) - { - ++p; - end = "\'"; // single quote in string - p = ReadText( p, &value, false, end, false, encoding ); - } - else if ( *p == DOUBLE_QUOTE ) - { - ++p; - end = "\""; // double quote in string - p = ReadText( p, &value, false, end, false, encoding ); - } - else - { - // All attribute values should be in single or double quotes. - // But this is such a common error that the parser will try - // its best, even without them. - value = ""; - while ( p && *p // existence - && !IsWhiteSpace( *p ) // whitespace - && *p != '/' && *p != '>' ) // tag end - { - if ( *p == SINGLE_QUOTE || *p == DOUBLE_QUOTE ) { - // [ 1451649 ] Attribute values with trailing quotes not handled correctly - // We did not have an opening quote but seem to have a - // closing one. Give up and throw an error. - if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); - return 0; - } - value += *p; - ++p; - } - } - return p; -} - -#ifdef TIXML_USE_STL -void TiXmlText::StreamIn( std::istream * in, TIXML_STRING * tag ) -{ - while ( in->good() ) - { - int c = in->peek(); - if ( !cdata && (c == '<' ) ) - { - return; - } - if ( c <= 0 ) - { - TiXmlDocument* document = GetDocument(); - if ( document ) - document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); - return; - } - - (*tag) += (char) c; - in->get(); // "commits" the peek made above - - if ( cdata && c == '>' && tag->size() >= 3 ) { - size_t len = tag->size(); - if ( (*tag)[len-2] == ']' && (*tag)[len-3] == ']' ) { - // terminator of cdata. - return; - } - } - } -} -#endif - -const char* TiXmlText::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ) -{ - value = ""; - TiXmlDocument* document = GetDocument(); - - if ( data ) - { - data->Stamp( p, encoding ); - location = data->Cursor(); - } - - const char* const startTag = ""; - - if ( cdata || StringEqual( p, startTag, false, encoding ) ) - { - cdata = true; - - if ( !StringEqual( p, startTag, false, encoding ) ) - { - if ( document ) - document->SetError( TIXML_ERROR_PARSING_CDATA, p, data, encoding ); - return 0; - } - p += strlen( startTag ); - - // Keep all the white space, ignore the encoding, etc. - while ( p && *p - && !StringEqual( p, endTag, false, encoding ) - ) - { - value += *p; - ++p; - } - - TIXML_STRING dummy; - p = ReadText( p, &dummy, false, endTag, false, encoding ); - return p; - } - else - { - bool ignoreWhite = true; - - const char* end = "<"; - p = ReadText( p, &value, ignoreWhite, end, false, encoding ); - if ( p && *p ) - return p-1; // don't truncate the '<' - return 0; - } -} - -#ifdef TIXML_USE_STL -void TiXmlDeclaration::StreamIn( std::istream * in, TIXML_STRING * tag ) -{ - while ( in->good() ) - { - int c = in->get(); - if ( c <= 0 ) - { - TiXmlDocument* document = GetDocument(); - if ( document ) - document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); - return; - } - (*tag) += (char) c; - - if ( c == '>' ) - { - // All is well. - return; - } - } -} -#endif - -const char* TiXmlDeclaration::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding _encoding ) -{ - p = SkipWhiteSpace( p, _encoding ); - // Find the beginning, find the end, and look for - // the stuff in-between. - TiXmlDocument* document = GetDocument(); - if ( !p || !*p || !StringEqual( p, "SetError( TIXML_ERROR_PARSING_DECLARATION, 0, 0, _encoding ); - return 0; - } - if ( data ) - { - data->Stamp( p, _encoding ); - location = data->Cursor(); - } - p += 5; - - version = ""; - encoding = ""; - standalone = ""; - - while ( p && *p ) - { - if ( *p == '>' ) - { - ++p; - return p; - } - - p = SkipWhiteSpace( p, _encoding ); - if ( StringEqual( p, "version", true, _encoding ) ) - { - TiXmlAttribute attrib; - p = attrib.Parse( p, data, _encoding ); - version = attrib.Value(); - } - else if ( StringEqual( p, "encoding", true, _encoding ) ) - { - TiXmlAttribute attrib; - p = attrib.Parse( p, data, _encoding ); - encoding = attrib.Value(); - } - else if ( StringEqual( p, "standalone", true, _encoding ) ) - { - TiXmlAttribute attrib; - p = attrib.Parse( p, data, _encoding ); - standalone = attrib.Value(); - } - else - { - // Read over whatever it is. - while( p && *p && *p != '>' && !IsWhiteSpace( *p ) ) - ++p; - } - } - return 0; -} - -bool TiXmlText::Blank() const -{ - for ( unsigned i=0; i - - The world has many languages - Мир имеет много Ñзыков - el mundo tiene muchos idiomas - 世界有很多语言 - <РуÑÑкий название="name" ценноÑÑ‚ÑŒ="value"><имеет> - <汉语 åå­—="name" 价值="value">世界有很多语言 - "Mëtæl!" - <ä>Umlaut Element - diff --git a/addons/urdfreader/thirdparty/tinyxml/utf8testverify.xml b/addons/urdfreader/thirdparty/tinyxml/utf8testverify.xml deleted file mode 100644 index 91a319d..0000000 --- a/addons/urdfreader/thirdparty/tinyxml/utf8testverify.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - The world has many languages - Мир имеет много Ñзыков - el mundo tiene muchos idiomas - 世界有很多语言 - <РуÑÑкий название="name" ценноÑÑ‚ÑŒ="value"><имеет> - <汉语 åå­—="name" 价值="value">世界有很多语言 - "Mëtæl!" - <ä>Umlaut Element - diff --git a/addons/urdfreader/thirdparty/urdf/boost_replacement/lexical_cast.h b/addons/urdfreader/thirdparty/urdf/boost_replacement/lexical_cast.h deleted file mode 100644 index 4086469..0000000 --- a/addons/urdfreader/thirdparty/urdf/boost_replacement/lexical_cast.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef BOOST_REPLACEMENT_LEXICAL_CAST_H -#define BOOST_REPLACEMENT_LEXICAL_CAST_H - -#include - -namespace boost -{ - - template T lexical_cast(const char* txt) - { - double result = atof(txt); - return result; - }; - - struct bad_lexical_cast - { - const char* what() - { - return ("bad lexical cast\n"); - } - - }; - -} //namespace boost - -#endif - diff --git a/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.cpp b/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.cpp deleted file mode 100644 index ed38915..0000000 --- a/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "printf_console.h" -#include - -#include - - -void logError(const char* msg, const char* arg0, const char* arg1, const char* arg2) -{ - LOG << msg << " " << arg0 << " " << arg1 << " " << arg2 << std::endl; -} - -void logDebug(const char* msg, float v0, float v1) -{ - LOG << msg << " " << v0 << " " << v1 << std::endl; -}; -void logDebug(const char* msg, const char* msg1, const char* arg1) -{ - LOG << msg << " " << msg1 << " " << arg1 << std::endl; -} - -void logInform(const char* msg, const char* arg0) -{ - LOG << msg << " " << arg0 << std::endl; -} -void logWarn(const char* msg,int id, const char* arg0) -{ - LOG << msg << " " << id << " " << arg0 << std::endl; -} diff --git a/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.h b/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.h deleted file mode 100644 index 247aab2..0000000 --- a/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef PRINTF_CONSOLE_H -#define PRINTF_CONSOLE_H - - -void logError(const char* msg="", const char* arg0="", const char* arg1="", const char* arg2=""); -void logDebug(const char* msg, float v0, float v1); -void logDebug(const char* msg, const char* msg1="", const char* arg1=""); -void logInform(const char* msg, const char* arg0=""); -void logWarn(const char* msg,int id, const char* arg0=""); - -#endif - - diff --git a/addons/urdfreader/thirdparty/urdf/boost_replacement/shared_ptr.h b/addons/urdfreader/thirdparty/urdf/boost_replacement/shared_ptr.h deleted file mode 100644 index 5d29732..0000000 --- a/addons/urdfreader/thirdparty/urdf/boost_replacement/shared_ptr.h +++ /dev/null @@ -1,210 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library Maya Plugin -Copyright (c) 2008 Walt Disney Studios - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising -from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must -not claim that you wrote the original software. If you use this -software in a product, an acknowledgment in the product documentation -would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must -not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -Written by: Nicola Candussi - -Modified by Francisco Gochez -Dec 2011 - Added deferencing operator -*/ - -//my_shared_ptr - -#ifndef DYN_SHARED_PTR_H -#define DYN_SHARED_PTR_H - -#define DYN_SHARED_PTR_THREAD_SAFE - - -#include - -#ifdef _WIN32 -#include - -class my_shared_count { -public: - my_shared_count(): m_count(1) { } - ~my_shared_count() { } - - long increment() - { -#ifdef DYN_SHARED_PTR_THREAD_SAFE - return InterlockedIncrement(&m_count); -#else - return ++m_count; -#endif - } - - long decrement() { -#ifdef DYN_SHARED_PTR_THREAD_SAFE - return InterlockedDecrement(&m_count); -#else - return ++m_count; -#endif - } - - long use_count() { return m_count; } - -private: - long m_count; -}; -#else //ifdef WIN32 - - -#include - -class my_shared_count { -public: - my_shared_count(): m_count(1) { -#ifdef DYN_SHARED_PTR_THREAD_SAFE - pthread_mutex_init(&m_mutex, 0); -#endif - } - ~my_shared_count() { -#ifdef DYN_SHARED_PTR_THREAD_SAFE - pthread_mutex_destroy(&m_mutex); -#endif - } - - long increment() - { -#ifdef DYN_SHARED_PTR_THREAD_SAFE - pthread_mutex_lock(&m_mutex); -#endif - long c = ++m_count; -#ifdef DYN_SHARED_PTR_THREAD_SAFE - pthread_mutex_unlock(&m_mutex); -#endif - return c; - } - - long decrement() { -#ifdef DYN_SHARED_PTR_THREAD_SAFE - pthread_mutex_lock(&m_mutex); -#endif - long c = --m_count; -#ifdef DYN_SHARED_PTR_THREAD_SAFE - pthread_mutex_unlock(&m_mutex); -#endif - return c; - } - - long use_count() { return m_count; } - -private: - long m_count; - mutable pthread_mutex_t m_mutex; -}; - -#endif - - -template -class my_shared_ptr -{ -public: - my_shared_ptr(): m_ptr(NULL), m_count(NULL) { } - my_shared_ptr(my_shared_ptr const& other): - m_ptr(other.m_ptr), - m_count(other.m_count) - { - if(other.m_count != NULL) other.m_count->increment(); - } - - template - my_shared_ptr(my_shared_ptr const& other): - m_ptr(other.m_ptr), - m_count(other.m_count) - { - if(other.m_count != NULL) other.m_count->increment(); - } - - my_shared_ptr(T const* other): m_ptr(const_cast(other)), m_count(NULL) - { - if(other != NULL) m_count = new my_shared_count; - } - - ~my_shared_ptr() - { - giveup_ownership(); - } - - void reset(T const* other) - { - if(m_ptr == other) return; - giveup_ownership(); - m_ptr = const_cast(other); - if(other != NULL) m_count = new my_shared_count; - else m_count = NULL; - } - - T* get() { return m_ptr; } - T const* get() const { return m_ptr; } - T* operator->() { return m_ptr; } - T const* operator->() const { return m_ptr; } - operator bool() const { return m_ptr != NULL; } - T& operator*() const - { - assert(m_ptr != 0); - return *m_ptr; - } - - bool operator<(my_shared_ptr const& rhs) const { return m_ptr < rhs.m_ptr; } - - my_shared_ptr& operator=(my_shared_ptr const& other) { - if(m_ptr == other.m_ptr) return *this; - giveup_ownership(); - m_ptr = other.m_ptr; - m_count = other.m_count; - if(other.m_count != NULL) m_count->increment(); - return *this; - } - - template - my_shared_ptr& operator=(my_shared_ptr& other) { - if(m_ptr == other.m_ptr) return *this; - giveup_ownership(); - m_ptr = other.m_ptr; - m_count = other.m_count; - if(other.m_count != NULL) m_count->increment(); - return *this; - } - -protected: - - template friend class my_shared_ptr; - void giveup_ownership() - { - if(m_count != NULL) { - if( m_count->decrement() == 0) { - delete m_ptr; - m_ptr = NULL; - delete m_count; - m_count = NULL; - } - } - } - -protected: - T *m_ptr; - my_shared_count *m_count; - -}; - - -#endif diff --git a/addons/urdfreader/thirdparty/urdf/boost_replacement/string_split.cpp b/addons/urdfreader/thirdparty/urdf/boost_replacement/string_split.cpp deleted file mode 100644 index 6c8a294..0000000 --- a/addons/urdfreader/thirdparty/urdf/boost_replacement/string_split.cpp +++ /dev/null @@ -1,253 +0,0 @@ - - -#include -//#include -#include -#include -#include - -#include "string_split.h" - -namespace boost -{ - void split( std::vector&pieces, const std::string& vector_str, std::vector separators) - { - assert(separators.size()==1); - if (separators.size()==1) - { - char** strArray = str_split(vector_str.c_str(),separators[0].c_str()); - int numSubStr = str_array_len(strArray); - for (int i=0;i is_any_of(const char* seps) - { - std::vector strArray; - - int numSeps = strlen(seps); - for (int i=0;i -#include -#include - -namespace boost -{ - void split( std::vector&pieces, const std::string& vector_str, std::vector separators); - std::vector is_any_of(const char* seps); -}; - -///The string split C code is by Lars Wirzenius -///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char - - -/* Split a string into substrings. Return dynamic array of dynamically - allocated substrings, or NULL if there was an error. Caller is - expected to free the memory, for example with str_array_free. */ -char** str_split(const char* input, const char* sep); - -/* Free a dynamic array of dynamic strings. */ -void str_array_free(char** array); - -/* Return length of a NULL-delimited array of strings. */ -size_t str_array_len(char** array); - -#endif //STRING_SPLIT_H - diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom/LICENSE b/addons/urdfreader/thirdparty/urdf/urdfdom/LICENSE deleted file mode 100644 index e80920e..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom/LICENSE +++ /dev/null @@ -1,15 +0,0 @@ -Software License Agreement (Apache License) - -Copyright 2011 John Hsu - -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom/README.txt b/addons/urdfreader/thirdparty/urdf/urdfdom/README.txt deleted file mode 100644 index 4e3bff6..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom/README.txt +++ /dev/null @@ -1,7 +0,0 @@ -The URDF (U-Robot Description Format) library - provides core data structures and a simple XML parsers - for populating the class data structures from an URDF file. - -For now, the details of the URDF specifications reside on - http://ros.org/wiki/urdf - diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h b/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h deleted file mode 100644 index 336af0f..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h +++ /dev/null @@ -1,63 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#ifndef URDF_PARSER_URDF_PARSER_H -#define URDF_PARSER_URDF_PARSER_H - -#include - -#include -#include "tinyxml/tinyxml.h" - -//#include - -#ifndef M_PI -#define M_PI 3.1415925438 -#endif //M_PI - - -#include - - - - -namespace urdf{ - - my_shared_ptr parseURDF(const std::string &xml_string); - -} - -#endif diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/check_urdf.cpp b/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/check_urdf.cpp deleted file mode 100644 index 67e9eb1..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/check_urdf.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" -#include -#include - -using namespace urdf; - -void printTree(my_shared_ptr link,int level = 0) -{ - level+=2; - int count = 0; - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) - { - if (*child) - { - for(int j=0;jname << std::endl; - // first grandchild - printTree(*child,level); - } - else - { - for(int j=0;jname << " has a null child!" << *child << std::endl; - } - } - -} - - -#define MSTRINGIFY(A) #A - - -const char* urdf_char = MSTRINGIFY( - - - - - - - - - - - - - - - - - - - - -); - - -int main(int argc, char** argv) -{ - - std::string xml_string; - - if (argc < 2){ - std::cerr << "No URDF file name provided, using a dummy test URDF" << std::endl; - - xml_string = std::string(urdf_char); - - } else - { - - - std::fstream xml_file(argv[1], std::fstream::in); - while ( xml_file.good() ) - { - std::string line; - std::getline( xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - } - - my_shared_ptr robot = parseURDF(xml_string); - if (!robot){ - std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; - return -1; - } - std::cout << "robot name is: " << robot->getName() << std::endl; - - // get info from parser - std::cout << "---------- Successfully Parsed XML ---------------" << std::endl; - // get root link - my_shared_ptr root_link=robot->getRoot(); - if (!root_link) return -1; - - std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; - - - // print entire tree - printTree(root_link); - return 0; -} - diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/joint.cpp b/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/joint.cpp deleted file mode 100644 index bde0c5f..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/joint.cpp +++ /dev/null @@ -1,579 +0,0 @@ -/********************************************************************* -* Software Ligcense Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - -#include -#include -#ifdef URDF_USE_BOOST -#include -#else -#include -#endif -#include - -#ifdef URDF_USE_CONSOLE_BRIDGE - #include -#else - #include "urdf/boost_replacement/printf_console.h" -#endif - -#include -#include - -namespace urdf{ - -bool parsePose(Pose &pose, TiXmlElement* xml); - -bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config) -{ - jd.clear(); - - // Get joint damping - const char* damping_str = config->Attribute("damping"); - if (damping_str == NULL){ - logDebug("urdfdom.joint_dynamics: no damping, defaults to 0"); - jd.damping = 0; - } - else - { - try - { - jd.damping = boost::lexical_cast(damping_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("damping value (%s) is not a float: %s",damping_str, e.what()); - return false; - } - } - - // Get joint friction - const char* friction_str = config->Attribute("friction"); - if (friction_str == NULL){ - logDebug("urdfdom.joint_dynamics: no friction, defaults to 0"); - jd.friction = 0; - } - else - { - try - { - jd.friction = boost::lexical_cast(friction_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("friction value (%s) is not a float: %s",friction_str, e.what()); - return false; - } - } - - if (damping_str == NULL && friction_str == NULL) - { - logError("joint dynamics element specified with no damping and no friction"); - return false; - } - else{ - logDebug("urdfdom.joint_dynamics: damping %f and friction %f", jd.damping, jd.friction); - return true; - } -} - -bool parseJointLimits(JointLimits &jl, TiXmlElement* config) -{ - jl.clear(); - - // Get lower joint limit - const char* lower_str = config->Attribute("lower"); - if (lower_str == NULL){ - logDebug("urdfdom.joint_limit: no lower, defaults to 0"); - jl.lower = 0; - } - else - { - try - { - jl.lower = boost::lexical_cast(lower_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("lower value (%s) is not a float: %s", lower_str, e.what()); - return false; - } - } - - // Get upper joint limit - const char* upper_str = config->Attribute("upper"); - if (upper_str == NULL){ - logDebug("urdfdom.joint_limit: no upper, , defaults to 0"); - jl.upper = 0; - } - else - { - try - { - jl.upper = boost::lexical_cast(upper_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("upper value (%s) is not a float: %s",upper_str, e.what()); - return false; - } - } - - // Get joint effort limit - const char* effort_str = config->Attribute("effort"); - if (effort_str == NULL){ - logError("joint limit: no effort"); - return false; - } - else - { - try - { - jl.effort = boost::lexical_cast(effort_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("effort value (%s) is not a float: %s",effort_str, e.what()); - return false; - } - } - - // Get joint velocity limit - const char* velocity_str = config->Attribute("velocity"); - if (velocity_str == NULL){ - logError("joint limit: no velocity"); - return false; - } - else - { - try - { - jl.velocity = boost::lexical_cast(velocity_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("velocity value (%s) is not a float: %s",velocity_str, e.what()); - return false; - } - } - - return true; -} - -bool parseJointSafety(JointSafety &js, TiXmlElement* config) -{ - js.clear(); - - // Get soft_lower_limit joint limit - const char* soft_lower_limit_str = config->Attribute("soft_lower_limit"); - if (soft_lower_limit_str == NULL) - { - logDebug("urdfdom.joint_safety: no soft_lower_limit, using default value"); - js.soft_lower_limit = 0; - } - else - { - try - { - js.soft_lower_limit = boost::lexical_cast(soft_lower_limit_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("soft_lower_limit value (%s) is not a float: %s",soft_lower_limit_str, e.what()); - return false; - } - } - - // Get soft_upper_limit joint limit - const char* soft_upper_limit_str = config->Attribute("soft_upper_limit"); - if (soft_upper_limit_str == NULL) - { - logDebug("urdfdom.joint_safety: no soft_upper_limit, using default value"); - js.soft_upper_limit = 0; - } - else - { - try - { - js.soft_upper_limit = boost::lexical_cast(soft_upper_limit_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("soft_upper_limit value (%s) is not a float: %s",soft_upper_limit_str, e.what()); - return false; - } - } - - // Get k_position_ safety "position" gain - not exactly position gain - const char* k_position_str = config->Attribute("k_position"); - if (k_position_str == NULL) - { - logDebug("urdfdom.joint_safety: no k_position, using default value"); - js.k_position = 0; - } - else - { - try - { - js.k_position = boost::lexical_cast(k_position_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("k_position value (%s) is not a float: %s",k_position_str, e.what()); - return false; - } - } - // Get k_velocity_ safety velocity gain - const char* k_velocity_str = config->Attribute("k_velocity"); - if (k_velocity_str == NULL) - { - logError("joint safety: no k_velocity"); - return false; - } - else - { - try - { - js.k_velocity = boost::lexical_cast(k_velocity_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("k_velocity value (%s) is not a float: %s",k_velocity_str, e.what()); - return false; - } - } - - return true; -} - -bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config) -{ - jc.clear(); - - // Get rising edge position - const char* rising_position_str = config->Attribute("rising"); - if (rising_position_str == NULL) - { - logDebug("urdfdom.joint_calibration: no rising, using default value"); - jc.rising.reset(0); - } - else - { - try - { - jc.rising.reset(new double(boost::lexical_cast(rising_position_str))); - } - catch (boost::bad_lexical_cast &e) - { - logError("risingvalue (%s) is not a float: %s",rising_position_str, e.what()); - return false; - } - } - - // Get falling edge position - const char* falling_position_str = config->Attribute("falling"); - if (falling_position_str == NULL) - { - logDebug("urdfdom.joint_calibration: no falling, using default value"); - jc.falling.reset(0); - } - else - { - try - { - jc.falling.reset(new double(boost::lexical_cast(falling_position_str))); - } - catch (boost::bad_lexical_cast &e) - { - logError("fallingvalue (%s) is not a float: %s",falling_position_str, e.what()); - return false; - } - } - - return true; -} - -bool parseJointMimic(JointMimic &jm, TiXmlElement* config) -{ - jm.clear(); - - // Get name of joint to mimic - const char* joint_name_str = config->Attribute("joint"); - - if (joint_name_str == NULL) - { - logError("joint mimic: no mimic joint specified"); - return false; - } - else - jm.joint_name = joint_name_str; - - // Get mimic multiplier - const char* multiplier_str = config->Attribute("multiplier"); - - if (multiplier_str == NULL) - { - logDebug("urdfdom.joint_mimic: no multiplier, using default value of 1"); - jm.multiplier = 1; - } - else - { - try - { - jm.multiplier = boost::lexical_cast(multiplier_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("multiplier value (%s) is not a float: %s",multiplier_str, e.what()); - return false; - } - } - - - // Get mimic offset - const char* offset_str = config->Attribute("offset"); - if (offset_str == NULL) - { - logDebug("urdfdom.joint_mimic: no offset, using default value of 0"); - jm.offset = 0; - } - else - { - try - { - jm.offset = boost::lexical_cast(offset_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("offset value (%s) is not a float: %s",offset_str, e.what()); - return false; - } - } - - return true; -} - -bool parseJoint(Joint &joint, TiXmlElement* config) -{ - joint.clear(); - - // Get Joint Name - const char *name = config->Attribute("name"); - if (!name) - { - logError("unnamed joint found"); - return false; - } - joint.name = name; - - // Get transform from Parent Link to Joint Frame - TiXmlElement *origin_xml = config->FirstChildElement("origin"); - if (!origin_xml) - { - logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str()); - joint.parent_to_joint_origin_transform.clear(); - } - else - { - if (!parsePose(joint.parent_to_joint_origin_transform, origin_xml)) - { - joint.parent_to_joint_origin_transform.clear(); - logError("Malformed parent origin element for joint [%s]", joint.name.c_str()); - return false; - } - } - - // Get Parent Link - TiXmlElement *parent_xml = config->FirstChildElement("parent"); - if (parent_xml) - { - const char *pname = parent_xml->Attribute("link"); - if (!pname) - { - logInform("no parent link name specified for Joint link [%s]. this might be the root?", joint.name.c_str()); - } - else - { - joint.parent_link_name = std::string(pname); - } - } - - // Get Child Link - TiXmlElement *child_xml = config->FirstChildElement("child"); - if (child_xml) - { - const char *pname = child_xml->Attribute("link"); - if (!pname) - { - logInform("no child link name specified for Joint link [%s].", joint.name.c_str()); - } - else - { - joint.child_link_name = std::string(pname); - } - } - - // Get Joint type - const char* type_char = config->Attribute("type"); - if (!type_char) - { - logError("joint [%s] has no type, check to see if it's a reference.", joint.name.c_str()); - return false; - } - - std::string type_str = type_char; - if (type_str == "planar") - joint.type = Joint::PLANAR; - else if (type_str == "floating") - joint.type = Joint::FLOATING; - else if (type_str == "revolute") - joint.type = Joint::REVOLUTE; - else if (type_str == "continuous") - joint.type = Joint::CONTINUOUS; - else if (type_str == "prismatic") - joint.type = Joint::PRISMATIC; - else if (type_str == "fixed") - joint.type = Joint::FIXED; - else - { - logError("Joint [%s] has no known type [%s]", joint.name.c_str(), type_str.c_str()); - return false; - } - - // Get Joint Axis - if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED) - { - // axis - TiXmlElement *axis_xml = config->FirstChildElement("axis"); - if (!axis_xml){ - logDebug("urdfdom: no axis elemement for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str()); - joint.axis = Vector3(1.0, 0.0, 0.0); - } - else{ - if (axis_xml->Attribute("xyz")){ - try { - joint.axis.init(axis_xml->Attribute("xyz")); - } - catch (ParseError &e) { - joint.axis.clear(); - logError("Malformed axis element for joint [%s]: %s", joint.name.c_str(), e.what()); - return false; - } - } - } - } - - // Get limit - TiXmlElement *limit_xml = config->FirstChildElement("limit"); - if (limit_xml) - { - joint.limits.reset(new JointLimits()); - if (!parseJointLimits(*joint.limits, limit_xml)) - { - logError("Could not parse limit element for joint [%s]", joint.name.c_str()); - joint.limits.reset(0); - return false; - } - } - else if (joint.type == Joint::REVOLUTE) - { - logError("Joint [%s] is of type REVOLUTE but it does not specify limits", joint.name.c_str()); - return false; - } - else if (joint.type == Joint::PRISMATIC) - { - logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str()); - return false; - } - - // Get safety - TiXmlElement *safety_xml = config->FirstChildElement("safety_controller"); - if (safety_xml) - { - joint.safety.reset(new JointSafety()); - if (!parseJointSafety(*joint.safety, safety_xml)) - { - logError("Could not parse safety element for joint [%s]", joint.name.c_str()); - joint.safety.reset(0); - return false; - } - } - - // Get calibration - TiXmlElement *calibration_xml = config->FirstChildElement("calibration"); - if (calibration_xml) - { - joint.calibration.reset(new JointCalibration()); - if (!parseJointCalibration(*joint.calibration, calibration_xml)) - { - logError("Could not parse calibration element for joint [%s]", joint.name.c_str()); - joint.calibration.reset(0); - return false; - } - } - - // Get Joint Mimic - TiXmlElement *mimic_xml = config->FirstChildElement("mimic"); - if (mimic_xml) - { - joint.mimic.reset(new JointMimic()); - if (!parseJointMimic(*joint.mimic, mimic_xml)) - { - logError("Could not parse mimic element for joint [%s]", joint.name.c_str()); - joint.mimic.reset(0); - return false; - } - } - - // Get Dynamics - TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); - if (prop_xml) - { - joint.dynamics.reset(new JointDynamics()); - if (!parseJointDynamics(*joint.dynamics, prop_xml)) - { - logError("Could not parse joint_dynamics element for joint [%s]", joint.name.c_str()); - joint.dynamics.reset(0); - return false; - } - } - - return true; -} - - - - -} diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/link.cpp b/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/link.cpp deleted file mode 100644 index a224146..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/link.cpp +++ /dev/null @@ -1,505 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - - -#include -#include -//#include -//#include -#ifdef URDF_USE_BOOST -#include -#else -#include -#endif - -#include -#include -#ifdef URDF_USE_CONSOLE_BRIDGE -#include -#else -#include "urdf/boost_replacement/printf_console.h" -#endif - - - -namespace urdf{ - -bool parsePose(Pose &pose, TiXmlElement* xml); - -bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok) -{ - bool has_rgb = false; - bool has_filename = false; - - material.clear(); - - if (!config->Attribute("name")) - { - logError("Material must contain a name attribute"); - return false; - } - - material.name = config->Attribute("name"); - - // texture - TiXmlElement *t = config->FirstChildElement("texture"); - if (t) - { - if (t->Attribute("filename")) - { - material.texture_filename = t->Attribute("filename"); - has_filename = true; - } - } - - // color - TiXmlElement *c = config->FirstChildElement("color"); - if (c) - { - if (c->Attribute("rgba")) { - - try { - material.color.init(c->Attribute("rgba")); - has_rgb = true; - } - catch (ParseError &e) { - material.color.clear(); - logError(std::string("Material [" + material.name + "] has malformed color rgba values: " + e.what()).c_str()); - } - } - } - - if (!has_rgb && !has_filename) { - if (!only_name_is_ok) // no need for an error if only name is ok - { - if (!has_rgb) logError(std::string("Material ["+material.name+"] color has no rgba").c_str()); - if (!has_filename) logError(std::string("Material ["+material.name+"] not defined in file").c_str()); - } - return false; - } - return true; -} - - -bool parseSphere(Sphere &s, TiXmlElement *c) -{ - s.clear(); - - s.type = Geometry::SPHERE; - if (!c->Attribute("radius")) - { - logError("Sphere shape must have a radius attribute"); - return false; - } - - try - { - s.radius = boost::lexical_cast(c->Attribute("radius")); - } - catch (boost::bad_lexical_cast &e) - { - // std::stringstream stm; - // stm << "radius [" << c->Attribute("radius") << "] is not a valid float: " << e.what(); - // logError(stm.str().c_str()); - logError("radius issue"); - return false; - } - - return true; -} - -bool parseBox(Box &b, TiXmlElement *c) -{ - b.clear(); - - b.type = Geometry::BOX; - if (!c->Attribute("size")) - { - logError("Box shape has no size attribute"); - return false; - } - try - { - b.dim.init(c->Attribute("size")); - } - catch (ParseError &e) - { - b.dim.clear(); - logError(e.what()); - return false; - } - return true; -} - -bool parseCylinder(Cylinder &y, TiXmlElement *c) -{ - y.clear(); - - y.type = Geometry::CYLINDER; - if (!c->Attribute("length") || - !c->Attribute("radius")) - { - logError("Cylinder shape must have both length and radius attributes"); - return false; - } - - try - { - y.length = boost::lexical_cast(c->Attribute("length")); - } - catch (boost::bad_lexical_cast &e) - { - // std::stringstream stm; - // stm << "length [" << c->Attribute("length") << "] is not a valid float"; - //logError(stm.str().c_str()); - logError("length"); - return false; - } - - try - { - y.radius = boost::lexical_cast(c->Attribute("radius")); - } - catch (boost::bad_lexical_cast &e) - { - // std::stringstream stm; - // stm << "radius [" << c->Attribute("radius") << "] is not a valid float"; - //logError(stm.str().c_str()); - logError("radius"); - return false; - } - return true; -} - - -bool parseMesh(Mesh &m, TiXmlElement *c) -{ - m.clear(); - - m.type = Geometry::MESH; - if (!c->Attribute("filename")) { - logError("Mesh must contain a filename attribute"); - return false; - } - - m.filename = c->Attribute("filename"); - - if (c->Attribute("scale")) { - try { - m.scale.init(c->Attribute("scale")); - } - catch (ParseError &e) { - m.scale.clear(); - logError("Mesh scale was specified, but could not be parsed: %s", e.what()); - return false; - } - } - else - { - m.scale.x = m.scale.y = m.scale.z = 1; - } - return true; -} - -my_shared_ptr parseGeometry(TiXmlElement *g) -{ - my_shared_ptr geom; - if (!g) return geom; - - TiXmlElement *shape = g->FirstChildElement(); - if (!shape) - { - logError("Geometry tag contains no child element."); - return geom; - } - - const std::string type_name = shape->ValueTStr().c_str(); - if (type_name == "sphere") - { - Sphere *s = new Sphere(); - geom.reset(s); - if (parseSphere(*s, shape)) - return geom; - } - else if (type_name == "box") - { - Box *b = new Box(); - geom.reset(b); - if (parseBox(*b, shape)) - return geom; - } - else if (type_name == "cylinder") - { - Cylinder *c = new Cylinder(); - geom.reset(c); - if (parseCylinder(*c, shape)) - return geom; - } - else if (type_name == "mesh") - { - Mesh *m = new Mesh(); - geom.reset(m); - if (parseMesh(*m, shape)) - return geom; - } - else - { - logError("Unknown geometry type '%s'", type_name.c_str()); - return geom; - } - - return my_shared_ptr(); -} - -bool parseInertial(Inertial &i, TiXmlElement *config) -{ - i.clear(); - - // Origin - TiXmlElement *o = config->FirstChildElement("origin"); - if (o) - { - if (!parsePose(i.origin, o)) - return false; - } - - TiXmlElement *mass_xml = config->FirstChildElement("mass"); - if (!mass_xml) - { - logError("Inertial element must have a mass element"); - return false; - } - if (!mass_xml->Attribute("value")) - { - logError("Inertial: mass element must have value attribute"); - return false; - } - - try - { - i.mass = boost::lexical_cast(mass_xml->Attribute("value")); - } - catch (boost::bad_lexical_cast &e) - { - // std::stringstream stm; - // stm << "Inertial: mass [" << mass_xml->Attribute("value") - // << "] is not a float"; - //logError(stm.str().c_str()); - logError("Inertial mass issue"); - return false; - } - - TiXmlElement *inertia_xml = config->FirstChildElement("inertia"); - if (!inertia_xml) - { - logError("Inertial element must have inertia element"); - return false; - } - if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") && - inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") && - inertia_xml->Attribute("izz"))) - { - logError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes"); - return false; - } - try - { - i.ixx = boost::lexical_cast(inertia_xml->Attribute("ixx")); - i.ixy = boost::lexical_cast(inertia_xml->Attribute("ixy")); - i.ixz = boost::lexical_cast(inertia_xml->Attribute("ixz")); - i.iyy = boost::lexical_cast(inertia_xml->Attribute("iyy")); - i.iyz = boost::lexical_cast(inertia_xml->Attribute("iyz")); - i.izz = boost::lexical_cast(inertia_xml->Attribute("izz")); - } - catch (boost::bad_lexical_cast &e) - { - /* std::stringstream stm; - stm << "Inertial: one of the inertia elements is not a valid double:" - << " ixx [" << inertia_xml->Attribute("ixx") << "]" - << " ixy [" << inertia_xml->Attribute("ixy") << "]" - << " ixz [" << inertia_xml->Attribute("ixz") << "]" - << " iyy [" << inertia_xml->Attribute("iyy") << "]" - << " iyz [" << inertia_xml->Attribute("iyz") << "]" - << " izz [" << inertia_xml->Attribute("izz") << "]"; - logError(stm.str().c_str()); - */ - logError("Inertia error"); - - return false; - } - return true; -} - -bool parseVisual(Visual &vis, TiXmlElement *config) -{ - vis.clear(); - - // Origin - TiXmlElement *o = config->FirstChildElement("origin"); - if (o) { - if (!parsePose(vis.origin, o)) - return false; - } - - // Geometry - TiXmlElement *geom = config->FirstChildElement("geometry"); - vis.geometry = parseGeometry(geom); - if (!vis.geometry) - return false; - - const char *name_char = config->Attribute("name"); - if (name_char) - vis.name = name_char; - - // Material - TiXmlElement *mat = config->FirstChildElement("material"); - if (mat) { - // get material name - if (!mat->Attribute("name")) { - logError("Visual material must contain a name attribute"); - return false; - } - vis.material_name = mat->Attribute("name"); - - // try to parse material element in place - vis.material.reset(new Material()); - if (!parseMaterial(*vis.material, mat, true)) - { - logDebug("urdfdom: material has only name, actual material definition may be in the model"); - } - } - - return true; -} - -bool parseCollision(Collision &col, TiXmlElement* config) -{ - col.clear(); - - // Origin - TiXmlElement *o = config->FirstChildElement("origin"); - if (o) { - if (!parsePose(col.origin, o)) - return false; - } - - // Geometry - TiXmlElement *geom = config->FirstChildElement("geometry"); - col.geometry = parseGeometry(geom); - if (!col.geometry) - return false; - - const char *name_char = config->Attribute("name"); - if (name_char) - col.name = name_char; - - return true; -} - -bool parseLink(Link &link, TiXmlElement* config) -{ - - link.clear(); - - const char *name_char = config->Attribute("name"); - if (!name_char) - { - logError("No name given for the link."); - return false; - } - link.name = std::string(name_char); - - // Inertial (optional) - TiXmlElement *i = config->FirstChildElement("inertial"); - if (i) - { - link.inertial.reset(new Inertial()); - if (!parseInertial(*link.inertial, i)) - { - logError("Could not parse inertial element for Link [%s]", link.name.c_str()); - return false; - } - } - - // Multiple Visuals (optional) - for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) - { - - my_shared_ptr vis; - vis.reset(new Visual()); - if (parseVisual(*vis, vis_xml)) - { - link.visual_array.push_back(vis); - } - else - { - vis.reset(0); - logError("Could not parse visual element for Link [%s]", link.name.c_str()); - return false; - } - } - - // Visual (optional) - // Assign the first visual to the .visual ptr, if it exists - if (!link.visual_array.empty()) - link.visual = link.visual_array[0]; - - // Multiple Collisions (optional) - for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) - { - my_shared_ptr col; - col.reset(new Collision()); - if (parseCollision(*col, col_xml)) - { - link.collision_array.push_back(col); - } - else - { - col.reset(0); - logError("Could not parse collision element for Link [%s]", link.name.c_str()); - return false; - } - } - - // Collision (optional) - // Assign the first collision to the .collision ptr, if it exists - if (!link.collision_array.empty()) - link.collision = link.collision_array[0]; - return true; - -} - -} diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/model.cpp b/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/model.cpp deleted file mode 100644 index e8562d0..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/model.cpp +++ /dev/null @@ -1,240 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ -//#include -#include -#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" -#ifdef URDF_USE_CONSOLE_BRIDGE - #include -#else - #include "urdf/boost_replacement/printf_console.h" -#endif - -namespace urdf{ - -bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok); -bool parseLink(Link &link, TiXmlElement *config); -bool parseJoint(Joint &joint, TiXmlElement *config); - - -my_shared_ptr parseURDF(const std::string &xml_string) -{ - my_shared_ptr model(new ModelInterface); - model->clear(); - - TiXmlDocument xml_doc; - xml_doc.Parse(xml_string.c_str()); - if (xml_doc.Error()) - { - logError(xml_doc.ErrorDesc()); - xml_doc.ClearError(); - model.reset(0); - return model; - } - - TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot"); - if (!robot_xml) - { - logError("Could not find the 'robot' element in the xml file"); - model.reset(0); - return model; - } - - // Get robot name - const char *name = robot_xml->Attribute("name"); - if (!name) - { - logError("No name given for the robot."); - model.reset(0); - return model; - } - model->name_ = std::string(name); - - // Get all Material elements - for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) - { - my_shared_ptr material; - material.reset(new Material); - - try { - parseMaterial(*material, material_xml, false); // material needs to be fully defined here - if (model->getMaterial(material->name)) - { - logError("material '%s' is not unique.", material->name.c_str()); - material.reset(0); - model.reset(0); - return model; - } - else - { - model->materials_.insert(make_pair(material->name,material)); - logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str()); - } - } - catch (ParseError &e) { - logError("material xml is not initialized correctly"); - material.reset(0); - model.reset(0); - return model; - } - } - - // Get all Link elements - for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) - { - my_shared_ptr link; - link.reset(new Link); - model->m_numLinks++; - - try { - parseLink(*link, link_xml); - if (model->getLink(link->name)) - { - logError("link '%s' is not unique.", link->name.c_str()); - model.reset(0); - return model; - } - else - { - // set link visual material - logDebug("urdfdom: setting link '%s' material", link->name.c_str()); - if (link->visual) - { - if (!link->visual->material_name.empty()) - { - if (model->getMaterial(link->visual->material_name)) - { - logDebug("urdfdom: setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str()); - link->visual->material = model->getMaterial( link->visual->material_name.c_str() ); - } - else - { - if (link->visual->material) - { - logDebug("urdfdom: link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str()); - model->materials_.insert(make_pair(link->visual->material->name,link->visual->material)); - } - else - { - logError("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str()); - model.reset(0); - return model; - } - } - } - } - - model->links_.insert(make_pair(link->name,link)); - logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str()); - } - } - catch (ParseError &e) { - logError("link xml is not initialized correctly"); - model.reset(0); - return model; - } - } - if (model->links_.empty()){ - logError("No link elements found in urdf file"); - model.reset(0); - return model; - } - - // Get all Joint elements - for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) - { - my_shared_ptr joint; - joint.reset(new Joint); - model->m_numJoints++; - - if (parseJoint(*joint, joint_xml)) - { - if (model->getJoint(joint->name)) - { - logError("joint '%s' is not unique.", joint->name.c_str()); - model.reset(0); - return model; - } - else - { - model->joints_.insert(make_pair(joint->name,joint)); - logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str()); - } - } - else - { - logError("joint xml is not initialized correctly"); - model.reset(0); - return model; - } - } - - - // every link has children links and joints, but no parents, so we create a - // local convenience data structure for keeping child->parent relations - std::map parent_link_tree; - parent_link_tree.clear(); - - // building tree: name mapping - try - { - model->initTree(parent_link_tree); - } - catch(ParseError &e) - { - logError("Failed to build tree: %s", e.what()); - model.reset(0); - return model; - } - - // find the root link - try - { - model->initRoot(parent_link_tree); - } - catch(ParseError &e) - { - logError("Failed to find root link: %s", e.what()); - model.reset(0); - return model; - } - - return model; -} - - - -} - diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/pose.cpp b/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/pose.cpp deleted file mode 100644 index e90247c..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/pose.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen, John Hsu */ - - -#include -#include -#include -//#include -#include - -#ifdef URDF_USE_CONSOLE_BRIDGE -#include -#else -#include "urdf/boost_replacement/printf_console.h" -#endif - -#include -#include - - -namespace urdf{ - -bool parsePose(Pose &pose, TiXmlElement* xml) -{ - pose.clear(); - if (xml) - { - const char* xyz_str = xml->Attribute("xyz"); - if (xyz_str != NULL) - { - try { - pose.position.init(xyz_str); - } - catch (ParseError &e) { - logError(e.what()); - return false; - } - } - - const char* rpy_str = xml->Attribute("rpy"); - if (rpy_str != NULL) - { - try { - pose.rotation.init(rpy_str); - } - catch (ParseError &e) { - logError(e.what()); - return false; - } - } - } - return true; -} - - -} - - diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/twist.cpp b/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/twist.cpp deleted file mode 100644 index 4980e17..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/twist.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - - -#include -#include -#include -#include -#include -#include -#include - -namespace urdf{ - -bool parseTwist(Twist &twist, TiXmlElement* xml) -{ - twist.clear(); - if (xml) - { - const char* linear_char = xml->Attribute("linear"); - if (linear_char != NULL) - { - try { - twist.linear.init(linear_char); - } - catch (ParseError &e) { - twist.linear.clear(); - logError("Malformed linear string [%s]: %s", linear_char, e.what()); - return false; - } - } - - const char* angular_char = xml->Attribute("angular"); - if (angular_char != NULL) - { - try { - twist.angular.init(angular_char); - } - catch (ParseError &e) { - twist.angular.clear(); - logError("Malformed angular [%s]: %s", angular_char, e.what()); - return false; - } - } - } - return true; -} - -} - - - diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp b/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp deleted file mode 100644 index f30b845..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp +++ /dev/null @@ -1,154 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - - -#include -#include -#include -#include -#include -#include -#include - -namespace urdf{ - -bool parseModelState(ModelState &ms, TiXmlElement* config) -{ - ms.clear(); - - const char *name_char = config->Attribute("name"); - if (!name_char) - { - logError("No name given for the model_state."); - return false; - } - ms.name = std::string(name_char); - - const char *time_stamp_char = config->Attribute("time_stamp"); - if (time_stamp_char) - { - try { - double sec = boost::lexical_cast(time_stamp_char); - ms.time_stamp.set(sec); - } - catch (boost::bad_lexical_cast &e) { - logError("Parsing time stamp [%s] failed: %s", time_stamp_char, e.what()); - return false; - } - } - - TiXmlElement *joint_state_elem = config->FirstChildElement("joint_state"); - if (joint_state_elem) - { - boost::shared_ptr joint_state; - joint_state.reset(new JointState()); - - const char *joint_char = joint_state_elem->Attribute("joint"); - if (joint_char) - joint_state->joint = std::string(joint_char); - else - { - logError("No joint name given for the model_state."); - return false; - } - - // parse position - const char *position_char = joint_state_elem->Attribute("position"); - if (position_char) - { - - std::vector pieces; - boost::split( pieces, position_char, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try { - joint_state->position.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) { - throw ParseError("position element ("+ pieces[i] +") is not a valid float"); - } - } - } - } - - // parse velocity - const char *velocity_char = joint_state_elem->Attribute("velocity"); - if (velocity_char) - { - - std::vector pieces; - boost::split( pieces, velocity_char, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try { - joint_state->velocity.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) { - throw ParseError("velocity element ("+ pieces[i] +") is not a valid float"); - } - } - } - } - - // parse effort - const char *effort_char = joint_state_elem->Attribute("effort"); - if (effort_char) - { - - std::vector pieces; - boost::split( pieces, effort_char, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try { - joint_state->effort.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) { - throw ParseError("effort element ("+ pieces[i] +") is not a valid float"); - } - } - } - } - - // add to vector - ms.joint_states.push_back(joint_state); - } -}; - - - -} - - diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp b/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp deleted file mode 100644 index 85a886d..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp +++ /dev/null @@ -1,364 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - - -#include -#include -#include -#include -#include -#include -#include - -namespace urdf{ - -bool parsePose(Pose &pose, TiXmlElement* xml); - -bool parseCamera(Camera &camera, TiXmlElement* config) -{ - camera.clear(); - camera.type = VisualSensor::CAMERA; - - TiXmlElement *image = config->FirstChildElement("image"); - if (image) - { - const char* width_char = image->Attribute("width"); - if (width_char) - { - try - { - camera.width = boost::lexical_cast(width_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Camera image width [%s] is not a valid int: %s", width_char, e.what()); - return false; - } - } - else - { - logError("Camera sensor needs an image width attribute"); - return false; - } - - const char* height_char = image->Attribute("height"); - if (height_char) - { - try - { - camera.height = boost::lexical_cast(height_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Camera image height [%s] is not a valid int: %s", height_char, e.what()); - return false; - } - } - else - { - logError("Camera sensor needs an image height attribute"); - return false; - } - - const char* format_char = image->Attribute("format"); - if (format_char) - camera.format = std::string(format_char); - else - { - logError("Camera sensor needs an image format attribute"); - return false; - } - - const char* hfov_char = image->Attribute("hfov"); - if (hfov_char) - { - try - { - camera.hfov = boost::lexical_cast(hfov_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Camera image hfov [%s] is not a valid float: %s", hfov_char, e.what()); - return false; - } - } - else - { - logError("Camera sensor needs an image hfov attribute"); - return false; - } - - const char* near_char = image->Attribute("near"); - if (near_char) - { - try - { - camera.near = boost::lexical_cast(near_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Camera image near [%s] is not a valid float: %s", near_char, e.what()); - return false; - } - } - else - { - logError("Camera sensor needs an image near attribute"); - return false; - } - - const char* far_char = image->Attribute("far"); - if (far_char) - { - try - { - camera.far = boost::lexical_cast(far_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Camera image far [%s] is not a valid float: %s", far_char, e.what()); - return false; - } - } - else - { - logError("Camera sensor needs an image far attribute"); - return false; - } - - } - else - { - logError("Camera sensor has no element"); - return false; - } - return true; -} - -bool parseRay(Ray &ray, TiXmlElement* config) -{ - ray.clear(); - ray.type = VisualSensor::RAY; - - TiXmlElement *horizontal = config->FirstChildElement("horizontal"); - if (horizontal) - { - const char* samples_char = horizontal->Attribute("samples"); - if (samples_char) - { - try - { - ray.horizontal_samples = boost::lexical_cast(samples_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what()); - return false; - } - } - - const char* resolution_char = horizontal->Attribute("resolution"); - if (resolution_char) - { - try - { - ray.horizontal_resolution = boost::lexical_cast(resolution_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray horizontal resolution [%s] is not a valid float: %s", resolution_char, e.what()); - return false; - } - } - - const char* min_angle_char = horizontal->Attribute("min_angle"); - if (min_angle_char) - { - try - { - ray.horizontal_min_angle = boost::lexical_cast(min_angle_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray horizontal min_angle [%s] is not a valid float: %s", min_angle_char, e.what()); - return false; - } - } - - const char* max_angle_char = horizontal->Attribute("max_angle"); - if (max_angle_char) - { - try - { - ray.horizontal_max_angle = boost::lexical_cast(max_angle_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray horizontal max_angle [%s] is not a valid float: %s", max_angle_char, e.what()); - return false; - } - } - } - - TiXmlElement *vertical = config->FirstChildElement("vertical"); - if (vertical) - { - const char* samples_char = vertical->Attribute("samples"); - if (samples_char) - { - try - { - ray.vertical_samples = boost::lexical_cast(samples_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what()); - return false; - } - } - - const char* resolution_char = vertical->Attribute("resolution"); - if (resolution_char) - { - try - { - ray.vertical_resolution = boost::lexical_cast(resolution_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray vertical resolution [%s] is not a valid float: %s", resolution_char, e.what()); - return false; - } - } - - const char* min_angle_char = vertical->Attribute("min_angle"); - if (min_angle_char) - { - try - { - ray.vertical_min_angle = boost::lexical_cast(min_angle_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray vertical min_angle [%s] is not a valid float: %s", min_angle_char, e.what()); - return false; - } - } - - const char* max_angle_char = vertical->Attribute("max_angle"); - if (max_angle_char) - { - try - { - ray.vertical_max_angle = boost::lexical_cast(max_angle_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray vertical max_angle [%s] is not a valid float: %s", max_angle_char, e.what()); - return false; - } - } - } -} - -boost::shared_ptr parseVisualSensor(TiXmlElement *g) -{ - boost::shared_ptr visual_sensor; - - // get sensor type - TiXmlElement *sensor_xml; - if (g->FirstChildElement("camera")) - { - Camera *camera = new Camera(); - visual_sensor.reset(camera); - sensor_xml = g->FirstChildElement("camera"); - if (!parseCamera(*camera, sensor_xml)) - visual_sensor.reset(); - } - else if (g->FirstChildElement("ray")) - { - Ray *ray = new Ray(); - visual_sensor.reset(ray); - sensor_xml = g->FirstChildElement("ray"); - if (!parseRay(*ray, sensor_xml)) - visual_sensor.reset(); - } - else - { - logError("No know sensor types [camera|ray] defined in block"); - } - return visual_sensor; -} - - -bool parseSensor(Sensor &sensor, TiXmlElement* config) -{ - sensor.clear(); - - const char *name_char = config->Attribute("name"); - if (!name_char) - { - logError("No name given for the sensor."); - return false; - } - sensor.name = std::string(name_char); - - // parse parent_link_name - const char *parent_link_name_char = config->Attribute("parent_link_name"); - if (!parent_link_name_char) - { - logError("No parent_link_name given for the sensor."); - return false; - } - sensor.parent_link_name = std::string(parent_link_name_char); - - // parse origin - TiXmlElement *o = config->FirstChildElement("origin"); - if (o) - { - if (!parsePose(sensor.origin, o)) - return false; - } - - // parse sensor - sensor.sensor = parseVisualSensor(config); - return true; -} - - -} - - diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/world.cpp b/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/world.cpp deleted file mode 100644 index ddc27c5..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/world.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace urdf{ - -bool parseWorld(World &world, TiXmlElement* config) -{ - - // to be implemented - - return true; -} - -bool exportWorld(World &world, TiXmlElement* xml) -{ - TiXmlElement * world_xml = new TiXmlElement("world"); - world_xml->SetAttribute("name", world.name); - - // to be implemented - // exportModels(*world.models, world_xml); - - xml->LinkEndChild(world_xml); - - return true; -} - -} diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/test/memtest.cpp b/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/test/memtest.cpp deleted file mode 100644 index d835eb3..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/test/memtest.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "urdf_parser/urdf_parser.h" -#include -#include - -int main(int argc, char** argv){ - while (true){ - std::string xml_string; - std::fstream xml_file(argv[1], std::fstream::in); - while ( xml_file.good() ) - { - std::string line; - std::getline( xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - - - urdf::parseURDF(xml_string); - } -} diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/LICENSE b/addons/urdfreader/thirdparty/urdf/urdfdom_headers/LICENSE deleted file mode 100644 index e80920e..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/LICENSE +++ /dev/null @@ -1,15 +0,0 @@ -Software License Agreement (Apache License) - -Copyright 2011 John Hsu - -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/README.txt b/addons/urdfreader/thirdparty/urdf/urdfdom_headers/README.txt deleted file mode 100644 index 6a841d5..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/README.txt +++ /dev/null @@ -1,6 +0,0 @@ -The URDF (U-Robot Description Format) headers - provides core data structure headers for URDF. - -For now, the details of the URDF specifications reside on - http://ros.org/wiki/urdf - diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h b/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h deleted file mode 100644 index 24222f1..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h +++ /dev/null @@ -1,53 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -// URDF exceptions -#ifndef URDF_INTERFACE_EXCEPTION_H_ -#define URDF_INTERFACE_EXCEPTION_H_ - -#include -#include - -namespace urdf -{ - -class ParseError: public std::runtime_error -{ -public: - ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {}; -}; - -} - -#endif diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h b/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h deleted file mode 100644 index 9c15dd7..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h +++ /dev/null @@ -1,101 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Josh Faust */ - -#ifndef URDF_INTERFACE_COLOR_H -#define URDF_INTERFACE_COLOR_H - -#include -#include -#include -//#include -//#include - -namespace urdf -{ - -class Color -{ -public: - Color() {this->clear();}; - float r; - float g; - float b; - float a; - - void clear() - { - r = g = b = 0.0f; - a = 1.0f; - } - bool init(const std::string &vector_str) - { - this->clear(); - std::vector pieces; - std::vector rgba; - - boost::split( pieces, vector_str, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i) - { - if (!pieces[i].empty()) - { - try - { - rgba.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) - { - return false; - } - } - } - - if (rgba.size() != 4) - { - return false; - } - this->r = rgba[0]; - this->g = rgba[1]; - this->b = rgba[2]; - this->a = rgba[3]; - - return true; - }; -}; - -} - -#endif - diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h b/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h deleted file mode 100644 index cd889dc..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h +++ /dev/null @@ -1,234 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#ifndef URDF_INTERFACE_JOINT_H -#define URDF_INTERFACE_JOINT_H - -#include -#include -#ifdef URDF_USE_BOOST -#include -#define my_shared_ptr my_shared_ptr -#else -#include -#endif - -#include - - -namespace urdf{ - -class Link; - -class JointDynamics -{ -public: - JointDynamics() { this->clear(); }; - double damping; - double friction; - - void clear() - { - damping = 0; - friction = 0; - }; -}; - -class JointLimits -{ -public: - JointLimits() { this->clear(); }; - double lower; - double upper; - double effort; - double velocity; - - void clear() - { - lower = 0; - upper = 0; - effort = 0; - velocity = 0; - }; -}; - -/// \brief Parameters for Joint Safety Controllers -class JointSafety -{ -public: - /// clear variables on construction - JointSafety() { this->clear(); }; - - /// - /// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage. - /// - /// Basic safety controller operation is as follows - /// - /// current safety controllers will take effect on joints outside the position range below: - /// - /// position range: [JointSafety::soft_lower_limit + JointLimits::velocity / JointSafety::k_position, - /// JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position] - /// - /// if (joint_position is outside of the position range above) - /// velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit) - /// velocity_limit_max = JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit) - /// else - /// velocity_limit_min = -JointLimits::velocity - /// velocity_limit_max = JointLimits::velocity - /// - /// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity, - /// velocity_limit_max - JointLimits::effort / JointSafety::k_velocity] - /// - /// if (joint_velocity is outside of the velocity range above) - /// effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min) - /// effort_limit_max = JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max) - /// else - /// effort_limit_min = -JointLimits::effort - /// effort_limit_max = JointLimits::effort - /// - /// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max] - /// - /// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits - /// - double soft_upper_limit; - double soft_lower_limit; - double k_position; - double k_velocity; - - void clear() - { - soft_upper_limit = 0; - soft_lower_limit = 0; - k_position = 0; - k_velocity = 0; - }; -}; - - -class JointCalibration -{ -public: - JointCalibration() { this->clear(); }; - double reference_position; - my_shared_ptr rising, falling; - - void clear() - { - reference_position = 0; - }; -}; - -class JointMimic -{ -public: - JointMimic() { this->clear(); }; - double offset; - double multiplier; - std::string joint_name; - - void clear() - { - offset = 0.0; - multiplier = 0.0; - joint_name.clear(); - }; -}; - - -class Joint -{ -public: - - Joint() { this->clear(); }; - - std::string name; - enum - { - UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED - } type; - - /// \brief type_ meaning of axis_ - /// ------------------------------------------------------ - /// UNKNOWN unknown type - /// REVOLUTE rotation axis - /// PRISMATIC translation axis - /// FLOATING N/A - /// PLANAR plane normal axis - /// FIXED N/A - Vector3 axis; - - /// child Link element - /// child link frame is the same as the Joint frame - std::string child_link_name; - - /// parent Link element - /// origin specifies the transform from Parent Link to Joint Frame - std::string parent_link_name; - /// transform from Parent Link frame to Joint frame - Pose parent_to_joint_origin_transform; - - /// Joint Dynamics - my_shared_ptr dynamics; - - /// Joint Limits - my_shared_ptr limits; - - /// Unsupported Hidden Feature - my_shared_ptr safety; - - /// Unsupported Hidden Feature - my_shared_ptr calibration; - - /// Option to Mimic another Joint - my_shared_ptr mimic; - - void clear() - { - this->axis.clear(); - this->child_link_name.clear(); - this->parent_link_name.clear(); - this->parent_to_joint_origin_transform.clear(); - this->dynamics.reset(0); - this->limits.reset(0); - this->safety.reset(0); - this->calibration.reset(0); - this->type = UNKNOWN; - }; -}; - -} - -#endif diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h b/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h deleted file mode 100644 index 22e64cf..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h +++ /dev/null @@ -1,262 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#ifndef URDF_INTERFACE_LINK_H -#define URDF_INTERFACE_LINK_H - -#include -#include -#include - - -#ifdef URDF_USE_BOOST - #include - #include -#else - #include -#endif - -#include "joint.h" -#include "color.h" -#include "pose.h" - -namespace urdf{ - -class Geometry -{ -public: - enum {SPHERE, BOX, CYLINDER, MESH} type; - - virtual ~Geometry(void) - { - } -}; - -class Sphere : public Geometry -{ -public: - Sphere() { this->clear(); type = SPHERE; }; - double radius; - - void clear() - { - radius = 0; - }; -}; - -class Box : public Geometry -{ -public: - Box() { this->clear(); type = BOX; } - Vector3 dim; - - void clear() - { - this->dim.clear(); - }; -}; - -class Cylinder : public Geometry -{ -public: - Cylinder() { this->clear(); type = CYLINDER; }; - double length; - double radius; - - void clear() - { - length = 0; - radius = 0; - }; -}; - -class Mesh : public Geometry -{ -public: - Mesh() { this->clear(); type = MESH; }; - std::string filename; - Vector3 scale; - - void clear() - { - filename.clear(); - // default scale - scale.x = 1; - scale.y = 1; - scale.z = 1; - }; -}; - -class Material -{ -public: - Material() { this->clear(); }; - std::string name; - std::string texture_filename; - Color color; - - void clear() - { - color.clear(); - texture_filename.clear(); - name.clear(); - }; -}; - -class Inertial -{ -public: - Inertial() { this->clear(); }; - Pose origin; - double mass; - double ixx,ixy,ixz,iyy,iyz,izz; - - void clear() - { - origin.clear(); - mass = 0; - ixx = ixy = ixz = iyy = iyz = izz = 0; - }; -}; - -class Visual -{ -public: - Visual() { this->clear(); }; - Pose origin; - my_shared_ptr geometry; - - std::string material_name; - my_shared_ptr material; - - void clear() - { - origin.clear(); - material_name.clear(); - material.reset(0); - geometry.reset(0); - name.clear(); - }; - - std::string name; -}; - -class Collision -{ -public: - Collision() { this->clear(); }; - Pose origin; - my_shared_ptr geometry; - - void clear() - { - origin.clear(); - geometry.reset(0); - name.clear(); - }; - - std::string name; - -}; - - -class Link -{ -public: - Link() { this->clear(); }; - - std::string name; - - /// inertial element - my_shared_ptr inertial; - - /// visual element - my_shared_ptr visual; - - /// collision element - my_shared_ptr collision; - - /// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array) - std::vector > collision_array; - - /// if more than one visual element is specified, all visual elements are placed in this array (the visual member points to the first element of the array) - std::vector > visual_array; - - /// Parent Joint element - /// explicitly stating "parent" because we want directional-ness for tree structure - /// every link can have one parent - my_shared_ptr parent_joint; - - std::vector > child_joints; - std::vector > child_links; - - mutable int m_link_index; - - const Link* getParent() const - {return parent_link_;} - - void setParent(const my_shared_ptr &parent) - { - parent_link_ = parent.get(); - } - - void clear() - { - this->name.clear(); - this->inertial.reset(0); - this->visual.reset(0); - this->collision.reset(0); - this->parent_joint.reset(0); - this->child_joints.clear(); - this->child_links.clear(); - this->collision_array.clear(); - this->visual_array.clear(); - this->m_link_index=-1; - this->parent_link_ = NULL; - }; - -private: -// boost::weak_ptr parent_link_; - const Link* parent_link_; - -}; - - - - -} - -#endif diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h b/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h deleted file mode 100644 index 8e93d94..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h +++ /dev/null @@ -1,220 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#ifndef URDF_INTERFACE_MODEL_H -#define URDF_INTERFACE_MODEL_H - -#include -#include -//#include -#include -#include //printf -#include - -namespace urdf { - -class ModelInterface -{ -public: - my_shared_ptr getRoot(void) const{return this->root_link_;}; - my_shared_ptr getLink(const std::string& name) const - { - my_shared_ptr ptr; - if (this->links_.find(name) == this->links_.end()) - ptr.reset(0); - else - ptr = this->links_.find(name)->second; - return ptr; - }; - - my_shared_ptr getJoint(const std::string& name) const - { - my_shared_ptr ptr; - if (this->joints_.find(name) == this->joints_.end()) - ptr.reset(0); - else - ptr = this->joints_.find(name)->second; - return ptr; - }; - - - const std::string& getName() const {return name_;}; - void getLinks(std::vector >& links) const - { - for (std::map >::const_iterator link = this->links_.begin();link != this->links_.end(); link++) - { - links.push_back(link->second); - } - }; - - void clear() - { - m_numLinks=0; - m_numJoints = 0; - name_.clear(); - this->links_.clear(); - this->joints_.clear(); - this->materials_.clear(); - this->root_link_.reset(0); - }; - - /// non-const getLink() - void getLink(const std::string& name,my_shared_ptr &link) const - { - my_shared_ptr ptr; - if (this->links_.find(name) == this->links_.end()) - ptr.reset(0); - else - ptr = this->links_.find(name)->second; - link = ptr; - }; - - /// non-const getMaterial() - my_shared_ptr getMaterial(const std::string& name) const - { - my_shared_ptr ptr; - if (this->materials_.find(name) == this->materials_.end()) - ptr.reset(0); - else - ptr = this->materials_.find(name)->second; - return ptr; - }; - - void initTree(std::map &parent_link_tree) - { - // loop through all joints, for every link, assign children links and children joints - for (std::map >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) - { - std::string parent_link_name = joint->second->parent_link_name; - std::string child_link_name = joint->second->child_link_name; - - if (parent_link_name.empty() || child_link_name.empty()) - { - assert(0); - - // throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification."); - } - else - { - // find child and parent links - my_shared_ptr child_link, parent_link; - this->getLink(child_link_name, child_link); - if (!child_link) - { - printf("Error: child link [%s] of joint [%s] not found\n", child_link_name.c_str(),joint->first.c_str() ); - assert(0); -// throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found"); - } - this->getLink(parent_link_name, parent_link); - if (!parent_link) - { - assert(0); - -/* throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found. This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"\" to your urdf file."); - - */} - - //set parent link for child link - child_link->setParent(parent_link); - - //set parent joint for child link - child_link->parent_joint = joint->second; - - //set child joint for parent link - parent_link->child_joints.push_back(joint->second); - - //set child link for parent link - parent_link->child_links.push_back(child_link); - - // fill in child/parent string map - parent_link_tree[child_link->name] = parent_link_name; - } - } - } - - void initRoot(const std::map &parent_link_tree) - { - this->root_link_.reset(0); - - // find the links that have no parent in the tree - for (std::map >::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++) - { - std::map::const_iterator parent = parent_link_tree.find(l->first); - if (parent == parent_link_tree.end()) - { - // store root link - if (!this->root_link_) - { - getLink(l->first, this->root_link_); - } - // we already found a root link - else - { - assert(0); - // throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]"); - } - } - } - if (!this->root_link_) - { - assert(0); - //throw ParseError("No root link found. The robot xml is not a valid tree."); - } - } - - - /// \brief complete list of Links - std::map > links_; - /// \brief complete list of Joints - std::map > joints_; - /// \brief complete list of Materials - std::map > materials_; - - /// \brief The name of the robot model - std::string name_; - - /// \brief The root is always a link (the parent of the tree describing the robot) - my_shared_ptr root_link_; - - int m_numLinks;//includes parent - int m_numJoints; - - -}; - -} - -#endif diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h b/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h deleted file mode 100644 index 93183c8..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h +++ /dev/null @@ -1,265 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#ifndef URDF_INTERFACE_POSE_H -#define URDF_INTERFACE_POSE_H - -#include -//#include -#include -#include -#ifndef M_PI -#define M_PI 3.141592538 -#endif //M_PI - -#ifdef URDF_USE_BOOST - #include - #include -#else - #include - #include -#endif //URDF_USE_BOOST - -#include -#include - -namespace urdf{ - -class Vector3 -{ -public: - Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;}; - Vector3() {this->clear();}; - double x; - double y; - double z; - - void clear() {this->x=this->y=this->z=0.0;}; - void init(const std::string &vector_str) - { - this->clear(); - std::vector pieces; - std::vector xyz; - boost::split( pieces, vector_str, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try { - xyz.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) - { - assert(0); - // throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)"); - } - } - } - - - - if (xyz.size() != 3) - { - assert(0); - // throw ParseError("Parser found " + boost::lexical_cast(xyz.size()) + " elements but 3 expected while parsing vector [" + vector_str + "]"); - } - this->x = xyz[0]; - this->y = xyz[1]; - this->z = xyz[2]; - } - - Vector3 operator+(Vector3 vec) - { - return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z); - }; -}; - -class Rotation -{ -public: - Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;}; - Rotation() {this->clear();}; - void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const - { - quat_x = this->x; - quat_y = this->y; - quat_z = this->z; - quat_w = this->w; - }; - void getRPY(double &roll,double &pitch,double &yaw) const - { - double sqw; - double sqx; - double sqy; - double sqz; - - sqx = this->x * this->x; - sqy = this->y * this->y; - sqz = this->z * this->z; - sqw = this->w * this->w; - - roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz); - double sarg = -2 * (this->x*this->z - this->w*this->y); - pitch = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg)); - yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz); - - }; - void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w) - { - this->x = quat_x; - this->y = quat_y; - this->z = quat_z; - this->w = quat_w; - this->normalize(); - }; - void setFromRPY(double roll, double pitch, double yaw) - { - double phi, the, psi; - - phi = roll / 2.0; - the = pitch / 2.0; - psi = yaw / 2.0; - - this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi); - this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi); - this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi); - this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi); - - this->normalize(); - }; - - double x,y,z,w; - - void init(const std::string &rotation_str) - { - this->clear(); - Vector3 rpy; - rpy.init(rotation_str); - setFromRPY(rpy.x, rpy.y, rpy.z); - } - - void clear() { this->x=this->y=this->z=0.0;this->w=1.0; } - - void normalize() - { - double s = sqrt(this->x * this->x + - this->y * this->y + - this->z * this->z + - this->w * this->w); - if (s == 0.0) - { - this->x = 0.0; - this->y = 0.0; - this->z = 0.0; - this->w = 1.0; - } - else - { - this->x /= s; - this->y /= s; - this->z /= s; - this->w /= s; - } - }; - - // Multiplication operator (copied from gazebo) - Rotation operator*( const Rotation &qt ) const - { - Rotation c; - - c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y; - c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x; - c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w; - c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z; - - return c; - }; - /// Rotate a vector using the quaternion - Vector3 operator*(Vector3 vec) const - { - Rotation tmp; - Vector3 result; - - tmp.w = 0.0; - tmp.x = vec.x; - tmp.y = vec.y; - tmp.z = vec.z; - - tmp = (*this) * (tmp * this->GetInverse()); - - result.x = tmp.x; - result.y = tmp.y; - result.z = tmp.z; - - return result; - }; - // Get the inverse of this quaternion - Rotation GetInverse() const - { - Rotation q; - - double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z; - - if (norm > 0.0) - { - q.w = this->w / norm; - q.x = -this->x / norm; - q.y = -this->y / norm; - q.z = -this->z / norm; - } - - return q; - }; - - -}; - -class Pose -{ -public: - Pose() { this->clear(); }; - - Vector3 position; - Rotation rotation; - - void clear() - { - this->position.clear(); - this->rotation.clear(); - }; -}; - -} - -#endif diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h b/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h deleted file mode 100644 index 5560de3..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h +++ /dev/null @@ -1,68 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - -#ifndef URDF_TWIST_H -#define URDF_TWIST_H - -#include -#include -#include -#include -#include - -namespace urdf{ - - -class Twist -{ -public: - Twist() { this->clear(); }; - - Vector3 linear; - // Angular velocity represented by Euler angles - Vector3 angular; - - void clear() - { - this->linear.clear(); - this->angular.clear(); - }; -}; - -} - -#endif - diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h b/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h deleted file mode 100644 index b132719..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h +++ /dev/null @@ -1,141 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - -#ifndef URDF_MODEL_STATE_H -#define URDF_MODEL_STATE_H - -#include -#include -#include -#include -#include - -#include "urdf_model/pose.h" -#include - - -namespace urdf{ - -class Time -{ -public: - Time() { this->clear(); }; - - void set(double _seconds) - { - this->sec = (int32_t)(floor(_seconds)); - this->nsec = (int32_t)(round((_seconds - this->sec) * 1e9)); - this->Correct(); - }; - - operator double () - { - return (static_cast(this->sec) + - static_cast(this->nsec)*1e-9); - }; - - int32_t sec; - int32_t nsec; - - void clear() - { - this->sec = 0; - this->nsec = 0; - }; -private: - void Correct() - { - // Make any corrections - if (this->nsec >= 1e9) - { - this->sec++; - this->nsec = (int32_t)(this->nsec - 1e9); - } - else if (this->nsec < 0) - { - this->sec--; - this->nsec = (int32_t)(this->nsec + 1e9); - } - }; -}; - - -class JointState -{ -public: - JointState() { this->clear(); }; - - /// joint name - std::string joint; - - std::vector position; - std::vector velocity; - std::vector effort; - - void clear() - { - this->joint.clear(); - this->position.clear(); - this->velocity.clear(); - this->effort.clear(); - } -}; - -class ModelState -{ -public: - ModelState() { this->clear(); }; - - /// state name must be unique - std::string name; - - Time time_stamp; - - void clear() - { - this->name.clear(); - this->time_stamp.set(0); - this->joint_states.clear(); - }; - - std::vector > joint_states; - -}; - -} - -#endif - diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h b/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h deleted file mode 100644 index 05f1917..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h +++ /dev/null @@ -1,42 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -#ifndef URDF_MODEL_STATE_TWIST_ -#define URDF_MODEL_STATE_TWIST_ - -#warning "Please Use #include " - -#include - -#endif diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h b/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h deleted file mode 100644 index 3b99695..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h +++ /dev/null @@ -1,176 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - -/* example - - - - - 1.5708 - - - - - - - - - - - - - - -*/ - - - -#ifndef URDF_SENSOR_H -#define URDF_SENSOR_H - -#include -#include -#include -#include -#include -#include "urdf_model/pose.h" -#include "urdf_model/joint.h" -#include "urdf_model/link.h" - -namespace urdf{ - -class VisualSensor -{ -public: - enum {CAMERA, RAY} type; - virtual ~VisualSensor(void) - { - } -}; - -class Camera : public VisualSensor -{ -public: - Camera() { this->clear(); }; - unsigned int width, height; - /// format is optional: defaults to R8G8B8), but can be - /// (L8|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8) - std::string format; - double hfov; - double near; - double far; - - void clear() - { - hfov = 0; - width = 0; - height = 0; - format.clear(); - near = 0; - far = 0; - }; -}; - -class Ray : public VisualSensor -{ -public: - Ray() { this->clear(); }; - unsigned int horizontal_samples; - double horizontal_resolution; - double horizontal_min_angle; - double horizontal_max_angle; - unsigned int vertical_samples; - double vertical_resolution; - double vertical_min_angle; - double vertical_max_angle; - - void clear() - { - // set defaults - horizontal_samples = 1; - horizontal_resolution = 1; - horizontal_min_angle = 0; - horizontal_max_angle = 0; - vertical_samples = 1; - vertical_resolution = 1; - vertical_min_angle = 0; - vertical_max_angle = 0; - }; -}; - - -class Sensor -{ -public: - Sensor() { this->clear(); }; - - /// sensor name must be unique - std::string name; - - /// update rate in Hz - double update_rate; - - /// transform from parent frame to optical center - /// with z-forward and x-right, y-down - Pose origin; - - /// sensor - boost::shared_ptr sensor; - - - /// Parent link element name. A pointer is stored in parent_link_. - std::string parent_link_name; - - boost::shared_ptr getParent() const - {return parent_link_.lock();}; - - void setParent(boost::shared_ptr parent) - { this->parent_link_ = parent; } - - void clear() - { - this->name.clear(); - this->sensor.reset(); - this->parent_link_name.clear(); - this->parent_link_.reset(); - }; - -private: - boost::weak_ptr parent_link_; - -}; -} -#endif diff --git a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h b/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h deleted file mode 100644 index eb13fc4..0000000 --- a/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h +++ /dev/null @@ -1,114 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - -/* encapsulates components in a world - see http://ros.org/wiki/usdf/XML/urdf_world and - for details -*/ -/* example world XML - - - - - ... - - - - - - - - - - - - - - - - - - - -*/ - -#ifndef USDF_STATE_H -#define USDF_STATE_H - -#include -#include -#include -#include -#include -#include - -#include "urdf_model/model.h" -#include "urdf_model/pose.h" -#include "urdf_model/twist.h" - -namespace urdf{ - -class Entity -{ -public: - boost::shared_ptr model; - Pose origin; - Twist twist; -}; - -class World -{ -public: - World() { this->clear(); }; - - /// world name must be unique - std::string name; - - std::vector models; - - void initXml(TiXmlElement* config); - - void clear() - { - this->name.clear(); - }; -}; -} - -#endif - diff --git a/addons/urdfreader/urdfreader.cc b/addons/urdfreader/urdfreader.cc index 5b9e3e5..db98c3b 100644 --- a/addons/urdfreader/urdfreader.cc +++ b/addons/urdfreader/urdfreader.cc @@ -9,116 +9,148 @@ #include #ifdef RBDL_USE_ROS_URDF_LIBRARY -#include -#include - -typedef boost::shared_ptr LinkPtr; -typedef const boost::shared_ptr ConstLinkPtr; -typedef boost::shared_ptr JointPtr; -typedef boost::shared_ptr ModelPtr; - + #include + #include + #include + + typedef urdf::LinkSharedPtr LinkPtr; + typedef const urdf::LinkConstSharedPtr ConstLinkPtr; + typedef urdf::JointSharedPtr JointPtr; + typedef urdf::ModelInterfaceSharedPtr ModelPtr; + typedef urdf::Joint UrdfJointType; + + #define LINKMAP links_ + #define JOINTMAP joints_ + #define PARENT_TRANSFORM parent_to_joint_origin_transform + #define RPY getRPY #else -#include -#include - -typedef my_shared_ptr LinkPtr; -typedef const my_shared_ptr ConstLinkPtr; -typedef my_shared_ptr JointPtr; -typedef my_shared_ptr ModelPtr; - + #include + #include + #include + + typedef std::shared_ptr LinkPtr; + typedef std::shared_ptr ConstLinkPtr; + typedef std::shared_ptr JointPtr; + typedef std::shared_ptr ModelPtr; + typedef urdf::JointType UrdfJointType; + + #define LINKMAP link_map + #define JOINTMAP joint_map + #define PARENT_TRANSFORM parent_to_joint_transform + #define RPY getRpy #endif using namespace std; -namespace RigidBodyDynamics { +namespace RigidBodyDynamics +{ -namespace Addons { + namespace Addons + { -using namespace Math; + using namespace Math; + using namespace Errors; -typedef vector URDFLinkVector; -typedef vector URDFJointVector; -typedef map URDFLinkMap; -typedef map URDFJointMap; + typedef vector URDFLinkVector; + typedef vector URDFJointVector; + typedef map URDFLinkMap; + typedef map URDFJointMap; -bool construct_model (Model* rbdl_model, ModelPtr urdf_model, bool floating_base, bool verbose) { - LinkPtr urdf_root_link; +// ============================================================================= - URDFLinkMap link_map; - link_map = urdf_model->links_; +void construct_model(Model *rbdl_model, ModelPtr urdf_model, + bool floating_base, bool verbose) { - URDFJointMap joint_map; - joint_map = urdf_model->joints_; + LinkPtr urdf_root_link; + + URDFLinkMap link_map = urdf_model->LINKMAP; + URDFJointMap joint_map = urdf_model->JOINTMAP; vector joint_names; - // Holds the links that we are processing in our depth first traversal with the top element being the current link. - stack link_stack; + // Holds the links that we are processing in our depth first traversal + // with the top element being the current link. + stack link_stack; // Holds the child joint index of the current link stack joint_index_stack; // add the bodies in a depth-first order of the model tree - link_stack.push (link_map[(urdf_model->getRoot()->name)]); + link_stack.push(link_map[(urdf_model->getRoot()->name)]); // add the root body - ConstLinkPtr& root = urdf_model->getRoot (); - Vector3d root_inertial_rpy; + ConstLinkPtr root = urdf_model->getRoot(); Vector3d root_inertial_position; Matrix3d root_inertial_inertia; double root_inertial_mass; - if (root->inertial) { - root_inertial_mass = root->inertial->mass; - - root_inertial_position.set ( - root->inertial->origin.position.x, - root->inertial->origin.position.y, - root->inertial->origin.position.z); + Body root_link = Body(); - root_inertial_inertia(0,0) = root->inertial->ixx; - root_inertial_inertia(0,1) = root->inertial->ixy; - root_inertial_inertia(0,2) = root->inertial->ixz; +#ifdef RBDL_USE_ROS_URDF_LIBRARY + if (root->inertial != nullptr) { + auto I = root->inertial; +#else + if (root->inertial.has_value()) { + auto I = &root->inertial.value(); +#endif + root_inertial_mass = I->mass; + + root_inertial_position.set( + I->origin.position.x, + I->origin.position.y, + I->origin.position.z); + + root_inertial_inertia(0, 0) = I->ixx; + root_inertial_inertia(0, 1) = I->ixy; + root_inertial_inertia(0, 2) = I->ixz; + + root_inertial_inertia(1, 0) = I->ixy; + root_inertial_inertia(1, 1) = I->iyy; + root_inertial_inertia(1, 2) = I->iyz; + + root_inertial_inertia(2, 0) = I->ixz; + root_inertial_inertia(2, 1) = I->iyz; + root_inertial_inertia(2, 2) = I->izz; + + if (root_inertial_mass == 0. && root_inertial_inertia != Matrix3d::Zero()) { + std::ostringstream error_msg; + error_msg << "Error creating rbdl model! Urdf root link (" + << root->name + << ") has inertial but no mass!"; + throw RBDLFileParseError(error_msg.str()); + } - root_inertial_inertia(1,0) = root->inertial->ixy; - root_inertial_inertia(1,1) = root->inertial->iyy; - root_inertial_inertia(1,2) = root->inertial->iyz; + root_link.mMass = root_inertial_mass; + root_link.mInertia = root_inertial_inertia; + root_link.mCenterOfMass = root_inertial_position; + } - root_inertial_inertia(2,0) = root->inertial->ixz; - root_inertial_inertia(2,1) = root->inertial->iyz; - root_inertial_inertia(2,2) = root->inertial->izz; - root->inertial->origin.rotation.getRPY (root_inertial_rpy[0], root_inertial_rpy[1], root_inertial_rpy[2]); + Joint root_joint(JointTypeFixed); + if (floating_base) { + root_joint = JointTypeFloatingBase; + } - Body root_link = Body (root_inertial_mass, - root_inertial_position, - root_inertial_inertia); + SpatialTransform root_joint_frame = SpatialTransform(); - Joint root_joint (JointTypeFixed); + if (verbose) { + cout << "+ Adding Root Body " << endl; + cout << " joint frame: " << root_joint_frame << endl; if (floating_base) { - root_joint = JointTypeFloatingBase; - } - - SpatialTransform root_joint_frame = SpatialTransform (); - - if (verbose) { - cout << "+ Adding Root Body " << endl; - cout << " joint frame: " << root_joint_frame << endl; - if (floating_base) { - cout << " joint type : floating" << endl; - } else { - cout << " joint type : fixed" << endl; - } - cout << " body inertia: " << endl << root_link.mInertia << endl; - cout << " body mass : " << root_link.mMass << endl; - cout << " body name : " << root->name << endl; + cout << " joint type : floating" << endl; + } else { + cout << " joint type : fixed" << endl; } - - rbdl_model->AppendBody(root_joint_frame, - root_joint, - root_link, - root->name); + cout << " body inertia: " << endl + << root_link.mInertia << endl; + cout << " body mass : " << root_link.mMass << endl; + cout << " body name : " << root->name << endl; } + rbdl_model->AppendBody(root_joint_frame, + root_joint, + root_link, + root->name); + // depth first traversal: push the first child onto our joint_index_stack joint_index_stack.push(0); @@ -127,22 +159,24 @@ bool construct_model (Model* rbdl_model, ModelPtr urdf_model, bool floating_base unsigned int joint_idx = joint_index_stack.top(); - // Add any child bodies and increment current joint index if we still have child joints to process. + // Add any child bodies and increment current joint index if we still + // have child joints to process. if (joint_idx < cur_link->child_joints.size()) { JointPtr cur_joint = cur_link->child_joints[joint_idx]; // increment joint index joint_index_stack.pop(); - joint_index_stack.push (joint_idx + 1); + joint_index_stack.push(joint_idx + 1); - link_stack.push (link_map[cur_joint->child_link_name]); + link_stack.push(link_map[cur_joint->child_link_name]); joint_index_stack.push(0); if (verbose) { for (unsigned int i = 1; i < joint_index_stack.size() - 1; i++) { cout << " "; } - cout << "joint '" << cur_joint->name << "' child link '" << link_stack.top()->name << "' type = " << cur_joint->type << endl; + cout << "joint '" << cur_joint->name << "' child link '" << + link_stack.top()->name << "' type = " << cur_joint->type << endl; } joint_names.push_back(cur_joint->name); @@ -162,56 +196,61 @@ bool construct_model (Model* rbdl_model, ModelPtr urdf_model, bool floating_base unsigned int rbdl_parent_id = 0; if (urdf_parent->name != "base_link") { - rbdl_parent_id = rbdl_model->GetBodyId (urdf_parent->name.c_str()); + rbdl_parent_id = rbdl_model->GetBodyId(urdf_parent->name.c_str()); } - if (rbdl_parent_id == std::numeric_limits::max()) - cerr << "Error while processing joint '" << urdf_joint->name - << "': parent link '" << urdf_parent->name - << "' could not be found." << endl; - - //cout << "joint: " << urdf_joint->name << "\tparent = " << urdf_parent->name << " child = " << urdf_child->name << " parent_id = " << rbdl_parent_id << endl; + if (rbdl_parent_id == std::numeric_limits::max()) { + ostringstream error_msg; + error_msg << "Error while processing joint '" << urdf_joint->name + << "': parent link '" << urdf_parent->name + << "' could not be found." << endl; + throw RBDLFileParseError(error_msg.str()); + } // create the joint Joint rbdl_joint; - if (urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::CONTINUOUS) { - rbdl_joint = Joint (SpatialVector (urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z, 0., 0., 0.)); - } else if (urdf_joint->type == urdf::Joint::PRISMATIC) { - rbdl_joint = Joint (SpatialVector (0., 0., 0., urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z)); - } else if (urdf_joint->type == urdf::Joint::FIXED) { - rbdl_joint = Joint (JointTypeFixed); - } else if (urdf_joint->type == urdf::Joint::FLOATING) { + if (urdf_joint->type == UrdfJointType::REVOLUTE || + urdf_joint->type == UrdfJointType::CONTINUOUS) { + rbdl_joint = Joint(SpatialVector(urdf_joint->axis.x, urdf_joint->axis.y, + urdf_joint->axis.z, 0., 0., 0.)); + } else if (urdf_joint->type == UrdfJointType::PRISMATIC) { + rbdl_joint = Joint(SpatialVector(0., 0., 0., urdf_joint->axis.x, + urdf_joint->axis.y, urdf_joint->axis.z)); + } else if (urdf_joint->type == UrdfJointType::FIXED) { + rbdl_joint = Joint(JointTypeFixed); + } else if (urdf_joint->type == UrdfJointType::FLOATING) { // todo: what order of DoF should be used? - rbdl_joint = Joint ( - SpatialVector (0., 0., 0., 1., 0., 0.), - SpatialVector (0., 0., 0., 0., 1., 0.), - SpatialVector (0., 0., 0., 0., 0., 1.), - SpatialVector (1., 0., 0., 0., 0., 0.), - SpatialVector (0., 1., 0., 0., 0., 0.), - SpatialVector (0., 0., 1., 0., 0., 0.)); - } else if (urdf_joint->type == urdf::Joint::PLANAR) { + rbdl_joint = Joint( + SpatialVector(0., 0., 0., 1., 0., 0.), + SpatialVector(0., 0., 0., 0., 1., 0.), + SpatialVector(0., 0., 0., 0., 0., 1.), + SpatialVector(1., 0., 0., 0., 0., 0.), + SpatialVector(0., 1., 0., 0., 0., 0.), + SpatialVector(0., 0., 1., 0., 0., 0.)); + } else if (urdf_joint->type == UrdfJointType::PLANAR) { // todo: which two directions should be used that are perpendicular // to the specified axis? - cerr << "Error while processing joint '" << urdf_joint->name << "': planar joints not yet supported!" << endl; - return false; + ostringstream error_msg; + error_msg << "Error while processing joint '" << urdf_joint->name + << "': planar joints not yet supported!" << endl; + throw RBDLFileParseError(error_msg.str()); } // compute the joint transformation Vector3d joint_rpy; Vector3d joint_translation; - urdf_joint->parent_to_joint_origin_transform.rotation.getRPY (joint_rpy[0], joint_rpy[1], joint_rpy[2]); - joint_translation.set ( - urdf_joint->parent_to_joint_origin_transform.position.x, - urdf_joint->parent_to_joint_origin_transform.position.y, - urdf_joint->parent_to_joint_origin_transform.position.z - ); - SpatialTransform rbdl_joint_frame = - Xrot (joint_rpy[0], Vector3d (1., 0., 0.)) - * Xrot (joint_rpy[1], Vector3d (0., 1., 0.)) - * Xrot (joint_rpy[2], Vector3d (0., 0., 1.)) - * Xtrans (Vector3d ( - joint_translation - )); + urdf_joint->PARENT_TRANSFORM.rotation.RPY(joint_rpy[0], + joint_rpy[1], joint_rpy[2]); + joint_translation.set( + urdf_joint->PARENT_TRANSFORM.position.x, + urdf_joint->PARENT_TRANSFORM.position.y, + urdf_joint->PARENT_TRANSFORM.position.z); + SpatialTransform rbdl_joint_frame = + Xrotz(joint_rpy[2]) + * Xroty(joint_rpy[1]) + * Xrotx(joint_rpy[0]) + * Xtrans(joint_translation); + //rbdl_joint_frame = Xtrans(joint_translation); // assemble the body Vector3d link_inertial_position; @@ -220,35 +259,46 @@ bool construct_model (Model* rbdl_model, ModelPtr urdf_model, bool floating_base double link_inertial_mass = 0.; // but only if we actually have inertial data +#ifdef RBDL_USE_ROS_URDF_LIBRARY if (urdf_child->inertial) { - link_inertial_mass = urdf_child->inertial->mass; - - link_inertial_position.set ( - urdf_child->inertial->origin.position.x, - urdf_child->inertial->origin.position.y, - urdf_child->inertial->origin.position.z - ); - urdf_child->inertial->origin.rotation.getRPY (link_inertial_rpy[0], link_inertial_rpy[1], link_inertial_rpy[2]); - - link_inertial_inertia(0,0) = urdf_child->inertial->ixx; - link_inertial_inertia(0,1) = urdf_child->inertial->ixy; - link_inertial_inertia(0,2) = urdf_child->inertial->ixz; - - link_inertial_inertia(1,0) = urdf_child->inertial->ixy; - link_inertial_inertia(1,1) = urdf_child->inertial->iyy; - link_inertial_inertia(1,2) = urdf_child->inertial->iyz; - - link_inertial_inertia(2,0) = urdf_child->inertial->ixz; - link_inertial_inertia(2,1) = urdf_child->inertial->iyz; - link_inertial_inertia(2,2) = urdf_child->inertial->izz; - - if (link_inertial_rpy != Vector3d (0., 0., 0.)) { - cerr << "Error while processing body '" << urdf_child->name << "': rotation of body frames not yet supported. Please rotate the joint frame instead." << endl; - return false; + auto I_child = urdf_child->inertial; +#else + if (urdf_child->inertial.has_value()) { + auto I_child = &urdf_child->inertial.value(); +#endif + link_inertial_mass = I_child->mass; + + link_inertial_position.set( + I_child->origin.position.x, + I_child->origin.position.y, + I_child->origin.position.z); + I_child->origin.rotation.RPY(link_inertial_rpy[0], + link_inertial_rpy[1], + link_inertial_rpy[2]); + + link_inertial_inertia(0, 0) = I_child->ixx; + link_inertial_inertia(0, 1) = I_child->ixy; + link_inertial_inertia(0, 2) = I_child->ixz; + + link_inertial_inertia(1, 0) = I_child->ixy; + link_inertial_inertia(1, 1) = I_child->iyy; + link_inertial_inertia(1, 2) = I_child->iyz; + + link_inertial_inertia(2, 0) = I_child->ixz; + link_inertial_inertia(2, 1) = I_child->iyz; + link_inertial_inertia(2, 2) = I_child->izz; + + if (link_inertial_rpy != Vector3d(0., 0., 0.)) { + ostringstream error_msg; + error_msg << "Error while processing body '" << urdf_child->name + << "': rotation of body frames not yet supported." + << " Please rotate the joint frame instead." << endl; + throw RBDLFileParseError(error_msg.str()); } } - Body rbdl_body = Body (link_inertial_mass, link_inertial_position, link_inertial_inertia); + Body rbdl_body = Body(link_inertial_mass, link_inertial_position, + link_inertial_inertia); if (verbose) { cout << "+ Adding Body: " << urdf_child->name << endl; @@ -256,64 +306,77 @@ bool construct_model (Model* rbdl_model, ModelPtr urdf_model, bool floating_base cout << " joint frame: " << rbdl_joint_frame << endl; cout << " joint dofs : " << rbdl_joint.mDoFCount << endl; for (unsigned int j = 0; j < rbdl_joint.mDoFCount; j++) { - cout << " " << j << ": " << rbdl_joint.mJointAxes[j].transpose() << endl; + cout << " " << j << ": " + << rbdl_joint.mJointAxes[j].transpose() << endl; } - cout << " body inertia: " << endl << rbdl_body.mInertia << endl; + cout << " body inertia: " << endl + << rbdl_body.mInertia << endl; cout << " body mass : " << rbdl_body.mMass << endl; cout << " body name : " << urdf_child->name << endl; } - if (urdf_joint->type == urdf::Joint::FLOATING) { + if (urdf_joint->type == UrdfJointType::FLOATING) { Matrix3d zero_matrix = Matrix3d::Zero(); - Body null_body (0., Vector3d::Zero(3), zero_matrix); + Body null_body(0., Vector3d::Zero(3), zero_matrix); Joint joint_txtytz(JointTypeTranslationXYZ); string trans_body_name = urdf_child->name + "_Translate"; - rbdl_model->AddBody (rbdl_parent_id, rbdl_joint_frame, joint_txtytz, null_body, trans_body_name); + rbdl_model->AddBody(rbdl_parent_id, rbdl_joint_frame, + joint_txtytz, null_body, + trans_body_name); - Joint joint_euler_zyx (JointTypeEulerXYZ); - rbdl_model->AppendBody (SpatialTransform(), joint_euler_zyx, rbdl_body, urdf_child->name); + Joint joint_euler_zyx(JointTypeEulerXYZ); + rbdl_model->AppendBody(SpatialTransform(), joint_euler_zyx, + rbdl_body, urdf_child->name); } else { - rbdl_model->AddBody (rbdl_parent_id, rbdl_joint_frame, rbdl_joint, rbdl_body, urdf_child->name); + rbdl_model->AddBody(rbdl_parent_id, rbdl_joint_frame, rbdl_joint, + rbdl_body, urdf_child->name); } } - - return true; } +// ============================================================================= -RBDL_DLLAPI bool URDFReadFromFile (const char* filename, Model* model, bool floating_base, bool verbose) { - ifstream model_file (filename); +RBDL_DLLAPI bool URDFReadFromFile(const char *filename, Model *model, + bool floating_base, bool verbose) { + ifstream model_file(filename); if (!model_file) { cerr << "Error opening file '" << filename << "'." << endl; abort(); } - + // reserve memory for the contents of the file string model_xml_string; model_file.seekg(0, std::ios::end); model_xml_string.reserve(model_file.tellg()); model_file.seekg(0, std::ios::beg); - model_xml_string.assign((std::istreambuf_iterator(model_file)), std::istreambuf_iterator()); + model_xml_string.assign((std::istreambuf_iterator(model_file)), + std::istreambuf_iterator()); model_file.close(); - return URDFReadFromString (model_xml_string.c_str(), model, floating_base, verbose); + return URDFReadFromString(model_xml_string.c_str(), model, floating_base, + verbose); } -RBDL_DLLAPI bool URDFReadFromString (const char* model_xml_string, Model* model, bool floating_base, bool verbose) { - assert (model); +// ============================================================================= - ModelPtr urdf_model = urdf::parseURDF (model_xml_string); - - if (!construct_model (model, urdf_model, floating_base, verbose)) { - cerr << "Error constructing model from urdf file." << endl; - return false; - } + RBDL_DLLAPI bool URDFReadFromString(const char *model_xml_string, Model *model, + bool floating_base, bool verbose) + { + assert(model); - model->gravity.set (0., 0., -9.81); +#ifdef RBDL_USE_ROS_URDF_LIBRARY + ModelPtr urdf_model = urdf::parseURDF(model_xml_string); +#else + ModelPtr urdf_model = urdf::UrdfModel::fromUrdfStr(model_xml_string); +#endif - return true; -} + construct_model(model, urdf_model, floating_base, verbose); -} + model->gravity.set(0., 0., -9.81); -} + return true; + } + +} // namespace Addons + +} // namespace RigidBodyDynamics diff --git a/addons/urdfreader/urdfreader.h b/addons/urdfreader/urdfreader.h index 6e6e9ae..c479ae9 100644 --- a/addons/urdfreader/urdfreader.h +++ b/addons/urdfreader/urdfreader.h @@ -8,8 +8,30 @@ namespace RigidBodyDynamics { struct Model; namespace Addons { - RBDL_DLLAPI bool URDFReadFromFile (const char* filename, Model* model, bool floating_base, bool verbose = false); - RBDL_DLLAPI bool URDFReadFromString (const char* model_xml_string, Model* model, bool floating_base, bool verbose = false); + /** + * This function will load a URDF model from a file. + * + * @param filename: path to the URDF file + * @param model: reference to the (loaded) multibody model + * @param floating_pase: does the model use a floating base + * @param verbose: information will be printed to the command window if this + * is set to true + */ + RBDL_DLLAPI bool URDFReadFromFile (const char* filename, Model* model, + bool floating_base, bool verbose = false); + + /** + * This function will load a URDF model from a c string. + * + * @param model_xml_string: URDF data in form of a string + * @param model: reference to the (loaded) multibody model + * @param floating_pase: does the model use a floating base + * @param verbose: information will be printed to the command window if this + * is set to true + */ + RBDL_DLLAPI bool URDFReadFromString (const char* model_xml_string, + Model* model, bool floating_base, + bool verbose = false); } }