1 |
|
|
/** |
2 |
|
|
* @file |
3 |
|
|
* @copyright Copyright (c) 2022, LAAS-CNRS, Toward, PalRobotics |
4 |
|
|
* @brief |
5 |
|
|
*/ |
6 |
|
|
|
7 |
|
|
#include "dynacom/contact6d.hpp" |
8 |
|
|
#include "pinocchio/algorithm/geometry.hpp" |
9 |
|
|
|
10 |
|
|
namespace dynacom { |
11 |
|
|
|
12 |
|
|
Contact6D::Contact6D() {} |
13 |
|
|
Contact6D::Contact6D(const Contact6DSettings &settings) { |
14 |
|
|
initialize(settings); |
15 |
|
|
} |
16 |
|
|
void Contact6D::initialize(const Contact6DSettings &settings) { |
17 |
|
|
settings_ = settings; |
18 |
|
|
double hl = settings_.half_length; |
19 |
|
|
double hw = settings_.half_width; |
20 |
|
|
double mu = settings_.mu; |
21 |
|
|
double gu = settings_.gu; |
22 |
|
|
|
23 |
|
|
// Make matrices |
24 |
|
|
regularization_A_ << settings_.weights; |
25 |
|
|
regularization_b_ << Eigen::Matrix<double, 6, 1>::Zero(); |
26 |
|
|
|
27 |
|
|
unilaterality_A_ << Eigen::Matrix<double, 5, 6>::Zero(); |
28 |
|
|
unilaterality_A_.block<5, 1>(0, 2) << -1, -hl, -hw, -hl, -hw; |
29 |
|
|
unilaterality_A_.block<2, 2>(1, 3) << 0, -1, 1, 0; |
30 |
|
|
unilaterality_A_.block<2, 2>(3, 3) << 0, 1, -1, 0; |
31 |
|
|
unilaterality_b_ << Eigen::Matrix<double, 5, 1>::Zero(); |
32 |
|
|
|
33 |
|
|
friction_A_ << Eigen::Matrix<double, 6, 6>::Zero(); |
34 |
|
|
friction_A_.block<6, 1>(0, 2) << -mu, -mu, -mu, -mu, -gu, -gu; |
35 |
|
|
friction_A_.block<2, 2>(0, 0) << Eigen::Matrix2d::Identity(); |
36 |
|
|
friction_A_.block<2, 2>(2, 0) << -Eigen::Matrix2d::Identity(); |
37 |
|
|
friction_A_.block<2, 1>(4, 5) << 1, -1; |
38 |
|
|
friction_b_ << Eigen::Matrix<double, 6, 1>::Zero(); |
39 |
|
|
|
40 |
|
|
newton_euler_A_ << Eigen::Matrix<double, 6, 6>::Zero(); |
41 |
|
|
newton_euler_A_.block<3, 3>(0, 0) << Eigen::Matrix3d::Identity(); |
42 |
|
|
newton_euler_A_.block<3, 3>(3, 3) << Eigen::Matrix3d::Identity(); |
43 |
|
|
|
44 |
|
|
// Note: newton_euler must be updated before using it |
45 |
|
|
contactForce_ = Eigen::Matrix<double, 6, 1>::Zero(); |
46 |
|
|
} |
47 |
|
|
|
48 |
|
|
void Contact6D::setForceWeights(const Eigen::Vector3d &force_weights) { |
49 |
|
|
settings_.weights.head<3>() = force_weights; |
50 |
|
|
regularization_A_.head<3>() = force_weights; |
51 |
|
|
} |
52 |
|
|
|
53 |
|
|
void Contact6D::setTorqueWeights(const Eigen::Vector3d &torque_weights) { |
54 |
|
|
settings_.weights.tail<3>() = torque_weights; |
55 |
|
|
regularization_A_.tail<3>() = torque_weights; |
56 |
|
|
} |
57 |
|
|
|
58 |
|
|
void Contact6D::setSurfaceHalfWidth(const double &half_width) { |
59 |
|
|
settings_.half_width = half_width; |
60 |
|
|
unilaterality_A_(2, 2) = -half_width; |
61 |
|
|
unilaterality_A_(4, 2) = -half_width; |
62 |
|
|
} |
63 |
|
|
|
64 |
|
|
void Contact6D::setSurfaceHalfLength(const double &half_length) { |
65 |
|
|
settings_.half_length = half_length; |
66 |
|
|
unilaterality_A_(1, 2) = -half_length; |
67 |
|
|
unilaterality_A_(3, 2) = -half_length; |
68 |
|
|
} |
69 |
|
|
|
70 |
|
|
void Contact6D::setMu(const double &mu) { |
71 |
|
|
settings_.mu = mu; |
72 |
|
|
friction_A_.block<4, 1>(0, 2) << -mu, -mu, -mu, -mu; |
73 |
|
|
} |
74 |
|
|
|
75 |
|
|
void Contact6D::setGu(const double &gu) { |
76 |
|
|
settings_.gu = gu; |
77 |
|
|
friction_A_.block<2, 1>(4, 2) << -gu, -gu; |
78 |
|
|
} |
79 |
|
|
|
80 |
|
|
void Contact6D::updateNewtonEuler(const Eigen::Vector3d &CoM, |
81 |
|
|
const pinocchio::SE3 &oMs) { |
82 |
|
|
/** |
83 |
|
|
* @brief Assuming that the orientation of the world frame is the identity. |
84 |
|
|
* |
85 |
|
|
*/ |
86 |
|
|
|
87 |
|
|
oMs_ = oMs; |
88 |
|
|
cMo_ = pinocchio::SE3(Eigen::Matrix3d::Identity(), -CoM); |
89 |
|
|
|
90 |
|
|
newton_euler_A_ << (cMo_.act(oMs_)).toActionMatrixInverse().transpose(); |
91 |
|
|
} |
92 |
|
|
} // namespace dynacom |