Skip to content
Snippets Groups Projects
Commit 29a5dc8b authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

Remove multicontact api related things

parent ea0edd75
No related branches found
No related tags found
2 merge requests!23After cmake and const refactor,!11Resolve "Turn ProcessorPointFeetNomove into a processor tracker"
......@@ -2,39 +2,12 @@
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})
add_executable(solo_real_povcdl_estimation solo_real_povcdl_estimation.cpp)
target_link_libraries(solo_real_povcdl_estimation
mcapi_utils
......
This diff is collapsed.
This diff is collapsed.
# 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_foot_nomove: 0.000001
std_foot_nomove: 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"
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#include "mcapi_utils.h"
Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr)
{
return ddr + w.cross(dr);
}
std::vector<Vector3d> contacts_from_footrect_center()
{
// compute feet corners coordinates like from the leg_X_6_joint
double lx = 0.1;
double ly = 0.065;
double lz = 0.107;
std::vector<Vector3d> contacts; contacts.resize(4);
contacts[0] = {-lx, -ly, -lz};
contacts[1] = {-lx, ly, -lz};
contacts[2] = { lx, -ly, -lz};
contacts[3] = { lx, ly, -lz};
return contacts;
}
Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts,
const Matrix<double, 12, 1>& cf12)
{
Vector3d f = cf12.segment<3>(0) + cf12.segment<3>(3) + cf12.segment<3>(6) + cf12.segment<3>(9);
Vector3d tau = contacts[0].cross(cf12.segment<3>(0))
+ contacts[1].cross(cf12.segment<3>(3))
+ contacts[2].cross(cf12.segment<3>(6))
+ contacts[3].cross(cf12.segment<3>(9));
Matrix<double, 6, 1> wrench; wrench << f, tau;
return wrench;
}
Matrix3d compute_Iw(pinocchio::Model& model,
pinocchio::Data& data,
VectorXd& q_static,
Vector3d& b_p_bc)
{
MatrixXd b_Mc = pinocchio::crba(model, data, q_static); // mass matrix at b frame expressed in b frame
// MatrixXd b_Mc = crba(model_dist, data_dist, q_static); // mass matrix at b frame expressed in b frame
MatrixXd b_I_b = b_Mc.topLeftCorner<6,6>(); // inertia matrix at b frame expressed in b frame
pinocchio::SE3 bTc (Eigen::Matrix3d::Identity(), b_p_bc); // "no free flyer robot -> c frame oriented the same as b"
pinocchio::SE3 cTb = bTc.inverse();
// Ic = cXb^star * Ib * bXc = bXc^T * Ib * bXc
// c_I_c block diagonal:
// [m*Id3, 03;
// 0, Iw]
MatrixXd c_I_c = cTb.toDualActionMatrix() * b_I_b * bTc.toActionMatrix();
Matrix3d b_Iw = c_I_c.bottomRightCorner<3,3>(); // meas
return b_Iw;
}
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#include <vector>
#include "Eigen/Dense"
#include "pinocchio/algorithm/crba.hpp"
using namespace Eigen;
/**
* \brief Compute a 3D acceleration from 6D spatial acceleration
*
* \param ddr spatial acc linear part
* \param w spatial acc rotational part
* \param dr spatial velocity linear part
**/
Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr);
/**
* \brief Compute the relative contact points from foot center of a Talos foot.
*
* Order is clockwise, starting from bottom left (looking at a forward pointing foot from above).
* Expressed in local foot coordinates.
**/
std::vector<Vector3d> contacts_from_footrect_center();
/**
* \brief Compute the wrench at the end effector center expressed in world coordinates.
* Each contact force (at a foot for instance) is represented in MAPI as a set of
* 4 forces at the corners of the contact polygone (foot -> rectangle). These forces,
* as the other quantities, are expressed in world coordinates. From this 4 3D forces,
* we can compute the 6D wrench at the center of the origin point of the contacts vectors.
* \param contacts std vector of 3D contact forces
* \param cforces 12D corner forces vector ordered as [fx1, fy1, fz1, fx2, fy2... fz4], same order as contacts
* \param wRl rotation matrix, orientation from world frame to foot frame
**/
Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts,
const Matrix<double, 12, 1>& cf12);
/**
* \brief Compute the centroidal angular inertia used to compute the angular momentum.
* \param model: pinocchio robot model used
* \param data: pinocchio robot data used
* \param q_static: configuration of the robot with free flyer pose at origin (local base frame = world frame)
* \param b_p_bc: measure of the CoM position relative to the base
**/
Matrix3d compute_Iw(pinocchio::Model& model,
pinocchio::Data& data,
VectorXd& q_static,
Vector3d& b_p_bc);
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment