Commit 067f32f5 authored by Jordan Lack's avatar Jordan Lack

Merge branch 'release/3.2.0'

parents 16324e03 c24f1115
Pipeline #60902284 passed with stages
in 47 minutes and 56 seconds
image: registry.gitlab.com/jlack/rdl image: registry.gitlab.com/jlack/rdl:ci-docker-melodic
variables: variables:
WS_NAME: "rdl_ws" WS_NAME: "rdl_ws"
...@@ -15,6 +15,7 @@ stages: ...@@ -15,6 +15,7 @@ stages:
build_and_test: build_and_test:
stage: test stage: test
retry: 1
script: script:
- echo $CI_PROJECT_NAME - echo $CI_PROJECT_NAME
- mkdir -p $WS_NAME/src && cd $WS_NAME/src - mkdir -p $WS_NAME/src && cd $WS_NAME/src
...@@ -35,21 +36,6 @@ build_and_test: ...@@ -35,21 +36,6 @@ build_and_test:
- $WS_NAME/ - $WS_NAME/
expire_in: 1 day expire_in: 1 day
coverage_rdl_dynamics:
stage: analyze
dependencies:
- build_and_test
script:
- cd $WS_NAME && rosdep install --from-paths src -i -y --as-root apt:false
- catkin build
- catkin run_tests --no-deps rdl_dynamics -DCMAKE_BUILD_TYPE=Coverage --make-args run_coverage_rdl_dynamics
- gcovr -r .
- mv build/rdl_dynamics/coverage $CI_PROJECT_DIR/rdl_dynamics_coverage
artifacts:
name: rdl_dynamics_coverage
paths:
- $CI_PROJECT_DIR/rdl_dynamics_coverage/
coverage_rdl_urdfreader: coverage_rdl_urdfreader:
stage: analyze stage: analyze
dependencies: dependencies:
...@@ -80,6 +66,21 @@ coverage_rdl_ros_tools: ...@@ -80,6 +66,21 @@ coverage_rdl_ros_tools:
paths: paths:
- $CI_PROJECT_DIR/rdl_ros_tools_coverage/ - $CI_PROJECT_DIR/rdl_ros_tools_coverage/
coverage_rdl_dynamics:
stage: analyze
dependencies:
- build_and_test
script:
- cd $WS_NAME && rosdep install --from-paths src -i -y --as-root apt:false
- catkin build
- catkin run_tests --no-deps rdl_dynamics -DCMAKE_BUILD_TYPE=Coverage --make-args run_coverage_rdl_dynamics
- gcovr -r .
- mv build/rdl_dynamics/coverage $CI_PROJECT_DIR/rdl_dynamics_coverage
artifacts:
name: rdl_dynamics_coverage
paths:
- $CI_PROJECT_DIR/rdl_dynamics_coverage/
pages: pages:
stage: pages stage: pages
dependencies: dependencies:
......
...@@ -29,5 +29,25 @@ fixed frame. Added bugfix here to account for that ...@@ -29,5 +29,25 @@ fixed frame. Added bugfix here to account for that
--- 01-17-2019 --- --- 01-17-2019 ---
Fix bug with FrameOriention where operation was incorrect when calling changeFrame Fix bug with FrameOriention where operation was incorrect when calling changeFrame
--- 03-07/2019 --- --- 03-07-2019 ---
Add rdl_ros_tools package and rdl_msgs for the beginning of a set of ROS tools Add rdl_ros_tools package and rdl_msgs for the beginning of a set of ROS tools
\ No newline at end of file
--- 03-17-2019 ---
Fix issues making rdl incompatible with melodic
--- 04-05-2019 ---
Removed a bunch of deprecated functions
--- 04-06-2019 ---
Reworked FrameOrientation inheritance so it inherits from Quaternion instead of having a Quaternion
--- 04-07-2019 ---
Added Model::mass variable where you can get the mass of the model
--- 05-11-2019 ---
Remove the use of raw pointers and replace them with typedef'd std::shared_ptr's
Remove ReferenceFrameHolder
Minor rework to ReferenceFrame internal functionality to improve efficiency
--- 05-11-2019 ---
Create types.hpp file that auto generates shared_ptr typedef's
\ No newline at end of file
...@@ -38,7 +38,7 @@ namespace RobotDynamics ...@@ -38,7 +38,7 @@ namespace RobotDynamics
* // only motivation for using a pointer instead of an instance directly. * // only motivation for using a pointer instead of an instance directly.
* *
* // Upon construction, a body for world and a world frame are created, but no more bodies/frames exist until we add them. * // Upon construction, a body for world and a world frame are created, but no more bodies/frames exist until we add them.
* std::shared_ptr<RobotDynamics::Model> model(new RobotDynamics::Model()); * RobotDynamics::ModelPtr model(new RobotDynamics::Model());
* *
* // Create body 1 * // Create body 1
* RobotDynamics::Body body1(1.0, Math::Vector3d(0.,0.,-0.1), Math::Vector3d(0.1,0.1,0.1); * RobotDynamics::Body body1(1.0, Math::Vector3d(0.,0.,-0.1), Math::Vector3d(0.1,0.1,0.1);
...@@ -53,25 +53,25 @@ namespace RobotDynamics ...@@ -53,25 +53,25 @@ namespace RobotDynamics
* unsigned int body2_id = model->appendBody(Math::SpatialTransform(RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(0.,0.,-1.0))), JointTypeRevoluteX,body2,"body2"); * unsigned int body2_id = model->appendBody(Math::SpatialTransform(RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(0.,0.,-1.0))), JointTypeRevoluteX,body2,"body2");
* *
* // Each time a body is added, a reference frame is created for the body. These frames can be accessed by, * // Each time a body is added, a reference frame is created for the body. These frames can be accessed by,
* std::shared_ptr<RobotDynamics::ReferenceFrame> body1_frame = model->bodyFrames[body1_id]; * RobotDynamics::ReferenceFramePtr body1_frame = model->bodyFrames[body1_id];
* std::shared_ptr<RobotDynamics::ReferenceFrame> body2_frame = model->bodyFrames[body2_id]; * RobotDynamics::ReferenceFramePtr body2_frame = model->bodyFrames[body2_id];
* *
* // Frames are alse created at the body's center of mass. These frames can be accessed by, * // Frames are alse created at the body's center of mass. These frames can be accessed by,
* std::shared_ptr<RobotDynamics::ReferenceFrame> body1_com_frame = model->bodyCenteredFrames[body1_id]; * RobotDynamics::ReferenceFramePtr body1_com_frame = model->bodyCenteredFrames[body1_id];
* std::shared_ptr<RobotDynamics::ReferenceFrame> body2_com_frame = model->bodyCenteredFrames[body2_id]; * RobotDynamics::ReferenceFramePtr body2_com_frame = model->bodyCenteredFrames[body2_id];
* *
* // Now update kinematics(see the available functions from the kinematics module or look in Kinematics.h) * // Now update kinematics(see the available functions from the kinematics module or look in Kinematics.h)
* void updateKinematics(model, Q, QDot, QDDot) // Q,QDot, and QDDot here are vectors of joint positions, velocities, and accelerations * void updateKinematics(*model, Q, QDot, QDDot) // Q,QDot, and QDDot here are vectors of joint positions, velocities, and accelerations
* // Until kinematics have been updated, the transforms stored in each reference frame will not necessarily be correct. * // Until kinematics have been updated, the transforms stored in each reference frame will not necessarily be correct.
* // Also, you should be sure to update the kinematics each control tick. * // Also, you should be sure to update the kinematics each control tick.
* *
* // Now some examples of some simple things you can do utilizing the reference frames * // Now some examples of some simple things you can do utilizing the reference frames
* // Create a point at the origin of body2_frame * // Create a point at the origin of body2_frame
* FramePoint body2_origin(body2_frame.get(), 0.,0.,0.); * FramePoint body2_origin(body2_frame, 0.,0.,0.);
* *
* // Now lets express that point in body1_frame * // Now lets express that point in body1_frame
* body2_origin.changeFrame(body1_frame.get()) // body2_origin will now be expressed in the reference frame attached to body1 * body2_origin.changeFrame(body1_frame) // body2_origin will now be expressed in the reference frame attached to body1
* *
* // To express the frame point(or any geometric quantity that has a frame) in world frame, this con be done by, * // To express the frame point(or any geometric quantity that has a frame) in world frame, this con be done by,
* body2_origin.changeFrame(model->worldFrame); // Now it's in world frame * body2_origin.changeFrame(model->worldFrame); // Now it's in world frame
......
<?xml version="1.0"?> <?xml version="1.0"?>
<package format="2"> <package format="2">
<name>rdl</name> <name>rdl</name>
<version>1.1.0</version> <version>3.2.0</version>
<description>The rdl meta-package</description> <description>The rdl meta-package</description>
<maintainer email="jlack1987@gmail.com">jordan</maintainer> <maintainer email="jlack1987@gmail.com">jordan</maintainer>
......
CMAKE_MINIMUM_REQUIRED(VERSION 3.4) CMAKE_MINIMUM_REQUIRED(VERSION 3.4)
PROJECT (rdl_benchmark) PROJECT (rdl_benchmark)
find_package(catkin REQUIRED COMPONENTS rdl_cmake rdl_dynamics rdl_urdfreader) set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
# check c++11 / c++0x find_package(catkin REQUIRED COMPONENTS rdl_cmake rdl_dynamics rdl_urdfreader)
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "-std=c++0x")
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
set(INCLUDES_DIR include) set(INCLUDES_DIR include)
......
#ifndef _HUMAN36MODEL_H #ifndef _HUMAN36MODEL_H
#define _HUMAN36MODEL_H #define _HUMAN36MODEL_H
#include "rdl_dynamics/types.hpp"
namespace RobotDynamics namespace RobotDynamics
{ {
class Model; class Model;
} }
void generate_human36model(RobotDynamics::Model *model); void generate_human36model(RobotDynamics::ModelPtr model);
/* _HUMAN36MODEL_H */ /* _HUMAN36MODEL_H */
#endif #endif
#ifndef _SAMPLE_DATA_H #ifndef _RDL_BENCHMARK_SAMPLE_DATA_H__
#define _SAMPLE_DATA_H #define _RDL_BENCHMARK_SAMPLE_DATA_H__
#include "rdl_dynamics/rdl_eigenmath.h"
struct SampleData struct SampleData
{ {
......
#ifndef _MODEL_GENERATOR_H #ifndef _MODEL_GENERATOR_H
#define _MODEL_GENERATOR_H #define _MODEL_GENERATOR_H
#include "rdl_dynamics/types.hpp"
namespace RobotDynamics namespace RobotDynamics
{ {
class Model; class Model;
} }
void generate_planar_tree(RobotDynamics::Model *model, int depth); void generate_planar_tree(RobotDynamics::ModelPtr model, int depth);
/* _MODEL_GENERATOR_H */ /* _MODEL_GENERATOR_H */
#endif #endif
<?xml version="1.0"?> <?xml version="1.0"?>
<package format="2"> <package format="2">
<name>rdl_benchmark</name> <name>rdl_benchmark</name>
<version>1.1.0</version> <version>3.2.0</version>
<description>The rdl_benchmark package</description> <description>The rdl_benchmark package</description>
<maintainer email="jlack1987@gmail.com">jordan</maintainer> <maintainer email="jlack1987@gmail.com">jordan</maintainer>
......
...@@ -27,7 +27,7 @@ Body create_body(SegmentName segment) ...@@ -27,7 +27,7 @@ Body create_body(SegmentName segment)
return RobotDynamics::Body(SegmentMass[segment], RobotDynamics::Math::Vector3d(SegmentCOM[segment][0], SegmentCOM[segment][1], SegmentCOM[segment][2]), inertia_C); return RobotDynamics::Body(SegmentMass[segment], RobotDynamics::Math::Vector3d(SegmentCOM[segment][0], SegmentCOM[segment][1], SegmentCOM[segment][2]), inertia_C);
} }
void generate_human36model(RobotDynamics::Model *model) void generate_human36model(RobotDynamics::ModelPtr model)
{ {
Body pelvis_body = create_body(SegmentPelvis); Body pelvis_body = create_body(SegmentPelvis);
Body thigh_body = create_body(SegmentThigh); Body thigh_body = create_body(SegmentThigh);
......
...@@ -7,13 +7,14 @@ ...@@ -7,13 +7,14 @@
#include <iomanip> #include <iomanip>
#include <sstream> #include <sstream>
#include "rdl_dynamics/Contacts.h"
#include "rdl_dynamics/Dynamics.h"
#include "rdl_dynamics/Model.h"
#include "rdl_benchmark/model_generator.h" #include "rdl_benchmark/model_generator.h"
#include "rdl_benchmark/Human36Model.h" #include "rdl_benchmark/Human36Model.h"
#include "rdl_benchmark/SampleData.h" #include "rdl_benchmark/SampleData.h"
#include "rdl_benchmark/Timer.h" #include "rdl_benchmark/Timer.h"
#include "rdl_dynamics/Contacts.h"
#include "rdl_dynamics/Dynamics.h"
#include "rdl_dynamics/Model.h"
#include "rdl_dynamics/types.hpp"
#include "rdl_urdfreader/urdfreader.h" #include "rdl_urdfreader/urdfreader.h"
bool have_urdfreader = true; bool have_urdfreader = true;
...@@ -41,7 +42,7 @@ enum ContactsMethod ...@@ -41,7 +42,7 @@ enum ContactsMethod
ContactsMethodLagrangian = 0, ContactsMethodRangeSpaceSparse, ContactsMethodNullSpace, ContactsMethodKokkevis ContactsMethodLagrangian = 0, ContactsMethodRangeSpaceSparse, ContactsMethodNullSpace, ContactsMethodKokkevis
}; };
double run_forward_dynamics_ABA_benchmark(Model *model, int sample_count) double run_forward_dynamics_ABA_benchmark(ModelPtr model, int sample_count)
{ {
SampleData sample_data; SampleData sample_data;
sample_data.fillRandom(model->dof_count, sample_count); sample_data.fillRandom(model->dof_count, sample_count);
...@@ -61,7 +62,7 @@ double run_forward_dynamics_ABA_benchmark(Model *model, int sample_count) ...@@ -61,7 +62,7 @@ double run_forward_dynamics_ABA_benchmark(Model *model, int sample_count)
return duration; return duration;
} }
double run_forward_dynamics_lagrangian_benchmark(Model *model, int sample_count) double run_forward_dynamics_lagrangian_benchmark(ModelPtr model, int sample_count)
{ {
SampleData sample_data; SampleData sample_data;
sample_data.fillRandom(model->dof_count, sample_count); sample_data.fillRandom(model->dof_count, sample_count);
...@@ -74,7 +75,7 @@ double run_forward_dynamics_lagrangian_benchmark(Model *model, int sample_count) ...@@ -74,7 +75,7 @@ double run_forward_dynamics_lagrangian_benchmark(Model *model, int sample_count)
for(int i = 0; i < sample_count; i++) for(int i = 0; i < sample_count; i++)
{ {
forwardDynamicsLagrangian(*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], sample_data.qddot[i], Math::LinearSolverPartialPivLU, NULL, &H, &C); forwardDynamicsLagrangian(*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], sample_data.qddot[i], H, C, Math::LinearSolverPartialPivLU, NULL);
} }
double duration = timer_stop(&tinfo); double duration = timer_stop(&tinfo);
...@@ -84,7 +85,7 @@ double run_forward_dynamics_lagrangian_benchmark(Model *model, int sample_count) ...@@ -84,7 +85,7 @@ double run_forward_dynamics_lagrangian_benchmark(Model *model, int sample_count)
return duration; return duration;
} }
double run_inverse_dynamics_RNEA_benchmark(Model *model, int sample_count) double run_inverse_dynamics_RNEA_benchmark(ModelPtr model, int sample_count)
{ {
SampleData sample_data; SampleData sample_data;
sample_data.fillRandom(model->dof_count, sample_count); sample_data.fillRandom(model->dof_count, sample_count);
...@@ -104,7 +105,7 @@ double run_inverse_dynamics_RNEA_benchmark(Model *model, int sample_count) ...@@ -104,7 +105,7 @@ double run_inverse_dynamics_RNEA_benchmark(Model *model, int sample_count)
return duration; return duration;
} }
double run_CRBA_benchmark(Model *model, int sample_count) double run_CRBA_benchmark(ModelPtr model, int sample_count)
{ {
SampleData sample_data; SampleData sample_data;
sample_data.fillRandom(model->dof_count, sample_count); sample_data.fillRandom(model->dof_count, sample_count);
...@@ -128,7 +129,7 @@ double run_CRBA_benchmark(Model *model, int sample_count) ...@@ -128,7 +129,7 @@ double run_CRBA_benchmark(Model *model, int sample_count)
return duration; return duration;
} }
double run_nle_benchmark(Model *model, int sample_count) double run_nle_benchmark(ModelPtr model, int sample_count)
{ {
SampleData sample_data; SampleData sample_data;
sample_data.fillRandom(model->dof_count, sample_count); sample_data.fillRandom(model->dof_count, sample_count);
...@@ -148,7 +149,7 @@ double run_nle_benchmark(Model *model, int sample_count) ...@@ -148,7 +149,7 @@ double run_nle_benchmark(Model *model, int sample_count)
return duration; return duration;
} }
double run_calc_minv_times_tau_benchmark(Model *model, int sample_count) double run_calc_minv_times_tau_benchmark(ModelPtr model, int sample_count)
{ {
SampleData sample_data; SampleData sample_data;
sample_data.fillRandom(model->dof_count, sample_count); sample_data.fillRandom(model->dof_count, sample_count);
...@@ -170,7 +171,7 @@ double run_calc_minv_times_tau_benchmark(Model *model, int sample_count) ...@@ -170,7 +171,7 @@ double run_calc_minv_times_tau_benchmark(Model *model, int sample_count)
return duration; return duration;
} }
double run_contacts_lagrangian_benchmark(Model *model, ConstraintSet *constraint_set, int sample_count) double run_contacts_lagrangian_benchmark(ModelPtr model, ConstraintSet *constraint_set, int sample_count)
{ {
SampleData sample_data; SampleData sample_data;
sample_data.fillRandom(model->dof_count, sample_count); sample_data.fillRandom(model->dof_count, sample_count);
...@@ -188,7 +189,7 @@ double run_contacts_lagrangian_benchmark(Model *model, ConstraintSet *constraint ...@@ -188,7 +189,7 @@ double run_contacts_lagrangian_benchmark(Model *model, ConstraintSet *constraint
return duration; return duration;
} }
double run_contacts_lagrangian_sparse_benchmark(Model *model, ConstraintSet *constraint_set, int sample_count) double run_contacts_lagrangian_sparse_benchmark(ModelPtr model, ConstraintSet *constraint_set, int sample_count)
{ {
SampleData sample_data; SampleData sample_data;
sample_data.fillRandom(model->dof_count, sample_count); sample_data.fillRandom(model->dof_count, sample_count);
...@@ -206,7 +207,7 @@ double run_contacts_lagrangian_sparse_benchmark(Model *model, ConstraintSet *con ...@@ -206,7 +207,7 @@ double run_contacts_lagrangian_sparse_benchmark(Model *model, ConstraintSet *con
return duration; return duration;
} }
double run_contacts_null_space(Model *model, ConstraintSet *constraint_set, int sample_count) double run_contacts_null_space(ModelPtr model, ConstraintSet *constraint_set, int sample_count)
{ {
SampleData sample_data; SampleData sample_data;
sample_data.fillRandom(model->dof_count, sample_count); sample_data.fillRandom(model->dof_count, sample_count);
...@@ -224,7 +225,7 @@ double run_contacts_null_space(Model *model, ConstraintSet *constraint_set, int ...@@ -224,7 +225,7 @@ double run_contacts_null_space(Model *model, ConstraintSet *constraint_set, int
return duration; return duration;
} }
double run_contacts_kokkevis_benchmark(Model *model, ConstraintSet *constraint_set, int sample_count) double run_contacts_kokkevis_benchmark(ModelPtr model, ConstraintSet *constraint_set, int sample_count)
{ {
SampleData sample_data; SampleData sample_data;
sample_data.fillRandom(model->dof_count, sample_count); sample_data.fillRandom(model->dof_count, sample_count);
...@@ -245,7 +246,7 @@ double run_contacts_kokkevis_benchmark(Model *model, ConstraintSet *constraint_s ...@@ -245,7 +246,7 @@ double run_contacts_kokkevis_benchmark(Model *model, ConstraintSet *constraint_s
double contacts_benchmark(int sample_count, ContactsMethod contacts_method) double contacts_benchmark(int sample_count, ContactsMethod contacts_method)
{ {
// initialize the human model // initialize the human model
Model *model = new Model(); ModelPtr model(new Model());
generate_human36model(model); generate_human36model(model);
// initialize the constraint sets // initialize the constraint sets
...@@ -459,8 +460,6 @@ double contacts_benchmark(int sample_count, ContactsMethod contacts_method) ...@@ -459,8 +460,6 @@ double contacts_benchmark(int sample_count, ContactsMethod contacts_method)
cout << "ConstraintSet: 4 Bodies 4 Constraints: " << " duration = " << setw(10) << duration << "(s)" << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; cout << "ConstraintSet: 4 Bodies 4 Constraints: " << " duration = " << setw(10) << duration << "(s)" << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
delete model;
return duration; return duration;
} }
...@@ -599,9 +598,7 @@ int main(int argc, char *argv[]) ...@@ -599,9 +598,7 @@ int main(int argc, char *argv[])
{ {
parse_args(argc, argv); parse_args(argc, argv);
Model *model = NULL; ModelPtr model(new Model());
model = new Model();
if(model_file != "") if(model_file != "")
{ {
...@@ -637,8 +634,6 @@ int main(int argc, char *argv[]) ...@@ -637,8 +634,6 @@ int main(int argc, char *argv[])
run_nle_benchmark(model, benchmark_sample_count); run_nle_benchmark(model, benchmark_sample_count);
} }
delete model;
return 0; return 0;
} }
...@@ -649,14 +644,12 @@ int main(int argc, char *argv[]) ...@@ -649,14 +644,12 @@ int main(int argc, char *argv[])
cout << "= Forward Dynamics: ABA =" << endl; cout << "= Forward Dynamics: ABA =" << endl;
for(int depth = 1; depth <= benchmark_model_max_depth; depth++) for(int depth = 1; depth <= benchmark_model_max_depth; depth++)
{ {
model = new Model(); model.reset(new Model());
model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.); model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
generate_planar_tree(model, depth); generate_planar_tree(model, depth);
run_forward_dynamics_ABA_benchmark(model, benchmark_sample_count); run_forward_dynamics_ABA_benchmark(model, benchmark_sample_count);
delete model;
} }
cout << endl; cout << endl;
} }
...@@ -666,14 +659,12 @@ int main(int argc, char *argv[]) ...@@ -666,14 +659,12 @@ int main(int argc, char *argv[])
cout << "= Forward Dynamics: Lagrangian (Piv. LU decomposition) =" << endl; cout << "= Forward Dynamics: Lagrangian (Piv. LU decomposition) =" << endl;
for(int depth = 1; depth <= benchmark_model_max_depth; depth++) for(int depth = 1; depth <= benchmark_model_max_depth; depth++)
{ {
model = new Model(); model.reset(new Model());
model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.); model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
generate_planar_tree(model, depth); generate_planar_tree(model, depth);
run_forward_dynamics_lagrangian_benchmark(model, benchmark_sample_count); run_forward_dynamics_lagrangian_benchmark(model, benchmark_sample_count);
delete model;
} }
cout << endl; cout << endl;
} }
...@@ -683,14 +674,12 @@ int main(int argc, char *argv[]) ...@@ -683,14 +674,12 @@ int main(int argc, char *argv[])
cout << "= Inverse Dynamics: RNEA =" << endl; cout << "= Inverse Dynamics: RNEA =" << endl;
for(int depth = 1; depth <= benchmark_model_max_depth; depth++) for(int depth = 1; depth <= benchmark_model_max_depth; depth++)
{ {
model = new Model(); model.reset(new Model());
model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.); model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
generate_planar_tree(model, depth); generate_planar_tree(model, depth);
run_inverse_dynamics_RNEA_benchmark(model, benchmark_sample_count); run_inverse_dynamics_RNEA_benchmark(model, benchmark_sample_count);
delete model;
} }
cout << endl; cout << endl;
} }
...@@ -700,14 +689,12 @@ int main(int argc, char *argv[]) ...@@ -700,14 +689,12 @@ int main(int argc, char *argv[])
cout << "= Joint Space Inertia Matrix: CRBA =" << endl; cout << "= Joint Space Inertia Matrix: CRBA =" << endl;
for(int depth = 1; depth <= benchmark_model_max_depth; depth++) for(int depth = 1; depth <= benchmark_model_max_depth; depth++)
{ {
model = new Model(); model.reset(new Model());
model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.); model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
generate_planar_tree(model, depth); generate_planar_tree(model, depth);
run_CRBA_benchmark(model, benchmark_sample_count); run_CRBA_benchmark(model, benchmark_sample_count);
delete model;
} }
cout << endl; cout << endl;
} }
...@@ -717,14 +704,12 @@ int main(int argc, char *argv[]) ...@@ -717,14 +704,12 @@ int main(int argc, char *argv[])
cout << "= Nonlinear Effects =" << endl; cout << "= Nonlinear Effects =" << endl;
for(int depth = 1; depth <= benchmark_model_max_depth; depth++) for(int depth = 1; depth <= benchmark_model_max_depth; depth++)
{ {
model = new Model(); model.reset(new Model());
model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.); model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
generate_planar_tree(model, depth); generate_planar_tree(model, depth);
run_nle_benchmark(model, benchmark_sample_count); run_nle_benchmark(model, benchmark_sample_count);
delete model;
} }
cout << endl; cout << endl;
} }
...@@ -734,14 +719,12 @@ int main(int argc, char *argv[]) ...@@ -734,14 +719,12 @@ int main(int argc, char *argv[])
cout << "= CalcMInvTimesTau =" << endl; cout << "= CalcMInvTimesTau =" << endl;
for(int depth = 1; depth <= benchmark_model_max_depth; depth++) for(int depth = 1; depth <= benchmark_model_max_depth; depth++)
{ {
model = new Model(); model.reset(new Model());
model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.); model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
generate_planar_tree(model, depth); generate_planar_tree(model, depth);
run_calc_minv_times_tau_benchmark(model, benchmark_sample_count); run_calc_minv_times_tau_benchmark(model, benchmark_sample_count);
delete model;
} }
cout << endl; cout << endl;
} }
......
#include "rdl_benchmark/model_generator.h" #include "rdl_benchmark/model_generator.h"
#include "rdl_dynamics/Model.h" #include "rdl_dynamics/Model.h"
#include "rdl_dynamics/types.hpp"
using namespace RobotDynamics; using namespace RobotDynamics;
using namespace RobotDynamics::Math; using namespace RobotDynamics::Math;
void generate_planar_tree_recursive(Model *model, unsigned int parent_body_id, int depth, double length) void generate_planar_tree_recursive(ModelPtr model, unsigned int parent_body_id, int depth, double length)
{ {
if(depth == 0) if(depth == 0)
{ {
...@@ -26,7 +27,7 @@ void generate_planar_tree_recursive(Model *model, unsigned int parent_body_id, i ...@@ -26,7 +27,7 @@ void generate_planar_tree_recursive(Model *model, unsigned int parent_body_id, i
generate_planar_tree_recursive(model, child_right, depth - 1, length * 0.4); generate_planar_tree_recursive(model, child_right, depth - 1, length * 0.4);
} }
void generate_planar_tree(Model *model, int depth) void generate_planar_tree(ModelPtr model, int depth)