robotoc
robotoc - efficient ROBOT Optimal Control solvers
Loading...
Searching...
No Matches
impact_friction_cone.hpp
Go to the documentation of this file.
1#ifndef ROBOTOC_IMPACT_FRICTION_CONE_HPP_
2#define ROBOTOC_IMPACT_FRICTION_CONE_HPP_
3
4#include "Eigen/Core"
5
14
15
16namespace robotoc {
17
23public:
24 using Vector5d = Eigen::Matrix<double, 5, 1>;
25
31
36
41
46
51
56
60 ImpactFrictionCone& operator=(ImpactFrictionCone&&) noexcept = default;
61
63
64 void allocateExtraData(ConstraintComponentData& data) const override;
65
66 bool isFeasible(Robot& robot, const ImpactStatus& impact_status,
68 const SplitSolution& s) const override;
69
70 void setSlack(Robot& robot, const ImpactStatus& impact_status,
72 const SplitSolution& s) const override;
73
74 void evalConstraint(Robot& robot, const ImpactStatus& impact_status,
76 const SplitSolution& s) const override;
77
78 void evalDerivatives(Robot& robot, const ImpactStatus& impact_status,
80 const SplitSolution& s,
81 SplitKKTResidual& kkt_residual) const override;
82
83 void condenseSlackAndDual(const ImpactStatus& impact_status,
85 SplitKKTMatrix& kkt_matrix,
86 SplitKKTResidual& kkt_residual) const override;
87
88 void expandSlackAndDual(const ImpactStatus& impact_status,
90 const SplitDirection& d) const override;
91
92 int dimc() const override;
93
102 template <typename VectorType1, typename VectorType2>
103 static void frictionConeResidual(const double mu,
104 const Eigen::MatrixBase<VectorType1>& f_world,
105 const Eigen::Matrix3d& R_surface,
106 const Eigen::MatrixBase<VectorType2>& res) {
107 assert(mu > 0);
108 assert(f_world.size() == 3);
109 assert((R_surface*R_surface.transpose()).isIdentity());
110 assert(res.size() == 5);
111 const Eigen::Vector3d f_local = R_surface.transpose() * f_world;
112 const_cast<Eigen::MatrixBase<VectorType2>&>(res).coeffRef(0) = - f_local.coeff(2);
113 const_cast<Eigen::MatrixBase<VectorType2>&>(res).coeffRef(1)
114 = f_local.coeff(0) - mu * f_local.coeff(2) / std::sqrt(2);
115 const_cast<Eigen::MatrixBase<VectorType2>&>(res).coeffRef(2)
116 = - f_local.coeff(0) - mu * f_local.coeff(2) / std::sqrt(2);
117 const_cast<Eigen::MatrixBase<VectorType2>&>(res).coeffRef(3)
118 = f_local.coeff(1) - mu * f_local.coeff(2) / std::sqrt(2);
119 const_cast<Eigen::MatrixBase<VectorType2>&>(res).coeffRef(4)
120 = - f_local.coeff(1) - mu * f_local.coeff(2) / std::sqrt(2);
121 }
122
123 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
124
125private:
126 int dimv_, dimc_, max_num_contacts_;
127 std::vector<int> contact_frame_;
128 std::vector<ContactType> contact_types_;
129
130 Eigen::VectorXd& fW(ConstraintComponentData& data,
131 const int contact_idx) const {
132 return data.r[contact_idx];
133 }
134
135 Eigen::VectorXd& r(ConstraintComponentData& data,
136 const int contact_idx) const {
137 return data.r[max_num_contacts_+contact_idx];
138 }
139
140 Eigen::MatrixXd& dg_dq(ConstraintComponentData& data,
141 const int contact_idx) const {
142 return data.J[contact_idx];
143 }
144
145 Eigen::MatrixXd& dg_df(ConstraintComponentData& data,
146 const int contact_idx) const {
147 return data.J[max_num_contacts_+contact_idx];
148 }
149
150 Eigen::MatrixXd& dfW_dq(ConstraintComponentData& data,
151 const int contact_idx) const {
152 return data.J[2*max_num_contacts_+contact_idx];
153 }
154
155 Eigen::MatrixXd& r_dg_df(ConstraintComponentData& data,
156 const int contact_idx) const {
157 return data.J[3*max_num_contacts_+contact_idx];
158 }
159
160 Eigen::MatrixXd& cone_local(ConstraintComponentData& data,
161 const int contact_idx) const {
162 return data.J[4*max_num_contacts_+contact_idx];
163 }
164
165 Eigen::MatrixXd& cone_world(ConstraintComponentData& data,
166 const int contact_idx) const {
167 return data.J[5*max_num_contacts_+contact_idx];
168 }
169
170};
171
172} // namespace robotoc
173
174#endif // ROBOTOC_IMPACT_FRICTION_CONE_HPP_
Data used in constraint components. Composed by slack, dual (Lagrange multiplier),...
Definition: constraint_component_data.hpp:17
std::vector< Eigen::VectorXd > r
std vector of Eigen::VectorXd used to store residual temporaly. Only be allocated in ConstraintCompon...
Definition: constraint_component_data.hpp:108
Base class for impact constraint components.
Definition: impact_constraint_component_base.hpp:24
Constraint on the inner-approximated impact firction cone.
Definition: impact_friction_cone.hpp:22
Eigen::Matrix< double, 5, 1 > Vector5d
Definition: impact_friction_cone.hpp:24
KinematicsLevel kinematicsLevel() const override
Checks the kinematics level of the constraint component.
bool isFeasible(Robot &robot, const ImpactStatus &impact_status, ConstraintComponentData &data, const SplitSolution &s) const override
Checks whether the current solution s is feasible or not.
void evalConstraint(Robot &robot, const ImpactStatus &impact_status, ConstraintComponentData &data, const SplitSolution &s) const override
Computes the primal residual, residual in the complementary slackness, and the log-barrier function o...
ImpactFrictionCone(ImpactFrictionCone &&) noexcept=default
Default move constructor.
ImpactFrictionCone & operator=(const ImpactFrictionCone &)=default
Default copy operator.
void allocateExtraData(ConstraintComponentData &data) const override
Allocates extra data in ConstraintComponentData.
void condenseSlackAndDual(const ImpactStatus &impact_status, ConstraintComponentData &data, SplitKKTMatrix &kkt_matrix, SplitKKTResidual &kkt_residual) const override
Condenses the slack and dual variables, i.e., factorizes the condensed Hessians and KKT residuals....
void evalDerivatives(Robot &robot, const ImpactStatus &impact_status, ConstraintComponentData &data, const SplitSolution &s, SplitKKTResidual &kkt_residual) const override
Computes the derivatives of the priaml residual, i.e., the Jacobian of the inequality constraint,...
ImpactFrictionCone()
Default constructor.
void setSlack(Robot &robot, const ImpactStatus &impact_status, ConstraintComponentData &data, const SplitSolution &s) const override
Sets the slack variables of each constraint components.
ImpactFrictionCone(const Robot &robot)
Constructor.
ImpactFrictionCone(const ImpactFrictionCone &)=default
Default copy constructor.
int dimc() const override
Returns the size of the constraint.
static void frictionConeResidual(const double mu, const Eigen::MatrixBase< VectorType1 > &f_world, const Eigen::Matrix3d &R_surface, const Eigen::MatrixBase< VectorType2 > &res)
Computes the friction cone residual.
Definition: impact_friction_cone.hpp:103
void expandSlackAndDual(const ImpactStatus &impact_status, ConstraintComponentData &data, const SplitDirection &d) const override
Expands the slack and dual, i.e., computes the directions of the slack and dual variables from the di...
~ImpactFrictionCone()
Destructor.
Impact status of robot model. Wrapper of ContactStatus to treat impacts.
Definition: impact_status.hpp:21
Dynamics and kinematics model of robots. Wraps pinocchio::Model and pinocchio::Data....
Definition: robot.hpp:32
Newton direction of the solution to the optimal control problem split into a time stage.
Definition: split_direction.hpp:20
The KKT matrix split into a time stage.
Definition: split_kkt_matrix.hpp:18
KKT residual split into each time stage.
Definition: split_kkt_residual.hpp:18
Solution to the optimal control problem split into a time stage.
Definition: split_solution.hpp:20
Definition: constraint_component_base.hpp:17
KinematicsLevel
Kinematics level of the constraint component used in ConstraintComponentBase.
Definition: constraint_component_base.hpp:24