1#ifndef ROBOTOC_IMPACT_FRICTION_CONE_HPP_
2#define ROBOTOC_IMPACT_FRICTION_CONE_HPP_
102 template <typename VectorType1, typename VectorType2>
104 const Eigen::MatrixBase<VectorType1>& f_world,
105 const Eigen::Matrix3d& R_surface,
106 const Eigen::MatrixBase<VectorType2>& res) {
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);
123 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
126 int dimv_, dimc_, max_num_contacts_;
127 std::vector<int> contact_frame_;
128 std::vector<ContactType> contact_types_;
131 const int contact_idx)
const {
132 return data.
r[contact_idx];
135 Eigen::VectorXd& r(ConstraintComponentData& data,
136 const int contact_idx)
const {
137 return data.r[max_num_contacts_+contact_idx];
140 Eigen::MatrixXd& dg_dq(ConstraintComponentData& data,
141 const int contact_idx)
const {
142 return data.J[contact_idx];
145 Eigen::MatrixXd& dg_df(ConstraintComponentData& data,
146 const int contact_idx)
const {
147 return data.J[max_num_contacts_+contact_idx];
150 Eigen::MatrixXd& dfW_dq(ConstraintComponentData& data,
151 const int contact_idx)
const {
152 return data.J[2*max_num_contacts_+contact_idx];
155 Eigen::MatrixXd& r_dg_df(ConstraintComponentData& data,
156 const int contact_idx)
const {
157 return data.J[3*max_num_contacts_+contact_idx];
160 Eigen::MatrixXd& cone_local(ConstraintComponentData& data,
161 const int contact_idx)
const {
162 return data.J[4*max_num_contacts_+contact_idx];
165 Eigen::MatrixXd& cone_world(ConstraintComponentData& data,
166 const int contact_idx)
const {
167 return data.J[5*max_num_contacts_+contact_idx];
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