Commit f2aa20fb authored by Pierre Desreumaux's avatar Pierre Desreumaux
Browse files

update capture point inequality

parent 6e04ab53
......@@ -102,7 +102,6 @@ SET(${PROJECT_NAME}_TASKS_HEADERS
include/tsid/tasks/task-base.hpp
include/tsid/tasks/task-motion.hpp
include/tsid/tasks/task-actuation.hpp
include/tsid/tasks/task-capture-point.hpp
include/tsid/tasks/task-contact-force.hpp
include/tsid/tasks/task-com-equality.hpp
include/tsid/tasks/task-se3-equality.hpp
......@@ -112,6 +111,7 @@ SET(${PROJECT_NAME}_TASKS_HEADERS
include/tsid/tasks/task-joint-bounds.hpp
include/tsid/tasks/task-joint-posture.hpp
include/tsid/tasks/task-joint-posVelAcc-bounds.hpp
include/tsid/tasks/task-capture-point-inequality.hpp
include/tsid/tasks/task-angular-momentum-equality.hpp
)
......@@ -194,13 +194,13 @@ SET(${PROJECT_NAME}_TASKS_SOURCES
src/tasks/task-actuation-bounds.cpp
src/tasks/task-actuation-equality.cpp
src/tasks/task-actuation.cpp
src/tasks/task-capture-point.cpp
src/tasks/task-com-equality.cpp
src/tasks/task-contact-force-equality.cpp
src/tasks/task-contact-force.cpp
src/tasks/task-joint-bounds.cpp
src/tasks/task-joint-posture.cpp
src/tasks/task-joint-posVelAcc-bounds.cpp
src/tasks/task-capture-point-inequality.cpp
src/tasks/task-motion.cpp
src/tasks/task-se3-equality.cpp
src/tasks/task-angular-momentum-equality.cpp
......
......@@ -36,20 +36,16 @@ namespace tsid
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Index Index;
// typedef trajectories::TrajectorySample TrajectorySample;
typedef math::Vector Vector;
typedef math::Matrix Matrix;
typedef math::Vector3 Vector3;
typedef math::ConstraintInequality ConstraintInequality;
typedef pinocchio::Data Data;
// typedef pinocchio::Motion Motion;
typedef pinocchio::SE3 SE3;
TaskCapturePointInequality(const std::string & name,
RobotWrapper & robot,
const std::vector<std::string> & links_in_contact,
const double safety_margin = 0.01,
const double timeStep = 0.001);
const double timeStep);
int dim() const;
//
......@@ -60,33 +56,16 @@ namespace tsid
const ConstraintBase & getConstraint() const;
/** Return the task acceleration (after applying the specified mask).
* The value is expressed in local frame is the local_frame flag is true,
* otherwise it is expressed in a local world-oriented frame.
*/
Vector getAcceleration(ConstRefVector dv) const;
const Vector & position() const;
bool getSupportPolygonPoints(const std::vector<std::string> & links_in_contact,
const std::string & referenceFrame,
const Data & data);
bool computeCapturePoint(std::vector<Vector> & points, std::vector<Vector> & ch);
bool getCapturePoint(std::vector<Vector> & ch, const Data & data, std::string & fame);
void setSupportLimitsXAxis(const double x_min, const double x_max);
void setSafetyMargin(const double safetyMargin);
void setSupportLimitsYAxis(const double y_min, const double y_max);
std::vector<std::string> getLinksInContact() const;
void setSafetyMargin(const double x_margin, const double y_margin);
void setLinksInContact(const std::vector<std::string> & links_in_contact);
void getConstraints(const std::vector<Vector> & capture_point, Matrix & A, Vector & b,
const double safety_margin, const double timeStep);
protected:
......@@ -96,28 +75,21 @@ namespace tsid
Vector m_rp_min;
Vector m_rp_max;
TrajectorySample m_ref;
ConstraintInequality m_constraint;
SE3 m_M_com;
std::vector<Vector> m_ch;
std::vector<Vector> m_points;
std::vector<std::string> m_links_in_contact;
double m_safety_margin;
Vector m_safety_margin;
Vector m_support_limits_x;
Vector m_support_limits_y;
double m_nv;
double m_delta_t;
SE3 com_M_point;
SE3 w_M_point;
SE3 w_M_com;
Index frame_id;
Vector com_p_point;
Matrix A;
double m_g;
double m_w;
double m_ka;
int m_dim;
Vector b;
Vector b_min;
Vector b_max;
Vector b_lower;
Vector b_upper;
};
}
......
......@@ -32,7 +32,7 @@ namespace tsid
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Index Index;
typedef trajectories::TrajectorySample TrajectorySample;
typedef math::Vector Vector;
......@@ -81,7 +81,7 @@ namespace tsid
TrajectorySample m_ref;
ConstraintEquality m_constraint;
};
}
}
......
......@@ -23,7 +23,7 @@
#include <tsid/math/constraint-inequality.hpp>
/** This class has been implemented following :
* Andrea del Prete. Joint Position and Velocity Bounds in Discrete-Time
* Andrea del Prete. Joint Position and Velocity Bounds in Discrete-Time
* Acceleration/Torque Control of Robot Manipulators. IEEE Robotics and Automation
* Letters, IEEE 2018, 3 (1), pp.281-288.￿10.1109/LRA.2017.2738321￿. hal-01356989v3
* And
......@@ -92,7 +92,7 @@ namespace tsid
void isStateViable(ConstRefVector q,ConstRefVector dq ,bool verbose=true);
>>>>>>> ebc2966e0186cc3b354ca04912dd7c9b4dc198bc
/** Compute acceleration limits imposed by position bounds.
/** Compute acceleration limits imposed by position bounds.
* Fills in m_ddqLBPos and m_ddqUBPos
*/
<<<<<<< HEAD
......@@ -104,12 +104,12 @@ namespace tsid
/** Compute acceleration limits imposed by viability.
* ddqMax is the maximum acceleration that will be necessary to stop the
* joint before hitting the position limits.
*
*
* -sqrt( 2*ddqMax*(q-qMin) ) < dq[t+1] < sqrt( 2*ddqMax*(qMax-q) )
* ddqMin[2] = (-sqrt(max(0.0, 2*MAX_ACC*(q[i]+DT*dq[i]-qMin))) - dq[i])/DT;
* ddqMax[2] = (sqrt(max(0.0, 2*MAX_ACC*(qMax-q[i]-DT*dq[i]))) - dq[i])/DT;
*
* Fills in m_ddqLBVia and m_ddqUBVia
*
* Fills in m_ddqLBVia and m_ddqUBVia
*/
<<<<<<< HEAD
void computeAccLimitsFromViability(const Vector& q,const Vector& dq, bool verbose=true);
......@@ -195,7 +195,7 @@ namespace tsid
Vector m_ddqMax_q2;
Vector m_ddqMin_q2;
Vector m_minus_dq_over_dt;
//Used in computeAccLimitsFromViability
double m_dt_square;
Vector m_dt_dq;
......
......@@ -33,50 +33,37 @@ namespace tsid
TaskCapturePointInequality::TaskCapturePointInequality(const std::string & name,
RobotWrapper & robot,
const std::vector<std::string> & links_in_contact,
const double safety_margin,
const double timeStep):
TaskMotion(name, robot),
m_constraint(name, 2, robot.nv()),
m_nv(robot.nv()),
m_links_in_contact(links_in_contact),
m_safety_margin(safety_margin),
m_delta_t(timeStep)
{
assert(links_in_contact.size() > 0);
// std::cout << " CONSTRUCTOR CONVEX_HULL BEGIN "<< std::endl;
m_dim = 2;
m_p_com.setZero(3);
m_v_com.setZero(3);
m_safety_margin.setZero(m_dim);
m_rp_max.setZero(3);
m_rp_min.setZero(3);
m_support_limits_x.setZero(m_dim);
m_support_limits_y.setZero(m_dim);
Vector zero;
zero.setZero(3);
for(int i=0; i < links_in_contact.size(); i++)
{
m_ch.push_back(zero);
m_points.push_back(zero);
}
m_rp_max.setZero(m_dim);
m_rp_min.setZero(m_dim);
w_M_com.setIdentity();
com_M_point.setIdentity();
w_M_point.setIdentity();
com_p_point.setZero(3);
b_lower.setZero(m_dim);
b_upper.setZero(m_dim);
A.setZero(4,2);
b.setZero(4);
b_min.setZero(2);
b_max.setZero(2);
m_g = 9.81;
m_w = 0;
m_ka = 0;
// std::cout << " CONSTRUCTOR CONVEX_HULL END"<< std::endl;
}
int TaskCapturePointInequality::dim() const
{
//return self._mask.sum ()
return 2;
return m_dim;
}
Vector TaskCapturePointInequality::getAcceleration(ConstRefVector dv) const
......@@ -93,119 +80,24 @@ namespace tsid
return m_constraint;
}
bool TaskCapturePointInequality::getSupportPolygonPoints(const std::vector<std::string> & links_in_contact,
const std::string & referenceFrame,
const Data & data)
{
if(referenceFrame != "COM" && referenceFrame != "world")
std::cerr << "ERROR: "
<< "trying to get support polygon points in unknown reference frame "
<< std::endl;
if(links_in_contact.empty() ||
(referenceFrame != "COM" &&
referenceFrame != "world" ))
return false;
w_M_com.setIdentity();
w_M_com.translation(m_p_com.head<3>());
for(int i=0; i< links_in_contact.size(); i++)
{
if(referenceFrame == "world")
{
//get points in world frame
assert(m_robot.model().existFrame(links_in_contact[i]));
frame_id = m_robot.model().getFrameId(links_in_contact[i]);
m_robot.framePosition(data, frame_id, w_M_point);
m_points[i]= w_M_point.translation();
//std::cout << " Transform world to points[" <<i<<"]: " << w_M_point << std::endl;
}
if(referenceFrame == "COM")
{
com_M_point = w_M_com.inverse() * w_M_point;
m_points[i]= com_M_point.translation();//com_p_point;
// std::cout << "points[" << i << "] : " << m_points[i] << std::endl;
}
}
return true;
}
bool TaskCapturePointInequality::getCapturePoint(std::vector<Vector> & ch,
const Data & data,
std::string & frame
)
void TaskCapturePointInequality::setSupportLimitsXAxis(const double x_min, const double x_max)
{
if(getSupportPolygonPoints(m_links_in_contact,frame, data))
{
// std::cout << "Points size.. ? : "<< m_points.size() << std::endl;
if(m_points.size()>2)
{
return true;
}
else
std::cout<<"Too few points for Convex Hull computation!, old Convex Hull will be used"<<std::endl;
}
else
std::cout<<"Problems getting Points for Convex Hull computation!, old Convex Hull will be used"<<std::endl;
return false;
}
bool TaskCapturePointInequality::computeCapturePoint(std::vector<Vector> & points, std::vector<Vector> & ch)
{
//TODO :reodering points to be a trigonometry rotation
ch = points;
// std::cout << "COMPUTE CONVEX_HULL END"<< std::endl;
return true;
assert(x_min >= x_max);
m_support_limits_x(0) = x_min;
m_support_limits_x(1) = x_max;
}
void TaskCapturePointInequality::setSafetyMargin(const double safetyMargin)
void TaskCapturePointInequality::setSupportLimitsYAxis(const double y_min, const double y_max)
{
m_safety_margin = safetyMargin;
assert(y_min >= y_max);
m_support_limits_y(0) = y_min;
m_support_limits_y(1) = y_max;
}
std::vector<std::string> TaskCapturePointInequality::getLinksInContact() const
void TaskCapturePointInequality::setSafetyMargin(const double x_margin, const double y_margin)
{
return m_links_in_contact;
}
void TaskCapturePointInequality::setLinksInContact(const std::vector<std::string> & links_in_contact)
{
m_links_in_contact = links_in_contact;
}
void TaskCapturePointInequality::getConstraints(const std::vector<Vector> & convex_hull, Matrix & A,
Vector & b, const double boundScaling, const double timeStep)
{
double m_g = 9.81;
double w = sqrt(m_g/m_p_com(2));
m_delta_t = timeStep;
double Ka = (2*w)/((w*m_delta_t+2)*m_delta_t);
// std::cout << "Ka = " << Ka << std::endl;
m_rp_min(0) = -0.127 +0.01; // x min support polygon
m_rp_min(1) = -0.135 +0.03; // y min support polygon
m_rp_max(0) = 0.07 -0.01; // x max support polygon
m_rp_max(1) = 0.135 -0.03; // y max support polygon
b_min(0) = Ka*(m_rp_min(0) - m_p_com(0) - m_v_com(0)*(m_delta_t+ 1/w)); // x axe
b_min(1) = Ka*(m_rp_min(1) - m_p_com(1) - m_v_com(1)*(m_delta_t+ 1/w)); // y axe
b_max(0) = Ka*(m_rp_max(0) - m_p_com(0) - m_v_com(0)*(m_delta_t+ 1/w)); // x axe
b_max(1) = Ka*(m_rp_max(1) - m_p_com(1) - m_v_com(1)*(m_delta_t+ 1/w)); // y axe
// std::cout << "b_max(0) = " << b_max(0) << std::endl;
// std::cout << "b_max(1) = " << b_max(1) << std::endl;
// std::cout << "b_min(0) = " << b_min(0) << std::endl;
// std::cout << "b_min(1) = " << b_min(1) << std::endl;
// std::cout << " GET CONSTRAINT END"<< std::endl;
m_safety_margin(0) = x_margin;
m_safety_margin(1) = y_margin;
}
const ConstraintBase & TaskCapturePointInequality::compute(const double t,
......@@ -213,59 +105,34 @@ namespace tsid
ConstRefVector v,
const Data & data)
{
// std::cout << " COMPUTE BEGIN"<< std::endl;
m_robot.com(data, m_p_com, m_v_com, m_drift);
const Matrix3x & Jcom = m_robot.Jcom(data);
std::string frame = "COM";
// if(getCapturePoint(m_ch, data, frame))
getConstraints(m_ch, A, b, m_safety_margin, m_delta_t);
m_drift_vec=m_drift.head(2);
m_w = sqrt(m_g/m_p_com(2));
m_ka = (2*m_w)/((m_w*m_delta_t+2)*m_delta_t);
Vector b_zero;
b_zero.setZero(2);
b_zero(0) = 0.1;
b_zero(1) = 0.1 ;
m_constraint.upperBound() = b_max ; //- m_drift_vec;
// std::cout << "b_max "<< b_max - m_drift_vec << std::endl;
b_zero(0) = 0.0;
b_zero(1) = 0.0;
m_constraint.lowerBound() = b_min ; //- m_drift_vec;
// std::cout << "b_min "<< b_min - m_drift_vec << std::endl;
//A = A* Jcom.block(0,0,2,m_nv);
m_rp_min(0) = m_support_limits_x(0) + m_safety_margin(0); // x min support polygon
m_rp_min(1) = m_support_limits_y(0) + m_safety_margin(1); // y min support polygon
// double m_g = 9.81;
// double w = sqrt(m_g/m_p_com(2));
m_rp_max(0) = m_support_limits_x(1) -m_safety_margin(0); // x max support polygon
m_rp_max(1) = m_support_limits_y(1) -m_safety_margin(1); // y max support polygon
// double Ka = 0.5*0.1*0.1 + 0.1/w;
// Matrix Ka_v;
// Ka_v.setZero(2,2);
// Ka_v(0,0) = Ka;
// Ka_v(1,1)= Ka;
// std::cout << "Jcom size: "<< std::endl;
// std::cout << Jcom.size() << std::endl;
// std::cout << "m_nv size: "<< std::endl;
// std::cout << m_nv << std::endl;
for(int i=0; i< m_dim; i++){
b_lower(i) = m_ka*(m_rp_min(i) - m_p_com(i) - m_v_com(i)*(m_delta_t+1/m_w));
b_upper(i) = m_ka*(m_rp_max(i) - m_p_com(i) - m_v_com(i)*(m_delta_t+1/m_w));
}
m_constraint.lowerBound() = b_lower - m_drift.head(m_dim);
m_constraint.upperBound() = b_upper - m_drift.head(m_dim);
m_constraint.setMatrix(Jcom.block(0,0,m_dim,m_nv));
// Matrix F;
// F.setZero(2,36);
// F = Ka_v*Jcom.block(0,0,2,m_nv);
m_constraint.setMatrix(Jcom.block(0,0,2,m_nv));
//m_constraint.lowerBound() = -100000*(b-A*m_drift_vec);
A.setZero(4,2);
b.setZero(4);
b_min.setZero(2);
b_max.setZero(2);
m_rp_min.setZero(2);
m_rp_max.setZero(2);
b_lower.setZero(m_dim);
b_upper.setZero(m_dim);
// std::cout << " COMPUTE END"<< std::endl;
return m_constraint;
}
}
}
}
\ No newline at end of file
......@@ -146,6 +146,6 @@ namespace tsid
m_constraint.setVector(m_a_des - m_drift);
return m_constraint;
}
}
}
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