Skip to content
Snippets Groups Projects
Commit 0b5dd05e authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Merge branch 'devel' into 'main' 

After cmake and const refactor

See merge request !23
parents 936dae25 c7951d46
No related branches found
No related tags found
1 merge request!23After cmake and const refactor
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 1987 additions and 2430 deletions
.gitignore
+ 4
− 2
View file @ 0b5dd05e
.cproject
\.cproject
.project
.settings/
README.txt
bin/
build/
build_debug/
build_release/
build-debug/
build-release/
lib/
.idea/
./Wolf.user
......@@ -30,6 +33,5 @@ src/CMakeFiles/cmake.check_cache
src/examples/map_apriltag_save.yaml
\.vscode/
build_release/
wolf.found
.gitlab-ci.yml
+ 10
− 0
View file @ 0b5dd05e
......@@ -3,6 +3,13 @@ stages:
- build_and_test
############ YAML ANCHORS ############
.print_variables_template: &print_variables_definition
# Print variables
- echo $WOLF_CORE_BRANCH
- echo $CI_COMMIT_BRANCH
- echo $WOLF_BODYDYNAMICS_BRANCH
- echo $WOLF_IMU_BRANCH
.preliminaries_template: &preliminaries_definition
## Install ssh-agent if not already installed, it is required by Docker.
## (change apt-get to yum if you use an RPM-based image)
......@@ -117,6 +124,7 @@ license_headers:
paths:
- ci_deps/wolf/
before_script:
- *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
script:
......@@ -134,6 +142,7 @@ build_and_test:bionic:
paths:
- ci_deps/imu/
before_script:
- *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_wolfimu_definition
......@@ -153,6 +162,7 @@ build_and_test:focal:
paths:
- ci_deps/imu/
before_script:
- *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_wolfimu_definition
......
CMakeLists.txt
+ 39
− 24
View file @ 0b5dd05e
# Pre-requisites about cmake itself
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
if(COMMAND cmake_policy)
cmake_policy(SET CMP0005 NEW)
cmake_policy(SET CMP0003 NEW)
endif(COMMAND cmake_policy)
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
......@@ -16,7 +12,8 @@ MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
SET(CMAKE_INSTALL_PREFIX /usr/local)
set(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
set(LIB_INSTALL_DIR lib/)
IF (NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE "DEBUG")
......@@ -67,10 +64,6 @@ if(BUILD_TESTS)
enable_testing()
endif()
#CMAKE modules
SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
MESSAGE(STATUS ${CMAKE_MODULE_PATH})
# Some wolf compilation options
IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
set(_WOLF_DEBUG true)
......@@ -79,12 +72,14 @@ ENDIF()
option(_WOLF_TRACE "Enable wolf tracing macro" ON)
# ============ DEPENDENCIES ============
FIND_PACKAGE(wolfcore REQUIRED)
FIND_PACKAGE(wolfcore REQUIRED CONFIG)
FIND_PACKAGE(wolfimu REQUIRED)
FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
# ============ CONFIG.H ============
string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
# variable used to compile the config.h.in file
string(TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
# Define the directory where will be the configured config.h
SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
......@@ -103,7 +98,6 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
include_directories("${PROJECT_BINARY_DIR}/conf")
# ============ INCLUDES ============
INCLUDE_DIRECTORIES(${wolfimu_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(BEFORE "include")
# ============ HEADERS ============
......@@ -121,6 +115,7 @@ include/bodydynamics/factor/factor_force_torque.h
include/bodydynamics/factor/factor_force_torque_preint.h
include/bodydynamics/factor/factor_inertial_kinematics.h
include/bodydynamics/factor/factor_point_feet_nomove.h
include/bodydynamics/factor/factor_point_feet_altitude.h
)
SET(HDRS_FEATURE
include/bodydynamics/feature/feature_force_torque.h
......@@ -184,11 +179,12 @@ endif()
#Link the created libraries
#===============EXAMPLE=========================
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfimu_LIBRARIES})
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC wolfcore)
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC wolfimu)
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC Eigen3::Eigen)
# Build demos
IF(BUILD_DEMOS)
#Build examples
MESSAGE("Building demos.")
ADD_SUBDIRECTORY(demos)
ENDIF(BUILD_DEMOS)
......@@ -199,14 +195,38 @@ IF(BUILD_TESTS)
add_subdirectory(test)
ENDIF(BUILD_TESTS)
#install library
#=============================================================
INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib)
LIBRARY DESTINATION ${LIB_INSTALL_DIR}
ARCHIVE DESTINATION ${LIB_INSTALL_DIR}
)
install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/${PLUGIN_NAME}/cmake)
# Configure the package installation
include(CMakePackageConfigHelpers)
configure_package_config_file(
${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake.in
${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
INSTALL_DESTINATION ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
PATH_VARS INCLUDE_INSTALL_DIR LIB_INSTALL_DIR
)
install(
FILES
${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
DESTINATION
${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
)
# Specifies include directories to use when compiling the plugin target
# This way, include_directories does not need to be called in plugins depending on this one
target_include_directories(${PLUGIN_NAME} INTERFACE
$<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
)
install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
#install headers
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
......@@ -221,17 +241,12 @@ INSTALL(FILES ${HDRS_PROCESSOR}
INSTALL(FILES ${HDRS_SENSOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
FILE(WRITE ${PROJECT_NAME}.found "")
INSTALL(FILES ${PROJECT_NAME}.found
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
export(PACKAGE ${PLUGIN_NAME})
FIND_PACKAGE(Doxygen)
FIND_PACKAGE(Doxygen MODULE)
FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
IF (IRI_DOC_DIR)
......
bodydynamics.found deleted 100644 → 0
+ 0
− 0
View file @ 936dae25
cmake_modules/wolfbodydynamicsConfig.cmake deleted 100644 → 0
+ 0
− 91
View file @ 936dae25
#edit the following line to add the librarie's header files
FIND_PATH(
wolfbodydynamics_INCLUDE_DIRS
NAMES bodydynamics.found
PATHS /usr/local/include/iri-algorithms/wolf/plugin_bodydynamics)
IF(wolfbodydynamics_INCLUDE_DIRS)
MESSAGE("Found wolf bodydynamics include dirs: ${wolfbodydynamics_INCLUDE_DIRS}")
ELSE(wolfbodydynamics_INCLUDE_DIRS)
MESSAGE("Couldn't find wolf bodydynamics include dirs")
ENDIF(wolfbodydynamics_INCLUDE_DIRS)
FIND_LIBRARY(
wolfbodydynamics_LIBRARIES
NAMES libwolfbodydynamics.so
PATHS /usr/local/lib)
IF(wolfbodydynamics_LIBRARIES)
MESSAGE("Found wolf bodydynamics lib: ${wolfbodydynamics_LIBRARIES}")
ELSE(wolfbodydynamics_LIBRARIES)
MESSAGE("Couldn't find wolf bodydynamics lib")
ENDIF(wolfbodydynamics_LIBRARIES)
IF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
SET(wolfbodydynamics_FOUND TRUE)
ELSE(wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
set(wolfbodydynamics_FOUND FALSE)
ENDIF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
IF (wolfbodydynamics_FOUND)
IF (NOT wolfbodydynamics_FIND_QUIETLY)
MESSAGE(STATUS "Found wolf bodydynamics: ${wolfbodydynamics_LIBRARIES}")
ENDIF (NOT wolfbodydynamics_FIND_QUIETLY)
ELSE (wolfbodydynamics_FOUND)
IF (wolfbodydynamics_FIND_REQUIRED)
MESSAGE(FATAL_ERROR "Could not find wolf bodydynamics")
ENDIF (wolfbodydynamics_FIND_REQUIRED)
ENDIF (wolfbodydynamics_FOUND)
macro(wolf_report_not_found REASON_MSG)
set(wolfbodydynamics_FOUND FALSE)
unset(wolfbodydynamics_INCLUDE_DIRS)
unset(wolfbodydynamics_LIBRARIES)
# Reset the CMake module path to its state when this script was called.
set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
# FindPackage() use the camelcase library name, not uppercase.
if (wolfbodydynamics_FIND_QUIETLY)
message(STATUS "Failed to find wolf bodydynamics- " ${REASON_MSG} ${ARGN})
elseif(wolfbodydynamics_FIND_REQUIRED)
message(FATAL_ERROR "Failed to find wolf bodydynamics - " ${REASON_MSG} ${ARGN})
else()
# Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
# that prevents generation, but continues configuration.
message(SEND_ERROR "Failed to find wolf bodydynamics - " ${REASON_MSG} ${ARGN})
endif ()
return()
endmacro(wolf_report_not_found)
if(NOT wolfbodydynamics_FOUND)
wolf_report_not_found("Something went wrong while setting up wolf bodydynamics.")
endif(NOT wolfbodydynamics_FOUND)
# Set the include directories for wolf (itself).
set(wolfbodydynamics_FOUND TRUE)
# Now we gather all the required dependencies for Wolf Template
#EXAMPLE Suppose that Wolf Template depends on laser_scan_utils
# FIND_PACKAGE(laser_scan_utils REQUIRED)
# list(APPEND wolflaser_INCLUDE_DIRS ${laser_scan_utils_INCLUDE_DIRS})
# list(APPEND wolflaser_LIBRARIES ${laser_scan_utils_LIBRARY})
# Making sure that wolf is always looked for
if(NOT wolf_FOUND)
FIND_PACKAGE(wolfcore REQUIRED)
#We reverse in order to insert at the start
list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
list(APPEND wolfbodydynamics_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
list(REVERSE wolfbodydynamics_LIBRARIES)
list(APPEND wolfbodydynamics_LIBRARIES ${wolfcore_LIBRARIES})
list(REVERSE wolfbodydynamics_LIBRARIES)
endif()
# provide both INCLUDE_DIR and INCLUDE_DIRS
SET(wolfbodydynamics_INCLUDE_DIR ${wolfbodydynamics_INCLUDE_DIRS})
# provide both LIBRARY and LIBRARIES
SET(wolfbodydynamics_LIBRARY ${wolfbodydynamics_LIBRARIES})
\ No newline at end of file
cmake_modules/wolfbodydynamicsConfig.cmake.in 0 → 100644
+ 13
− 0
View file @ 0b5dd05e
set(@PLUGIN_NAME@_VERSION 0.0.1)
@PACKAGE_INIT@
include(CMakeFindDependencyMacro)
FIND_DEPENDENCY(wolfcore REQUIRED)
FIND_DEPENDENCY(wolfimu REQUIRED)
FIND_DEPENDENCY(Eigen3 3.3 REQUIRED)
include("${CMAKE_CURRENT_LIST_DIR}/@PLUGIN_NAME@Targets.cmake")
check_required_components(@PLUGIN_NAME@)
\ No newline at end of file
demos/CMakeLists.txt
+ 36
− 47
View file @ 0b5dd05e
......@@ -2,74 +2,63 @@
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fext-numeric-literals") # necessary for files using boost
FIND_PACKAGE(pinocchio QUIET)
FIND_PACKAGE(multicontact-api QUIET)
if (pinocchio_FOUND)
# SYSTEM disables warnings from library headers
include_directories(
SYSTEM ${PINOCCHIO_INCLUDE_DIRS}
)
add_library(mcapi_utils mcapi_utils.cpp)
# add_executable(mcapi_povcdl_estimation mcapi_povcdl_estimation.cpp)
# target_link_libraries(mcapi_povcdl_estimation
# mcapi_utils
# ${wolfcore_LIBRARIES}
# ${wolfimu_LIBRARIES}
# ${PLUGIN_NAME}
# ${multicontact-api_LIBRARIES}
# ${pinocchio_LIBRARIES}
# )
# target_compile_definitions(mcapi_povcdl_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
# add_executable(mcapi_pov_estimation mcapi_pov_estimation.cpp)
# target_link_libraries(mcapi_pov_estimation
# mcapi_utils
# ${wolfcore_LIBRARIES}
# ${wolfimu_LIBRARIES}
# ${PLUGIN_NAME}
# ${multicontact-api_LIBRARIES}
# ${pinocchio_LIBRARIES}
# )
# target_compile_definitions(mcapi_pov_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
target_link_libraries(mcapi_utils Eigen3::Eigen)
add_executable(solo_real_povcdl_estimation solo_real_povcdl_estimation.cpp)
target_link_libraries(solo_real_povcdl_estimation
mcapi_utils
${wolfcore_LIBRARIES}
${wolfimu_LIBRARIES}
wolfcore
wolfimu
${PLUGIN_NAME}
${pinocchio_LIBRARIES}
/usr/local/lib/libcnpy.so
z
mcapi_utils
)
add_executable(solo_real_pov_estimation solo_real_pov_estimation.cpp)
target_link_libraries(solo_real_pov_estimation
mcapi_utils
${wolfcore_LIBRARIES}
${wolfimu_LIBRARIES}
add_executable(solo_imu_mocap solo_imu_mocap.cpp)
target_link_libraries(solo_imu_mocap
wolfcore
wolfimu
${PLUGIN_NAME}
${pinocchio_LIBRARIES}
/usr/local/lib/libcnpy.so
z
)
add_executable(solo_mocap_imu solo_mocap_imu.cpp)
target_link_libraries(solo_mocap_imu
${wolfcore_LIBRARIES}
${wolfimu_LIBRARIES}
${PLUGIN_NAME}
${pinocchio_LIBRARIES}
/usr/local/lib/libcnpy.so
z
add_executable(solo_imu_kine_mocap solo_imu_kine_mocap.cpp)
target_link_libraries(solo_imu_kine_mocap
wolfcore
wolfimu
${PLUGIN_NAME}
${pinocchio_LIBRARIES}
/usr/local/lib/libcnpy.so
z
)
endif()
# add_executable(test_cnpy test_cnpy.cpp)
# target_link_libraries(test_cnpy
# add_library(demo_logger demo_logger.cpp)
# target_include_directories (demo_logger PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
# add_executable(solo_imu_mocap_refactored solo_imu_mocap_refactored.cpp)
# target_link_libraries(solo_imu_mocap_refactored
# demo_logger
# ${wolfcore_LIBRARIES}
# ${wolfimu_LIBRARIES}
# ${PLUGIN_NAME}
# ${pinocchio_LIBRARIES}
# /usr/local/lib/libcnpy.so
# z
# )
demos/mcapi_pov_estimation.cpp deleted 100644 → 0
+ 0
− 751
View file @ 936dae25
This diff is collapsed.
demos/mcapi_povcdl_estimation.cpp deleted 100644 → 0
+ 0
− 969
View file @ 936dae25
This diff is collapsed.
demos/mcapi_povcdl_estimation.yaml deleted 100644 → 0
+ 0
− 76
View file @ 936dae25
# trajectory handling
dt: 0.001
min_t: -1.0 # -1 means from the beginning of the trajectory
max_t: -1 # -1 means until the end of the trajectory
solve_every_sec: -1 # < 0 strict --> no solve
solve_end: False
# solve_every_sec: 0.3 # < 0 strict --> no solve
# solve_end: True
# estimator sensor noises
std_acc_est: 0.001
std_gyr_est: 0.001
std_pbc_est: 0.001
std_vbc_est: 0.001 # higher than simulated -> has to take care of the non-modeled bias as well... Good value for solo sin traj
std_f_est: 0.001
std_tau_est: 0.001
# std_odom3d_est: 0.000001
std_odom3d_est: 100000
# simulated sensor noises
std_acc_sim: 0.0001
std_gyr_sim: 0.0001
std_pbc_sim: 0.0001
std_vbc_sim: 0.0001
std_f_sim: 0.0001
std_tau_sim: 0.0001
std_odom3d_sim: 100000000
# some controls
fz_thresh: 1
noisy_measurements: False
# priors
std_prior_p: 0.01
std_prior_o: 0.1
std_prior_v: 0.1
# std_abs_biasimu: 100000
std_abs_biasimu: 0.0000001 # almost fixed
std_abs_bias_pbc: 10000 # noprior
# std_abs_bias_pbc: 0.00001 # almost fixed
std_bp_drift: 10000 # All the drift you want
# std_bp_drift: 0.1 # no drift
bias_pbc_prior: [0,0,0]
# model disturbance
scale_dist: 0.00 # disturbance of link inertia
mass_dist: False
base_dist_only: False
# which measurement/parameter is disturbed?
vbc_is_dist: False
Iw_is_dist: False
Lgest_is_dist: False
# Robot model
# robot_urdf: "/opt/openrobots/share/example-robot-data/robots/talos_data/robots/talos_reduced.urdf"
# dimc: 6
robot_urdf: "/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf"
dimc: 3
# Trajectory files
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/talos_sin_traj.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/talos_contact_switch.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj.cs"
contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_traj_pyb.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_y_notrunk.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_r+y.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_sin_r+z.cs"
# contact_sequence_file: "/home/mfourmy/Documents/Phd_LAAS/data/trajs/solo_stamping.cs"
demos/processor_imu_solo12.yaml
+ 1
− 1
View file @ 0b5dd05e
......@@ -2,7 +2,7 @@ keyframe_vote:
angle_turned: 1000
dist_traveled: 20000.0
max_buff_length: 100000000000
max_time_span: 0.19999
max_time_span: 0.05
voting_active: true
name: Main imu
time_tolerance: 0.0005
......
demos/sensor_imu_solo12.yaml
+ 9
− 9
View file @ 0b5dd05e
type: "SensorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
name: "Main Imu Sensor" # This is ignored. The name provided to the SensorFactory prevails
motion_variances:
a_noise: 0.05 # standard deviation of Acceleration noise (same for all the axis) in m/s2
w_noise: 0.005 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
ab_initial_stdev: 0.00 # m/s2 - initial bias
wb_initial_stdev: 0.0 # rad/sec - initial bias
ab_rate_stdev: 0.0001 # m/s2/sqrt(s)
wb_rate_stdev: 0.0001 # rad/s/sqrt(s)
\ No newline at end of file
motion_variances:
a_noise: 0.02
ab_initial_stdev: 0.0
ab_rate_stdev: 1.0e-06
w_noise: 0.03
wb_initial_stdev: 0.0
wb_rate_stdev: 1.0e-06
name: Main Imu Sensor
type: SensorImu
demos/solo_imu_kine.cpp 0 → 100644
+ 742
− 0
View file @ 0b5dd05e
This diff is collapsed.
demos/solo_mocap_imu.cpp → demos/solo_imu_mocap.cpp
+ 184
− 95
View file @ 0b5dd05e
This diff is collapsed.
demos/solo_kine_mocap.cpp 0 → 100644
+ 640
− 0
View file @ 0b5dd05e
This diff is collapsed.
demos/solo_real_estimation.yaml
+ 17
− 91
View file @ 0b5dd05e
This diff is collapsed.
demos/solo_real_povcdl_estimation.cpp
+ 14
− 13
View file @ 0b5dd05e
......@@ -69,7 +69,6 @@
#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
#include "bodydynamics/processor/processor_point_feet_nomove.h"
// UTILS
#include "mcapi_utils.h"
......@@ -88,7 +87,7 @@ int main (int argc, char **argv) {
int dimc = config["dimc"].as<int>();
double mass_offset = config["mass_offset"].as<double>();
double dt = config["dt"].as<double>();
double min_t = config["min_t"].as<double>();
// double min_t = config["min_t"].as<double>();
double max_t = config["max_t"].as<double>();
double solve_every_sec = config["solve_every_sec"].as<double>();
bool solve_end = config["solve_end"].as<bool>();
......@@ -101,7 +100,7 @@ int main (int argc, char **argv) {
double std_vbc_est = config["std_vbc_est"].as<double>();
double std_f_est = config["std_f_est"].as<double>();
double std_tau_est = config["std_tau_est"].as<double>();
double std_odom3d_est = config["std_odom3d_est"].as<double>();
double std_foot_nomove = config["std_foot_nomove"].as<double>();
// priors
double std_prior_p = config["std_prior_p"].as<double>();
......@@ -303,7 +302,7 @@ int main (int argc, char **argv) {
// SENSOR + PROCESSOR POINT FEET NOMOVE
ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
intr_pfn->std_foot_nomove_ = std_odom3d_est;
intr_pfn->std_foot_nomove_ = std_foot_nomove;
VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
......@@ -334,7 +333,7 @@ int main (int argc, char **argv) {
// 1. Call setOrigin processorIMU to generate a fake captureIMU -> generates b_imu
// 2. process one IKin capture corresponding to the same KF, the factor needing b_imu -> generates a b_pbc
// 3. set processorForceTorquePreint origin which requires both b_imu and b_pbc
proc_inkin->keyFrameCallback(KF1, 0.0005);
proc_inkin->keyFrameCallback(KF1);
proc_imu->setOrigin(KF1);
// proc_legodom->setOrigin(KF1);
......@@ -577,19 +576,20 @@ int main (int argc, char **argv) {
}
}
CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
CPF->process();
// TODO: ???? kin_incontact is a map to Vector3d in this implementation, should be Vector7d [p, q]
// CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
// CPF->process();
// add zero vel artificial factor
if (problem->getTrajectory()->getFrameMap().size() > nb_kf){
auto kf_pair = problem->getTrajectory()->getFrameMap().rbegin();
std::cout << "New KF " << kf_pair->first << std::endl;
auto kf = kf_pair->second;
if (problem->getTrajectory()->size() > nb_kf)
{
auto kf = problem->getTrajectory()->getLastFrame();
std::cout << "New KF " << kf->getTimeStamp() << std::endl;
// CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(kf, "Vel0", kf_pair->first);
// CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(kf, "Vel0", kf->getTimeStamp());
// FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", Vector3d::Zero(), 0.00001*Matrix3d::Ones());
// FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, kf->getV(), nullptr, false);
......@@ -728,7 +728,8 @@ int main (int argc, char **argv) {
VectorComposite bp_bias_est;
CaptureBasePtr cap_imu;
VectorComposite bi_bias_est;
for (auto& elt: problem->getTrajectory()->getFrameMap()){
for (auto elt: problem->getTrajectory()->getFrameMap())
{
auto kf = elt.second;
kf_state = kf->getState();
cap_ikin = kf->getCaptureOf(sen_ikin);
......
demos/tree_manager.yaml
+ 1
− 1
View file @ 0b5dd05e
......@@ -2,6 +2,6 @@ config:
problem:
tree_manager:
n_fix_first_frames: 3
n_frames: 100000000000
n_frames: 50000
type: TreeManagerSlidingWindow
viral_remove_empty_parent: true
include/bodydynamics/capture/capture_force_torque_preint.h
+ 3
− 1
View file @ 0b5dd05e
......@@ -62,7 +62,9 @@ class CaptureForceTorquePreint : public CaptureMotion
~CaptureForceTorquePreint() override;
CaptureBasePtr getIkinCaptureOther(){ return cap_ikin_other_; }
CaptureBaseConstPtr getIkinCaptureOther() const { return cap_ikin_other_;}
CaptureBasePtr getIkinCaptureOther(){ return cap_ikin_other_;}
CaptureBaseConstPtr getGyroCaptureOther() const { return cap_gyro_other_;}
CaptureBasePtr getGyroCaptureOther(){ return cap_gyro_other_;}
private:
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment