Commit db24d93a authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Fix conflict

parents 96f0c8f9 4401d639
......@@ -110,6 +110,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
)
......@@ -198,6 +199,7 @@ SET(${PROJECT_NAME}_TASKS_SOURCES
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
......
Subproject commit e715bf761e97dbcd704adecd03c28d8e195e7811
Subproject commit c333a88decb3e4c0a86947bc6c7f072dc5c5df20
//
// Copyright (c) 2020 CNRS, NYU, MPI Tübingen, PAL Robotics
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//
#ifndef __invdyn_task_capture_point_inequality_hpp__
#define __invdyn_task_capture_point_inequality_hpp__
#include <tsid/tasks/task-motion.hpp>
#include <tsid/trajectories/trajectory-base.hpp>
#include <tsid/math/constraint-inequality.hpp>
#include <vector>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>
namespace tsid
{
namespace tasks
{
class TaskCapturePointInequality : public TaskMotion
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Index Index;
typedef math::Vector Vector;
typedef math::Matrix Matrix;
typedef math::Vector3 Vector3;
typedef math::ConstraintInequality ConstraintInequality;
typedef pinocchio::Data Data;
typedef pinocchio::SE3 SE3;
TaskCapturePointInequality(const std::string & name,
RobotWrapper & robot,
const double timeStep);
int dim() const;
const ConstraintBase & compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data);
const ConstraintBase & getConstraint() const;
Vector getAcceleration(ConstRefVector dv) const;
const Vector & position() const;
void setSupportLimitsXAxis(const double x_min, const double x_max);
void setSupportLimitsYAxis(const double y_min, const double y_max);
void setSafetyMargin(const double x_margin, const double y_margin);
protected:
Vector m_drift_vec;
Vector3 m_drift;
Vector m_p_com, m_v_com;
Vector m_rp_min;
Vector m_rp_max;
ConstraintInequality m_constraint;
Vector m_safety_margin;
Vector m_support_limits_x;
Vector m_support_limits_y;
double m_nv;
double m_delta_t;
double m_g;
double m_w;
double m_ka;
int m_dim;
Vector b_lower;
Vector b_upper;
};
}
}
#endif //ifndef __invdyn_task_capture_point_inequality_hpp__
......@@ -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
......@@ -81,7 +81,7 @@ namespace tsid
*/
void isStateViable(ConstRefVector q,ConstRefVector dq ,bool verbose=true);
/** Compute acceleration limits imposed by position bounds.
/** Compute acceleration limits imposed by position bounds.
* Fills in m_ddqLBPos and m_ddqUBPos
*/
void computeAccLimitsFromPosLimits(ConstRefVector q,ConstRefVector dq, bool verbose=true);
......@@ -89,12 +89,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
*/
void computeAccLimitsFromViability(ConstRefVector q,ConstRefVector dq, bool verbose=true);
......@@ -158,7 +158,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;
......
//
// Copyright (c) 2020 CNRS, NYU, MPI Tübingen, PAL Robotics
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//
#include <tsid/tasks/task-capture-point-inequality.hpp>
#include "tsid/math/utils.hpp"
#include "tsid/robots/robot-wrapper.hpp"
/** This class has been implemented following :
* Ramos, O. E., Mansard, N., & Soueres, P.
* (2014). Whole-body Motion Integrating the Capture Point in the Operational Space Inverse Dynamics Control.
* In IEEE-RAS International Conference on Humanoid Robots (Humanoids).
*/
namespace tsid
{
namespace tasks
{
using namespace math;
using namespace trajectories;
using namespace pinocchio;
TaskCapturePointInequality::TaskCapturePointInequality(const std::string & name,
RobotWrapper & robot,
const double timeStep):
TaskMotion(name, robot),
m_constraint(name, 2, robot.nv()),
m_nv(robot.nv()),
m_delta_t(timeStep)
{
m_dim = 2;
m_p_com.setZero(3);
m_v_com.setZero(3);
m_safety_margin.setZero(m_dim);
m_support_limits_x.setZero(m_dim);
m_support_limits_y.setZero(m_dim);
m_rp_max.setZero(m_dim);
m_rp_min.setZero(m_dim);
b_lower.setZero(m_dim);
b_upper.setZero(m_dim);
m_g = 9.81;
m_w = 0;
m_ka = 0;
}
int TaskCapturePointInequality::dim() const
{
return m_dim;
}
Vector TaskCapturePointInequality::getAcceleration(ConstRefVector dv) const
{
return m_constraint.matrix()*dv - m_drift;
}
const Vector & TaskCapturePointInequality::position() const
{
return m_p_com;
}
const ConstraintBase & TaskCapturePointInequality::getConstraint() const
{
return m_constraint;
}
void TaskCapturePointInequality::setSupportLimitsXAxis(const double x_min, const double x_max)
{
assert(x_min >= x_max);
m_support_limits_x(0) = x_min;
m_support_limits_x(1) = x_max;
}
void TaskCapturePointInequality::setSupportLimitsYAxis(const double y_min, const double y_max)
{
assert(y_min >= y_max);
m_support_limits_y(0) = y_min;
m_support_limits_y(1) = y_max;
}
void TaskCapturePointInequality::setSafetyMargin(const double x_margin, const double y_margin)
{
m_safety_margin(0) = x_margin;
m_safety_margin(1) = y_margin;
}
const ConstraintBase & TaskCapturePointInequality::compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data)
{
m_robot.com(data, m_p_com, m_v_com, m_drift);
const Matrix3x & Jcom = m_robot.Jcom(data);
m_w = sqrt(m_g/m_p_com(2));
m_ka = (2*m_w)/((m_w*m_delta_t+2)*m_delta_t);
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
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
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));
return m_constraint;
}
}
}
......@@ -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