Commit a5e8eea1 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

[QPStaticStability] Refactor and document.

parent 5a4f363f
......@@ -42,6 +42,17 @@ namespace hpp {
/// \addtogroup constraints
/// \{
///
/// Quasi static equilibrium constraint defined as a QP program
///
/// This class defines a function whose value is equal to zero
/// if and only if there exists a set of non-negative forces applied
/// at some given contact points along some given normals that
// counter-balance the gravity force and momentum of a kinematics chain.
///
/// \sa https://hal.archives-ouvertes.fr/tel-01516897 Chapter 2, Sections
/// 3.1 and 3.2.
class HPP_CONSTRAINTS_DLLAPI QPStaticStability : public DifferentiableFunction {
public:
static const Eigen::Matrix <value_type, 6, 1> Gravity;
......@@ -51,7 +62,9 @@ namespace hpp {
typedef ConvexShapeContact::ForceData ForceData;
/// Constructor
/// \param name name of the constraint
/// \param robot the robot the constraints is applied to,
/// \param contacts set of contact points
/// \param com COM of the robot
QPStaticStability (const std::string& name, const DevicePtr_t& robot,
const Contacts_t& contacts,
......@@ -89,7 +102,7 @@ namespace hpp {
const QPStaticStability& castother = dynamic_cast<const QPStaticStability&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
if (name() != castother.name())
return false;
if (robot_ != castother.robot_)
......@@ -102,7 +115,7 @@ namespace hpp {
return false;
if (nWSR != castother.nWSR)
return false;
return true;
}
private:
......
......@@ -50,16 +50,13 @@ namespace hpp {
static const Eigen::Matrix <value_type, 6, 1> Gravity;
struct Contact_t {
JointPtr_t joint1, joint2;
vector3_t point1, point2;
vector3_t normal1, normal2;
JointPtr_t joint;
vector3_t point;
vector3_t normal;
bool operator==(Contact_t const& other) const {
if (joint1 != other.joint1) return false;
if (joint2 != other.joint2) return false;
if (point1 != other.point1) return false;
if (point2 != other.point2) return false;
if (normal1 != other.normal1) return false;
if (normal2 != other.normal2) return false;
if (joint != other.joint) return false;
if (point != other.point) return false;
if (normal != other.normal) return false;
return true;
}
};
......
......@@ -77,9 +77,9 @@ namespace hpp {
Traits<PointCom>::Ptr_t OG = PointCom::create(com);
for (std::size_t i = 0; i < contacts.size(); ++i) {
Traits<PointInJoint>::Ptr_t OP2 = PointInJoint::create
(contacts[i].joint2,contacts[i].point2,robot->numberDof());
(contacts[i].joint,contacts[i].point,robot->numberDof());
Traits<VectorInJoint>::Ptr_t n2 = VectorInJoint::create
(contacts[i].joint2,contacts[i].normal2,robot->numberDof());
(contacts[i].joint,contacts[i].normal,robot->numberDof());
phi_ (0,i) = n2;
phi_ (1,i) = (OG - OP2) ^ n2;
......@@ -110,7 +110,7 @@ namespace hpp {
std::size_t col = 0;
for (std::size_t i = 0; i < contacts.size (); ++i) {
Traits<VectorInJoint>::Ptr_t n = VectorInJoint::create
(contacts[i].joint,contacts[i].normal,robot->numberDof());
(contacts[i].joint,contacts[i].normal,robot->numberDof());
for (std::size_t j = 0; j < contacts[i].points.size (); ++j) {
Traits<PointInJoint>::Ptr_t OP = PointInJoint::create
(contacts[i].joint,contacts[i].points[j],robot->numberDof());
......
......@@ -63,9 +63,9 @@ namespace hpp {
Traits<PointCom>::Ptr_t OG = PointCom::create (com);
for (std::size_t i = 0; i < contacts.size(); ++i) {
Traits<PointInJoint>::Ptr_t OP2 =
PointInJoint::create (contacts[i].joint2,contacts[i].point2,robot->numberDof());
PointInJoint::create (contacts[i].joint,contacts[i].point,robot->numberDof());
Traits<VectorInJoint>::Ptr_t n2 =
VectorInJoint::create (contacts[i].joint2,contacts[i].normal2,robot->numberDof());
VectorInJoint::create (contacts[i].joint,contacts[i].normal,robot->numberDof());
phi_ (0,i) = n2;
phi_ (1,i) = (OG - OP2) ^ n2;
......
Supports Markdown
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