From f66e0702a775c04ffafcc87ba96f04fc17db6fc0 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Wed, 1 Mar 2017 10:25:47 +0000 Subject: [PATCH 1/5] Fix Eigen alignment issues with some tasks --- src/QPTasks.cpp | 67 +++++++++++++++++++++++++++++++++++++++---------- src/QPTasks.h | 26 +++++++++++++------ src/Tasks.h | 2 ++ 3 files changed, 75 insertions(+), 20 deletions(-) diff --git a/src/QPTasks.cpp b/src/QPTasks.cpp index 5a6cb0b73..0326cff60 100644 --- a/src/QPTasks.cpp +++ b/src/QPTasks.cpp @@ -1076,7 +1076,7 @@ GazeTask::GazeTask(const std::vector& mbs, const Eigen::Vector2d& point2d, double depthEstimate, const sva::PTransformd& X_b_gaze, const Eigen::Vector2d& point2d_ref): - gazet_(mbs[robotIndex], bodyName, point2d, depthEstimate, X_b_gaze, point2d_ref), + gazet_(new tasks::GazeTask(mbs[robotIndex], bodyName, point2d, depthEstimate, X_b_gaze, point2d_ref)), robotIndex_(robotIndex) {} @@ -1085,10 +1085,30 @@ GazeTask::GazeTask(const std::vector& mbs, int robotIndex, const std::string& bodyName, const Eigen::Vector3d& point3d, const sva::PTransformd& X_b_gaze, const Eigen::Vector2d& point2d_ref): - gazet_(mbs[robotIndex], bodyName, point3d, X_b_gaze, point2d_ref), + gazet_(new tasks::GazeTask(mbs[robotIndex], bodyName, point3d, X_b_gaze, point2d_ref)), robotIndex_(robotIndex) {} +GazeTask::GazeTask(const GazeTask& rhs): + gazet_(new tasks::GazeTask(*rhs.gazet_)), + robotIndex_(rhs.robotIndex_) +{ +} + +GazeTask& GazeTask::operator=(const GazeTask& rhs) +{ + if(&rhs != this) + { + robotIndex_ = rhs.robotIndex_; + *gazet_ = *rhs.gazet_; + } + return *this; +} + +GazeTask::~GazeTask() +{ + delete gazet_; +} int GazeTask::dim() { @@ -1100,31 +1120,31 @@ void GazeTask::update(const std::vector& mbs, const std::vector& mbcs, const SolverData& data) { - gazet_.update(mbs[robotIndex_], mbcs[robotIndex_], data.normalAccB(robotIndex_)); + gazet_->update(mbs[robotIndex_], mbcs[robotIndex_], data.normalAccB(robotIndex_)); } const Eigen::MatrixXd& GazeTask::jac() { - return gazet_.jac(); + return gazet_->jac(); } const Eigen::VectorXd& GazeTask::eval() { - return gazet_.eval(); + return gazet_->eval(); } const Eigen::VectorXd& GazeTask::speed() { - return gazet_.speed(); + return gazet_->speed(); } const Eigen::VectorXd& GazeTask::normalAcc() { - return gazet_.normalAcc(); + return gazet_->normalAcc(); } @@ -1137,11 +1157,32 @@ PositionBasedVisServoTask::PositionBasedVisServoTask(const std::vector& mbs, const std::vector& mbcs, const SolverData& data) { - pbvst_.update(mbs[robotIndex_], mbcs[robotIndex_], data.normalAccB(robotIndex_)); + pbvst_->update(mbs[robotIndex_], mbcs[robotIndex_], data.normalAccB(robotIndex_)); } const Eigen::MatrixXd& PositionBasedVisServoTask::jac() { - return pbvst_.jac(); + return pbvst_->jac(); } const Eigen::VectorXd& PositionBasedVisServoTask::eval() { - return pbvst_.eval(); + return pbvst_->eval(); } const Eigen::VectorXd& PositionBasedVisServoTask::speed() { - return pbvst_.speed(); + return pbvst_->speed(); } const Eigen::VectorXd& PositionBasedVisServoTask::normalAcc() { - return pbvst_.normalAcc(); + return pbvst_->normalAcc(); } diff --git a/src/QPTasks.h b/src/QPTasks.h index f62c07d30..342b9fa5e 100644 --- a/src/QPTasks.h +++ b/src/QPTasks.h @@ -760,21 +760,27 @@ class TASKS_DLLAPI GazeTask : public HighLevelTask const sva::PTransformd& X_b_gaze, const Eigen::Vector2d& point2d_ref = Eigen::Vector2d::Zero()); + GazeTask(const GazeTask& rhs); + + GazeTask& operator=(const GazeTask& rhs); + + ~GazeTask(); + tasks::GazeTask& task() { - return gazet_; + return *gazet_; } void error(const Eigen::Vector2d& point2d, const Eigen::Vector2d& point2d_ref = Eigen::Vector2d::Zero()) { - gazet_.error(point2d, point2d_ref); + gazet_->error(point2d, point2d_ref); } void error(const Eigen::Vector3d& point3d, const Eigen::Vector2d& point2d_ref = Eigen::Vector2d::Zero()) { - gazet_.error(point3d, point2d_ref); + gazet_->error(point3d, point2d_ref); } virtual int dim(); @@ -787,7 +793,7 @@ class TASKS_DLLAPI GazeTask : public HighLevelTask virtual const Eigen::VectorXd& normalAcc(); private: - tasks::GazeTask gazet_; + tasks::GazeTask * const gazet_; int robotIndex_; }; @@ -800,14 +806,20 @@ class TASKS_DLLAPI PositionBasedVisServoTask : public HighLevelTask const std::string& bodyName, const sva::PTransformd& X_t_s, const sva::PTransformd& X_b_s=sva::PTransformd::Identity()); + PositionBasedVisServoTask(const PositionBasedVisServoTask & rhs); + + PositionBasedVisServoTask& operator=(const PositionBasedVisServoTask & rhs); + + ~PositionBasedVisServoTask(); + tasks::PositionBasedVisServoTask& task() { - return pbvst_; + return *pbvst_; } void error(const sva::PTransformd& X_t_s) { - pbvst_.error(X_t_s); + pbvst_->error(X_t_s); } virtual int dim(); @@ -820,7 +832,7 @@ class TASKS_DLLAPI PositionBasedVisServoTask : public HighLevelTask virtual const Eigen::VectorXd& normalAcc(); private: - tasks::PositionBasedVisServoTask pbvst_; + tasks::PositionBasedVisServoTask * const pbvst_; int robotIndex_; }; diff --git a/src/Tasks.h b/src/Tasks.h index f375bc34c..767c5903e 100644 --- a/src/Tasks.h +++ b/src/Tasks.h @@ -281,6 +281,7 @@ class TASKS_DLLAPI SurfaceOrientationTask class TASKS_DLLAPI GazeTask { public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW GazeTask(const rbd::MultiBody& mb, const std::string& bodyName, const Eigen::Vector2d &point2d, double depthEstimate, const sva::PTransformd& X_b_gaze, @@ -329,6 +330,7 @@ class TASKS_DLLAPI GazeTask class TASKS_DLLAPI PositionBasedVisServoTask { public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW PositionBasedVisServoTask(const rbd::MultiBody &mb, const std::string& bodyName, const sva::PTransformd& X_t_s, const sva::PTransformd& X_b_s); From ecc85243bff5c2f829dbf1c88ad6ac7db3a5d0fe Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Thu, 2 Mar 2017 06:22:17 +0000 Subject: [PATCH 2/5] Fix allocation issue on (a-priori) all dangerous objects --- src/QPConstr.cpp | 127 +++++++++++++++++++++++++++++---------- src/QPConstr.h | 35 ++++++++--- src/QPTasks.cpp | 71 +++++----------------- src/QPTasks.h | 32 ++++------ src/Tasks.cpp | 150 +++++++++++++++++++++++++++++++++++------------ src/Tasks.h | 38 ++++++++---- 6 files changed, 288 insertions(+), 165 deletions(-) diff --git a/src/QPConstr.cpp b/src/QPConstr.cpp index 25102b5dc..55685ed78 100644 --- a/src/QPConstr.cpp +++ b/src/QPConstr.cpp @@ -1217,17 +1217,17 @@ ImageConstr::ImageConstr(const std::vector& mbs, nrActivated_(0), jac_(mbs[robotIndex], bName), X_b_gaze_(X_b_gaze), - L_img_(Eigen::Matrix::Zero()), - surfaceVelocity_(Eigen::Matrix::Zero()), - L_Z_dot_(Eigen::Matrix ::Zero()), - L_img_dot_(Eigen::Matrix::Zero()), - speed_(Eigen::Vector2d::Zero()), - normalAcc_(Eigen::Vector2d::Zero()), + L_img_(new Eigen::Matrix(Eigen::Matrix::Zero())), + surfaceVelocity_(new Eigen::Matrix(Eigen::Matrix::Zero())), + L_Z_dot_(new Eigen::Matrix (Eigen::Matrix ::Zero())), + L_img_dot_(new Eigen::Matrix(Eigen::Matrix::Zero())), + speed_(new Eigen::Vector2d(Eigen::Vector2d::Zero())), + normalAcc_(new Eigen::Vector2d(Eigen::Vector2d::Zero())), jacMat_(2, mbs[robotIndex].nrDof()), - iDistMin_(Eigen::Vector2d::Zero()), - iDistMax_(Eigen::Vector2d::Zero()), - sDistMin_(Eigen::Vector2d::Zero()), - sDistMax_(Eigen::Vector2d::Zero()), + iDistMin_(new Eigen::Vector2d(Eigen::Vector2d::Zero())), + iDistMax_(new Eigen::Vector2d(Eigen::Vector2d::Zero())), + sDistMin_(new Eigen::Vector2d(Eigen::Vector2d::Zero())), + sDistMax_(new Eigen::Vector2d(Eigen::Vector2d::Zero())), damping_(0.), dampingOffset_(0.), ineqInversion_(1), @@ -1236,6 +1236,73 @@ ImageConstr::ImageConstr(const std::vector& mbs, bInEq_() {} +ImageConstr::ImageConstr(const ImageConstr& rhs): + dataVec_(rhs.dataVec_), + dataVecRob_(rhs.dataVecRob_), + robotIndex_(rhs.robotIndex_), + bodyIndex_(rhs.bodyIndex_), + alphaDBegin_(rhs.alphaDBegin_), + nrVars_(rhs.nrVars_), + step_(rhs.step_), + accelFactor_(rhs.accelFactor_), + nrActivated_(rhs.nrActivated_), + jac_(rhs.jac_), + X_b_gaze_(rhs.X_b_gaze_), + L_img_(new Eigen::Matrix(*rhs.L_img_)), + surfaceVelocity_(new Eigen::Matrix(*rhs.surfaceVelocity_)), + L_Z_dot_(new Eigen::Matrix(*rhs.L_Z_dot_)), + L_img_dot_(new Eigen::Matrix(*rhs.L_img_dot_)), + speed_(new Eigen::Vector2d(*rhs.speed_)), + normalAcc_(new Eigen::Vector2d(*rhs.normalAcc_)), + jacMat_(rhs.jacMat_), + iDistMin_(new Eigen::Vector2d(*rhs.iDistMin_)), + iDistMax_(new Eigen::Vector2d(*rhs.iDistMax_)), + sDistMin_(new Eigen::Vector2d(*rhs.sDistMin_)), + sDistMax_(new Eigen::Vector2d(*rhs.sDistMax_)), + damping_(rhs.damping_), + dampingOffset_(rhs.dampingOffset_), + ineqInversion_(rhs.ineqInversion_), + constrDirection_(rhs.constrDirection_), + AInEq_(rhs.AInEq_), + bInEq_(rhs.bInEq_) +{ +} + +ImageConstr& ImageConstr::operator=(const ImageConstr& rhs) +{ + if(&rhs != this) + { + dataVec_ = rhs.dataVec_; + dataVecRob_ = rhs.dataVecRob_; + robotIndex_ = rhs.robotIndex_; + bodyIndex_ = rhs.bodyIndex_; + alphaDBegin_ = rhs.alphaDBegin_; + nrVars_ = rhs.nrVars_; + step_ = rhs.step_; + accelFactor_ = rhs.accelFactor_; + nrActivated_ = rhs.nrActivated_; + jac_ = rhs.jac_; + X_b_gaze_ = rhs.X_b_gaze_; + *L_img_ = *rhs.L_img_; + *surfaceVelocity_ = *rhs.surfaceVelocity_; + *L_Z_dot_ = *rhs.L_Z_dot_; + *L_img_dot_ = *rhs.L_img_dot_; + *speed_ = *rhs.speed_; + *normalAcc_ = *rhs.normalAcc_; + jacMat_ = rhs.jacMat_; + *iDistMin_ = *rhs.iDistMin_; + *iDistMax_ = *rhs.iDistMax_; + *sDistMin_ = *rhs.sDistMin_; + *sDistMax_ = *rhs.sDistMax_; + damping_ = rhs.damping_; + dampingOffset_ = rhs.dampingOffset_; + ineqInversion_ = rhs.ineqInversion_; + constrDirection_ = rhs.constrDirection_; + AInEq_ = rhs.AInEq_; + bInEq_ = rhs.bInEq_; + } + return *this; +} int ImageConstr::addPoint(const Eigen::Vector2d& point2d, const double depthEstimate) { @@ -1291,10 +1358,10 @@ void ImageConstr::setLimits(const Eigen::Vector2d& min, const Eigen::Vector2d& m { // precompute avoidance boundaries Eigen::Vector2d dist = max - min; - iDistMin_ = min + constrDirection_*iPercent*dist; - iDistMax_ = max - constrDirection_*iPercent*dist; - sDistMin_ = min + constrDirection_*sPercent*dist; - sDistMax_ = max - constrDirection_*sPercent*dist; + *iDistMin_ = min + constrDirection_*iPercent*dist; + *iDistMax_ = max - constrDirection_*iPercent*dist; + *sDistMin_ = min + constrDirection_*sPercent*dist; + *sDistMax_ = max - constrDirection_*sPercent*dist; damping_ = damping; dampingOffset_ = constrDirection_*dampingOffsetPercent*damping; @@ -1306,22 +1373,22 @@ void ImageConstr::computeComponents(const rbd::MultiBody& mb, const rbd::MultiBo const sva::PTransformd& X_b_p, Eigen::MatrixXd& fullJacobian, Eigen::Vector2d& bCommonTerm) { // compute speed term - rbd::imagePointJacobian(point2d, depth, L_img_); - surfaceVelocity_ = (jac.velocity(mb, mbc, X_b_p)).vector(); - speed_ = L_img_*surfaceVelocity_; + rbd::imagePointJacobian(point2d, depth, *L_img_); + *surfaceVelocity_ = (jac.velocity(mb, mbc, X_b_p)).vector(); + *speed_ = (*L_img_)*(*surfaceVelocity_); // compute norm accel term - rbd::depthDotJacobian(speed_, depth, L_Z_dot_); - rbd::imagePointJacobianDot(point2d, speed_, depth, L_Z_dot_*surfaceVelocity_, L_img_dot_); - normalAcc_ = L_img_*(jac.normalAcceleration(mb, mbc, data.normalAccB(robotIndex_), X_b_p, - sva::MotionVecd(Eigen::Vector6d::Zero()))).vector() + L_img_dot_*surfaceVelocity_; + rbd::depthDotJacobian(*speed_, depth, *L_Z_dot_); + rbd::imagePointJacobianDot(point2d, *speed_, depth, (*L_Z_dot_)*(*surfaceVelocity_), *L_img_dot_); + *normalAcc_ = (*L_img_)*(jac.normalAcceleration(mb, mbc, data.normalAccB(robotIndex_), X_b_p, + sva::MotionVecd(Eigen::Vector6d::Zero()))).vector() + (*L_img_dot_)*(*surfaceVelocity_); // compute the shortened jacobian - const auto& shortJacMat = accelFactor_*L_img_*jac.jacobian(mb, mbc, X_b_p*mbc.bodyPosW[bodyIndex]).block(0, 0, 6, jac.dof()); + const auto& shortJacMat = accelFactor_*(*L_img_)*jac.jacobian(mb, mbc, X_b_p*mbc.bodyPosW[bodyIndex]).block(0, 0, 6, jac.dof()); // fill objects to return for the QP jac.fullJacobian(mb, shortJacMat, fullJacobian); - bCommonTerm = -step_*speed_-accelFactor_*normalAcc_; + bCommonTerm = -step_*(*speed_)-accelFactor_*(*normalAcc_); } @@ -1359,16 +1426,16 @@ void ImageConstr::update(const std::vector& mbs, std::size_t iOther = (i==0) ? 1 : 0; // check occlusion constraint if it is within the 2D image bounds - if((constrDirection_==1.) || ((point2d_[iOther] > iDistMin_[iOther]) && (point2d_[iOther] < iDistMax_[iOther]))) + if((constrDirection_==1.) || ((point2d_[iOther] > (*iDistMin_)[iOther]) && (point2d_[iOther] < (*iDistMax_)[iOther]))) { - if((constrDirection_*point2d_[i] < constrDirection_*iDistMin_[i]) //check min + if((constrDirection_*point2d_[i] < constrDirection_*(*iDistMin_)[i]) //check min && ((constrDirection_==1.) || (point2d_[i] < 0.))) //handle occlusion constraint ambiquity { - if(constrDirection_*point2d_ [i] > constrDirection_*sDistMin_[i]) + if(constrDirection_*point2d_ [i] > constrDirection_*(*sDistMin_)[i]) { isConstrActive = true; ineqInversion_ = -1.*constrDirection_; - bInEq_(nrActivated_) = constrDirection_*(damping_*((point2d_[i] - sDistMin_[i])/(iDistMin_[i] - sDistMin_[i])) - dampingOffset_) + ineqInversion_*bCommonTerm[i]; + bInEq_(nrActivated_) = constrDirection_*(damping_*((point2d_[i] - (*sDistMin_)[i])/((*iDistMin_)[i] - (*sDistMin_)[i])) - dampingOffset_) + ineqInversion_*bCommonTerm[i]; } else { @@ -1376,14 +1443,14 @@ void ImageConstr::update(const std::vector& mbs, bInEq_(nrActivated_) = - constrDirection_*dampingOffset_ + ineqInversion_*bCommonTerm[i]; } } - else if((constrDirection_*point2d_[i] > constrDirection_*iDistMax_[i]) // check max + else if((constrDirection_*point2d_[i] > constrDirection_*(*iDistMax_)[i]) // check max && ((constrDirection_==1.) || (point2d_[i] > 0.))) //handle occlusion constraint ambiquity { - if(constrDirection_*point2d_[i] < constrDirection_*sDistMax_[i]) + if(constrDirection_*point2d_[i] < constrDirection_*(*sDistMax_)[i]) { isConstrActive = true; ineqInversion_ = 1.*constrDirection_; - bInEq_(nrActivated_) = constrDirection_*(damping_*((point2d_[i] - sDistMax_[i])/(iDistMax_[i] - sDistMax_[i])) - dampingOffset_) + ineqInversion_*bCommonTerm[i]; + bInEq_(nrActivated_) = constrDirection_*(damping_*((point2d_[i] - (*sDistMax_)[i])/((*iDistMax_)[i] - (*sDistMax_)[i])) - dampingOffset_) + ineqInversion_*bCommonTerm[i]; } else { diff --git a/src/QPConstr.h b/src/QPConstr.h index 560f19d2c..9b4a488d7 100644 --- a/src/QPConstr.h +++ b/src/QPConstr.h @@ -20,6 +20,7 @@ // includes // Eigen #include +#include // RBDyn #include @@ -31,6 +32,9 @@ // Tasks #include "QPSolver.h" +// unique_ptr +#include + // forward declaration // sch namespace sch @@ -672,6 +676,18 @@ class ImageConstr : public ConstraintFunction ImageConstr(const std::vector& mbs, int robotIndex, const std::string &bName, const sva::PTransformd& X_b_gaze, double step, double constrDirection=1.); + /** Copy constructor */ + ImageConstr(const ImageConstr& rhs); + + /** Move constructor */ + ImageConstr(ImageConstr&&) = default; + + /** Copy assignment operator */ + ImageConstr& operator=(const ImageConstr& rhs); + + /** Move assignment operator */ + ImageConstr& operator=(ImageConstr&&) = default; + /** * @brief setLimits * @param min @@ -730,6 +746,7 @@ class ImageConstr : public ConstraintFunction private: struct PointData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW PointData(const Eigen::Vector2d& pt, const double d); Eigen::Vector2d point2d; double depthEstimate; @@ -737,13 +754,13 @@ class ImageConstr : public ConstraintFunction struct RobotPointData { RobotPointData(const std::string &bName, const sva::PTransformd& X, const rbd::Jacobian& j); - const std::string& bName; + std::string bName; sva::PTransformd X_b_p; rbd::Jacobian jac; }; private: - std::vector dataVec_; + std::vector> dataVec_; std::vector dataVecRob_; int robotIndex_, bodyIndex_, alphaDBegin_; int nrVars_; @@ -752,14 +769,14 @@ class ImageConstr : public ConstraintFunction rbd::Jacobian jac_; sva::PTransformd X_b_gaze_; - Eigen::Matrix L_img_; - Eigen::Matrix surfaceVelocity_; - Eigen::Matrix L_Z_dot_; - Eigen::Matrix L_img_dot_; - Eigen::Vector2d speed_; - Eigen::Vector2d normalAcc_; + std::unique_ptr> L_img_; + std::unique_ptr> surfaceVelocity_; + std::unique_ptr> L_Z_dot_; + std::unique_ptr> L_img_dot_; + std::unique_ptr speed_; + std::unique_ptr normalAcc_; Eigen::MatrixXd jacMat_; - Eigen::Vector2d iDistMin_, iDistMax_, sDistMin_, sDistMax_; + std::unique_ptr iDistMin_, iDistMax_, sDistMin_, sDistMax_; double damping_, dampingOffset_, ineqInversion_, constrDirection_; Eigen::MatrixXd AInEq_; Eigen::VectorXd bInEq_; diff --git a/src/QPTasks.cpp b/src/QPTasks.cpp index 0326cff60..64aa70ac7 100644 --- a/src/QPTasks.cpp +++ b/src/QPTasks.cpp @@ -1076,7 +1076,7 @@ GazeTask::GazeTask(const std::vector& mbs, const Eigen::Vector2d& point2d, double depthEstimate, const sva::PTransformd& X_b_gaze, const Eigen::Vector2d& point2d_ref): - gazet_(new tasks::GazeTask(mbs[robotIndex], bodyName, point2d, depthEstimate, X_b_gaze, point2d_ref)), + gazet_(mbs[robotIndex], bodyName, point2d, depthEstimate, X_b_gaze, point2d_ref), robotIndex_(robotIndex) {} @@ -1085,31 +1085,10 @@ GazeTask::GazeTask(const std::vector& mbs, int robotIndex, const std::string& bodyName, const Eigen::Vector3d& point3d, const sva::PTransformd& X_b_gaze, const Eigen::Vector2d& point2d_ref): - gazet_(new tasks::GazeTask(mbs[robotIndex], bodyName, point3d, X_b_gaze, point2d_ref)), + gazet_(mbs[robotIndex], bodyName, point3d, X_b_gaze, point2d_ref), robotIndex_(robotIndex) {} -GazeTask::GazeTask(const GazeTask& rhs): - gazet_(new tasks::GazeTask(*rhs.gazet_)), - robotIndex_(rhs.robotIndex_) -{ -} - -GazeTask& GazeTask::operator=(const GazeTask& rhs) -{ - if(&rhs != this) - { - robotIndex_ = rhs.robotIndex_; - *gazet_ = *rhs.gazet_; - } - return *this; -} - -GazeTask::~GazeTask() -{ - delete gazet_; -} - int GazeTask::dim() { return 2; @@ -1120,31 +1099,31 @@ void GazeTask::update(const std::vector& mbs, const std::vector& mbcs, const SolverData& data) { - gazet_->update(mbs[robotIndex_], mbcs[robotIndex_], data.normalAccB(robotIndex_)); + gazet_.update(mbs[robotIndex_], mbcs[robotIndex_], data.normalAccB(robotIndex_)); } const Eigen::MatrixXd& GazeTask::jac() { - return gazet_->jac(); + return gazet_.jac(); } const Eigen::VectorXd& GazeTask::eval() { - return gazet_->eval(); + return gazet_.eval(); } const Eigen::VectorXd& GazeTask::speed() { - return gazet_->speed(); + return gazet_.speed(); } const Eigen::VectorXd& GazeTask::normalAcc() { - return gazet_->normalAcc(); + return gazet_.normalAcc(); } @@ -1157,32 +1136,10 @@ PositionBasedVisServoTask::PositionBasedVisServoTask(const std::vector& mbs, const std::vector& mbcs, const SolverData& data) { - pbvst_->update(mbs[robotIndex_], mbcs[robotIndex_], data.normalAccB(robotIndex_)); + pbvst_.update(mbs[robotIndex_], mbcs[robotIndex_], data.normalAccB(robotIndex_)); } const Eigen::MatrixXd& PositionBasedVisServoTask::jac() { - return pbvst_->jac(); + return pbvst_.jac(); } const Eigen::VectorXd& PositionBasedVisServoTask::eval() { - return pbvst_->eval(); + return pbvst_.eval(); } const Eigen::VectorXd& PositionBasedVisServoTask::speed() { - return pbvst_->speed(); + return pbvst_.speed(); } const Eigen::VectorXd& PositionBasedVisServoTask::normalAcc() { - return pbvst_->normalAcc(); + return pbvst_.normalAcc(); } @@ -1449,7 +1406,7 @@ MultiRobotTransformTask::MultiRobotTransformTask( mrtt_(mbs, r1Index, r2Index, r1BodyName, r2BodyName, X_r1b_r1s, X_r2b_r2s), Q_(), C_(), - CSum_(), + CSum_(Eigen::Vector6d::Zero()), preQ_() { int maxDof = 0; diff --git a/src/QPTasks.h b/src/QPTasks.h index 342b9fa5e..40664fbe7 100644 --- a/src/QPTasks.h +++ b/src/QPTasks.h @@ -760,27 +760,21 @@ class TASKS_DLLAPI GazeTask : public HighLevelTask const sva::PTransformd& X_b_gaze, const Eigen::Vector2d& point2d_ref = Eigen::Vector2d::Zero()); - GazeTask(const GazeTask& rhs); - - GazeTask& operator=(const GazeTask& rhs); - - ~GazeTask(); - tasks::GazeTask& task() { - return *gazet_; + return gazet_; } void error(const Eigen::Vector2d& point2d, const Eigen::Vector2d& point2d_ref = Eigen::Vector2d::Zero()) { - gazet_->error(point2d, point2d_ref); + gazet_.error(point2d, point2d_ref); } void error(const Eigen::Vector3d& point3d, const Eigen::Vector2d& point2d_ref = Eigen::Vector2d::Zero()) { - gazet_->error(point3d, point2d_ref); + gazet_.error(point3d, point2d_ref); } virtual int dim(); @@ -793,7 +787,7 @@ class TASKS_DLLAPI GazeTask : public HighLevelTask virtual const Eigen::VectorXd& normalAcc(); private: - tasks::GazeTask * const gazet_; + tasks::GazeTask gazet_; int robotIndex_; }; @@ -806,20 +800,14 @@ class TASKS_DLLAPI PositionBasedVisServoTask : public HighLevelTask const std::string& bodyName, const sva::PTransformd& X_t_s, const sva::PTransformd& X_b_s=sva::PTransformd::Identity()); - PositionBasedVisServoTask(const PositionBasedVisServoTask & rhs); - - PositionBasedVisServoTask& operator=(const PositionBasedVisServoTask & rhs); - - ~PositionBasedVisServoTask(); - tasks::PositionBasedVisServoTask& task() { - return *pbvst_; + return pbvst_; } void error(const sva::PTransformd& X_t_s) { - pbvst_->error(X_t_s); + pbvst_.error(X_t_s); } virtual int dim(); @@ -832,7 +820,7 @@ class TASKS_DLLAPI PositionBasedVisServoTask : public HighLevelTask virtual const Eigen::VectorXd& normalAcc(); private: - tasks::PositionBasedVisServoTask * const pbvst_; + tasks::PositionBasedVisServoTask pbvst_; int robotIndex_; }; @@ -983,7 +971,7 @@ class TASKS_DLLAPI MultiRobotTransformTask : public Task void dimWeight(const Eigen::Vector6d& dim); - const Eigen::Vector6d& dimWeight() const + const Eigen::VectorXd& dimWeight() const { return dimWeight_; } @@ -1008,12 +996,12 @@ class TASKS_DLLAPI MultiRobotTransformTask : public Task private: int alphaDBegin_; double stiffness_, stiffnessSqrt_; - Eigen::Vector6d dimWeight_; + Eigen::VectorXd dimWeight_; std::vector posInQ_, robotIndexes_; tasks::MultiRobotTransformTask mrtt_; Eigen::MatrixXd Q_; Eigen::VectorXd C_; - Eigen::Vector6d CSum_; + Eigen::VectorXd CSum_; // cache Eigen::MatrixXd preQ_; }; diff --git a/src/Tasks.cpp b/src/Tasks.cpp index b74e6b54d..1afae6ae6 100644 --- a/src/Tasks.cpp +++ b/src/Tasks.cpp @@ -683,16 +683,16 @@ const Eigen::MatrixXd& SurfaceOrientationTask::jacDot() const GazeTask::GazeTask(const rbd::MultiBody &mb, const std::string& bodyName, const Eigen::Vector2d& point2d, double depthEstimate, const sva::PTransformd& X_b_gaze, const Eigen::Vector2d& point2d_ref): - point2d_(point2d), - point2d_ref_(point2d_ref), + point2d_(new Eigen::Vector2d(point2d)), + point2d_ref_(new Eigen::Vector2d(point2d_ref)), depthEstimate_(depthEstimate), bodyIndex_(mb.bodyIndexByName(bodyName)), jac_(mb, bodyName), X_b_gaze_(X_b_gaze), - L_img_(Eigen::Matrix::Zero()), - surfaceVelocity_(Eigen::Matrix::Zero()), - L_Z_dot_(Eigen::Matrix ::Zero()), - L_img_dot_(Eigen::Matrix::Zero()), + L_img_(new Eigen::Matrix(Eigen::Matrix::Zero())), + surfaceVelocity_(new Eigen::Matrix(Eigen::Matrix::Zero())), + L_Z_dot_(new Eigen::Matrix(Eigen::Matrix::Zero())), + L_img_dot_(new Eigen::Matrix(Eigen::Matrix::Zero())), eval_(2), speed_(2), normalAcc_(2), @@ -705,37 +705,77 @@ GazeTask::GazeTask(const rbd::MultiBody &mb, const std::string& bodyName, GazeTask::GazeTask(const rbd::MultiBody &mb, const std::string& bodyName, const Eigen::Vector3d &point3d, const sva::PTransformd& X_b_gaze, const Eigen::Vector2d& point2d_ref): - point2d_ref_(point2d_ref), + point2d_(new Eigen::Vector2d()), + point2d_ref_(new Eigen::Vector2d(point2d_ref)), bodyIndex_(mb.bodyIndexByName(bodyName)), jac_(mb, bodyName), X_b_gaze_(X_b_gaze), - L_img_(Eigen::Matrix::Zero()), - surfaceVelocity_(Eigen::Matrix::Zero()), - L_Z_dot_(Eigen::Matrix ::Zero()), - L_img_dot_(Eigen::Matrix::Zero()), + L_img_(new Eigen::Matrix(Eigen::Matrix::Zero())), + surfaceVelocity_(new Eigen::Matrix(Eigen::Matrix::Zero())), + L_Z_dot_(new Eigen::Matrix(Eigen::Matrix::Zero())), + L_img_dot_(new Eigen::Matrix(Eigen::Matrix::Zero())), eval_(2), speed_(2), normalAcc_(2), jacMat_(2, mb.nrDof()), jacDotMat_(2, mb.nrDof()) { - point2d_ << point3d[0]/point3d[2], point3d[1]/point3d[2]; + *point2d_ << point3d[0]/point3d[2], point3d[1]/point3d[2]; depthEstimate_ = point3d[2]; } +GazeTask::GazeTask(const GazeTask& rhs): + point2d_(new Eigen::Vector2d(*rhs.point2d_)), + point2d_ref_(new Eigen::Vector2d(*rhs.point2d_ref_)), + depthEstimate_(rhs.depthEstimate_), + bodyIndex_(rhs.bodyIndex_), + jac_(rhs.jac_), + X_b_gaze_(rhs.X_b_gaze_), + L_img_(new Eigen::Matrix(*rhs.L_img_)), + surfaceVelocity_(new Eigen::Matrix(*rhs.surfaceVelocity_)), + L_Z_dot_(new Eigen::Matrix(*rhs.L_Z_dot_)), + L_img_dot_(new Eigen::Matrix(*rhs.L_img_dot_)), + eval_(rhs.eval_), speed_(rhs.speed_), + normalAcc_(rhs.normalAcc_), jacMat_(rhs.jacMat_), + jacDotMat_(rhs.jacDotMat_) +{ +} + +GazeTask& GazeTask::operator=(const GazeTask& rhs) +{ + if(&rhs != this) + { + *point2d_ = *rhs.point2d_; + *point2d_ref_ = *rhs.point2d_ref_; + depthEstimate_ = rhs.depthEstimate_; + bodyIndex_ = rhs.bodyIndex_; + jac_ = rhs.jac_; + X_b_gaze_ = rhs.X_b_gaze_; + *L_img_ = *rhs.L_img_; + *surfaceVelocity_ = *rhs.surfaceVelocity_; + *L_Z_dot_ = *rhs.L_Z_dot_; + *L_img_dot_ = *rhs.L_img_dot_; + eval_ = rhs.eval_; + speed_ = rhs.speed_; + normalAcc_ = rhs.normalAcc_; + jacMat_ = rhs.jacMat_; + jacDotMat_ = rhs.jacDotMat_; + } + return *this; +} void GazeTask::error(const Eigen::Vector2d& point2d, const Eigen::Vector2d& point2d_ref) { - point2d_ = point2d; - point2d_ref_ = point2d_ref; + *point2d_ = point2d; + *point2d_ref_ = point2d_ref; } void GazeTask::error(const Eigen::Vector3d& point3d, const Eigen::Vector2d& point2d_ref) { - point2d_ << point3d[0]/point3d[2], point3d[1]/point3d[2]; + *point2d_ << point3d[0]/point3d[2], point3d[1]/point3d[2]; depthEstimate_ = point3d[2]; - point2d_ref_ = point2d_ref; + *point2d_ref_ = point2d_ref; } @@ -743,21 +783,21 @@ void GazeTask::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc, const std::vector& normalAccB) { // compute eval term - eval_ = point2d_ref_ - point2d_; + eval_ = *point2d_ref_ - *point2d_; // compute speed term - rbd::imagePointJacobian(point2d_, depthEstimate_, L_img_); - surfaceVelocity_ = (jac_.velocity(mb, mbc, X_b_gaze_)).vector(); - speed_ = L_img_*surfaceVelocity_; + rbd::imagePointJacobian(*point2d_, depthEstimate_, *L_img_); + *surfaceVelocity_ = (jac_.velocity(mb, mbc, X_b_gaze_)).vector(); + speed_ = (*L_img_)*(*surfaceVelocity_); // compute norm accel term - rbd::depthDotJacobian(speed_, depthEstimate_, L_Z_dot_); - rbd::imagePointJacobianDot(point2d_, speed_, depthEstimate_, L_Z_dot_*surfaceVelocity_, L_img_dot_); - normalAcc_ = L_img_*(jac_.normalAcceleration(mb, mbc, normalAccB, X_b_gaze_, - sva::MotionVecd(Eigen::Vector6d::Zero()))).vector() + L_img_dot_*surfaceVelocity_; + rbd::depthDotJacobian(speed_, depthEstimate_, *L_Z_dot_); + rbd::imagePointJacobianDot(*point2d_, speed_, depthEstimate_, (*L_Z_dot_)*(*surfaceVelocity_), *L_img_dot_); + normalAcc_ = (*L_img_)*(jac_.normalAcceleration(mb, mbc, normalAccB, X_b_gaze_, + sva::MotionVecd(Eigen::Vector6d::Zero()))).vector() + (*L_img_dot_)*(*surfaceVelocity_); // compute the task Jacobian - Eigen::MatrixXd shortJacMat = L_img_*jac_.jacobian(mb, mbc, X_b_gaze_*mbc.bodyPosW[bodyIndex_]).block(0, 0, 6, jac_.dof()); + Eigen::MatrixXd shortJacMat = (*L_img_)*jac_.jacobian(mb, mbc, X_b_gaze_*mbc.bodyPosW[bodyIndex_]).block(0, 0, 6, jac_.dof()); jac_.fullJacobian(mb, shortJacMat, jacMat_); } @@ -806,10 +846,10 @@ PositionBasedVisServoTask::PositionBasedVisServoTask(const rbd::MultiBody &mb, axis_(Eigen::Vector3d::Zero()), bodyIndex_(mb.bodyIndexByName(bodyName)), jac_(mb, bodyName), - L_pbvs_(Eigen::Matrix::Zero()), - surfaceVelocity_(Eigen::Matrix::Zero()), + L_pbvs_(new Eigen::Matrix(Eigen::Matrix::Zero())), + surfaceVelocity_(new Eigen::Matrix(Eigen::Matrix::Zero())), omegaSkew_(Eigen::Matrix3d::Zero()), - L_pbvs_dot_(Eigen::Matrix::Zero()), + L_pbvs_dot_(new Eigen::Matrix(Eigen::Matrix::Zero())), eval_(6), speed_(6), normalAcc_(6), @@ -818,6 +858,44 @@ PositionBasedVisServoTask::PositionBasedVisServoTask(const rbd::MultiBody &mb, { } +PositionBasedVisServoTask::PositionBasedVisServoTask(const PositionBasedVisServoTask& rhs): + X_t_s_(rhs.X_t_s_), + X_b_s_(rhs.X_b_s_), + angle_(rhs.angle_), + axis_(rhs.axis_), + bodyIndex_(rhs.bodyIndex_), + jac_(rhs.jac_), + L_pbvs_(new Eigen::Matrix(*rhs.L_pbvs_)), + surfaceVelocity_(new Eigen::Matrix(*rhs.surfaceVelocity_)), + omegaSkew_(rhs.omegaSkew_), + L_pbvs_dot_(new Eigen::Matrix(*rhs.L_pbvs_dot_)), + eval_(rhs.eval_), speed_(rhs.speed_), normalAcc_(rhs.normalAcc_), + jacMat_(rhs.jacMat_), jacDotMat_(rhs.jacDotMat_) +{ +} + +PositionBasedVisServoTask& PositionBasedVisServoTask::operator=(const PositionBasedVisServoTask& rhs) +{ + if(&rhs != this) + { + X_t_s_ = rhs.X_t_s_; + X_b_s_ = rhs.X_b_s_; + angle_ = rhs.angle_; + axis_ = rhs.axis_; + bodyIndex_ = rhs.bodyIndex_; + jac_ = rhs.jac_; + *L_pbvs_ = *rhs.L_pbvs_; + *surfaceVelocity_ = *rhs.surfaceVelocity_; + omegaSkew_ = rhs.omegaSkew_; + *L_pbvs_dot_ = *rhs.L_pbvs_dot_; + eval_ = rhs.eval_; + speed_ = rhs.speed_; + normalAcc_ = rhs.normalAcc_; + jacMat_ = rhs.jacMat_; + jacDotMat_ = rhs.jacDotMat_; + } + return *this; +} void PositionBasedVisServoTask::error(const sva::PTransformd& X_t_s) { @@ -833,19 +911,19 @@ void PositionBasedVisServoTask::update(const rbd::MultiBody& mb, const rbd::Mult eval_.head(3) = angle_*axis_; // compute speed term - rbd::poseJacobian(X_t_s_.rotation(), L_pbvs_); - surfaceVelocity_ = (jac_.velocity(mb, mbc, X_b_s_)).vector(); - speed_ = L_pbvs_*surfaceVelocity_; + rbd::poseJacobian(X_t_s_.rotation(), *L_pbvs_); + *surfaceVelocity_ = (jac_.velocity(mb, mbc, X_b_s_)).vector(); + speed_ = (*L_pbvs_)*(*surfaceVelocity_); // compute norm accel term - rbd::getSkewSym(surfaceVelocity_.head(3), omegaSkew_); - L_pbvs_dot_ << Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), + rbd::getSkewSym(surfaceVelocity_->head(3), omegaSkew_); + *L_pbvs_dot_ << Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), -X_t_s_.rotation().transpose()*omegaSkew_; - normalAcc_ = L_pbvs_*(jac_.normalAcceleration(mb, mbc, normalAccB, X_b_s_, - sva::MotionVecd(Eigen::Vector6d::Zero()))).vector() + L_pbvs_dot_*surfaceVelocity_; + normalAcc_ = (*L_pbvs_)*(jac_.normalAcceleration(mb, mbc, normalAccB, X_b_s_, + sva::MotionVecd(Eigen::Vector6d::Zero()))).vector() + (*L_pbvs_dot_)*(*surfaceVelocity_); // compute the task Jacobian - Eigen::MatrixXd shortJacMat = L_pbvs_*jac_.jacobian(mb, mbc, X_b_s_*mbc.bodyPosW[bodyIndex_]).block(0, 0, 6, jac_.dof()); + Eigen::MatrixXd shortJacMat = (*L_pbvs_)*jac_.jacobian(mb, mbc, X_b_s_*mbc.bodyPosW[bodyIndex_]).block(0, 0, 6, jac_.dof()); jac_.fullJacobian(mb, shortJacMat, jacMat_); } diff --git a/src/Tasks.h b/src/Tasks.h index 767c5903e..90cbbaa9e 100644 --- a/src/Tasks.h +++ b/src/Tasks.h @@ -29,6 +29,8 @@ #include +#include + // forward declarations // RBDyn namespace rbd @@ -281,7 +283,6 @@ class TASKS_DLLAPI SurfaceOrientationTask class TASKS_DLLAPI GazeTask { public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW GazeTask(const rbd::MultiBody& mb, const std::string& bodyName, const Eigen::Vector2d &point2d, double depthEstimate, const sva::PTransformd& X_b_gaze, @@ -291,6 +292,14 @@ class TASKS_DLLAPI GazeTask const sva::PTransformd& X_b_gaze, const Eigen::Vector2d &point2d_ref=Eigen::Vector2d::Zero()); + GazeTask(const GazeTask& rhs); + + GazeTask(GazeTask&&) = default; + + GazeTask& operator=(const GazeTask& rhs); + + GazeTask& operator=(GazeTask&&) = default; + void error(const Eigen::Vector2d& point2d, const Eigen::Vector2d &point2d_ref=Eigen::Vector2d::Zero()); void error(const Eigen::Vector3d& point3d, @@ -307,16 +316,16 @@ class TASKS_DLLAPI GazeTask const Eigen::MatrixXd& jacDot() const; private: - Eigen::Vector2d point2d_; - Eigen::Vector2d point2d_ref_; + std::unique_ptr point2d_; + std::unique_ptr point2d_ref_; double depthEstimate_; int bodyIndex_; rbd::Jacobian jac_; sva::PTransformd X_b_gaze_; - Eigen::Matrix L_img_; - Eigen::Matrix surfaceVelocity_; - Eigen::Matrix L_Z_dot_; - Eigen::Matrix L_img_dot_; + std::unique_ptr> L_img_; + std::unique_ptr> surfaceVelocity_; + std::unique_ptr> L_Z_dot_; + std::unique_ptr> L_img_dot_; Eigen::VectorXd eval_; Eigen::VectorXd speed_; @@ -330,10 +339,17 @@ class TASKS_DLLAPI GazeTask class TASKS_DLLAPI PositionBasedVisServoTask { public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW PositionBasedVisServoTask(const rbd::MultiBody &mb, const std::string& bodyName, const sva::PTransformd& X_t_s, const sva::PTransformd& X_b_s); + PositionBasedVisServoTask(const PositionBasedVisServoTask& rhs); + + PositionBasedVisServoTask(PositionBasedVisServoTask&&) = default; + + PositionBasedVisServoTask& operator=(const PositionBasedVisServoTask& rhs); + + PositionBasedVisServoTask& operator=(PositionBasedVisServoTask&&) = default; + void error(const sva::PTransformd& X_t_s); void update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc, @@ -353,10 +369,10 @@ class TASKS_DLLAPI PositionBasedVisServoTask Eigen::Vector3d axis_; int bodyIndex_; rbd::Jacobian jac_; - Eigen::Matrix L_pbvs_; - Eigen::Matrix surfaceVelocity_; + std::unique_ptr> L_pbvs_; + std::unique_ptr> surfaceVelocity_; Eigen::Matrix3d omegaSkew_; - Eigen::Matrix L_pbvs_dot_; + std::unique_ptr> L_pbvs_dot_; Eigen::VectorXd eval_; Eigen::VectorXd speed_; From 293a851c61309b2c6080428cc3378faaeb313329 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Thu, 2 Mar 2017 06:47:10 +0000 Subject: [PATCH 3/5] Add allocation test --- tests/AllocationTest.cpp | 95 ++++++++++++++++++++++++++++++++++++++++ tests/CMakeLists.txt | 1 + 2 files changed, 96 insertions(+) create mode 100644 tests/AllocationTest.cpp diff --git a/tests/AllocationTest.cpp b/tests/AllocationTest.cpp new file mode 100644 index 000000000..3d21e8776 --- /dev/null +++ b/tests/AllocationTest.cpp @@ -0,0 +1,95 @@ +// Copyright 2012-2017 CNRS-UM LIRMM, CNRS-AIST JRL +// +// This file is part of Tasks. +// +// Tasks 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. +// +// Tasks 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 Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with Tasks. If not, see . + + +// boost +#define BOOST_TEST_MODULE AllocationTest +#include +#include + +// Tasks +#include "QPConstr.h" +#include "QPTasks.h" + +// Arms +#include "arms.h" + +#pragma GCC diagnostic ignored "-Wunused-variable" + +std::string class_name(const std::string & fn_name) +{ + auto eq_pos = fn_name.find_first_of('=') + 2; + auto c_pos = fn_name.find_first_of(';'); + if(c_pos == std::string::npos) + { + c_pos = fn_name.find_first_of(','); + } + return fn_name.substr(eq_pos, c_pos - eq_pos); +} + +template +void test_raw_ptr_creation(Args ... args) +{ +#ifdef __GNUG__ + std::string T_name = class_name(__PRETTY_FUNCTION__); + std::cout << "Testing raw pointer creation for " << T_name << std::endl; +#endif + T * p = nullptr; + for(size_t i = 0; i < 100; ++i) + { + new T(args...); + } +} + +template +void test_shared_ptr_creation(Args ... args) +{ +#ifdef __GNUG__ + std::string T_name = class_name(__PRETTY_FUNCTION__); + std::cout << "Testing shared_ptr creation for for " << T_name << std::endl; +#endif + std::shared_ptr p = nullptr; + for(size_t i = 0; i < 100; ++i) + { + p = std::make_shared(args...); + } +} + +BOOST_AUTO_TEST_CASE(AllocationTest) +{ + rbd::MultiBody mb1, mb2; + rbd::MultiBodyConfig mbc1Init, mbc2Init; + + std::tie(mb1, mbc1Init) = makeZXZArm(); + std::tie(mb2, mbc2Init) = makeZXZArm(); + + std::vector mbs = {mb1, mb2}; + Eigen::Vector2d pt2d = Eigen::Vector2d::Zero(); + sva::PTransformd pt = sva::PTransformd::Identity(); + test_raw_ptr_creation(mbs, 0, "b3", pt, 1.0); + test_raw_ptr_creation(mbs[0], "b3", pt2d, 1.0, pt); + test_raw_ptr_creation(mbs[0], "b3", pt, pt); + test_raw_ptr_creation(mbs, 0, "b3", pt2d, 1.0, pt); + test_raw_ptr_creation(mbs, 0, "b3", pt, pt); + test_raw_ptr_creation(mbs, 0, 1, "b3", "b3", pt, pt, 1.0, 1.0); + test_shared_ptr_creation(mbs, 0, "b3", pt, 1.0); + test_shared_ptr_creation(mbs[0], "b3", pt2d, 1.0, pt); + test_shared_ptr_creation(mbs[0], "b3", pt, pt); + test_shared_ptr_creation(mbs, 0, "b3", pt2d, 1.0, pt); + test_shared_ptr_creation(mbs, 0, "b3", pt, pt); + test_shared_ptr_creation(mbs, 0, 1, "b3", "b3", pt, pt, 1.0, 1.0); +} diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 7bf9811f9..739b5f151 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -47,3 +47,4 @@ endmacro(addUnitTest) addUnitTest(QPSolverTest) addUnitTest(QPMultiRobotTest) addUnitTest(TasksTest) +addUnitTest(AllocationTest) From 39f0f9894c39c7f20411904feba70864a639af6b Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Thu, 2 Mar 2017 10:04:19 +0000 Subject: [PATCH 4/5] Add DLL interface for ImageConstr --- src/QPConstr.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/QPConstr.h b/src/QPConstr.h index 9b4a488d7..b13c06eb3 100644 --- a/src/QPConstr.h +++ b/src/QPConstr.h @@ -660,7 +660,7 @@ class TASKS_DLLAPI BoundedSpeedConstr : public ConstraintFunction * or an Occlusion avoidance constraint. Both with damping * Note that the point must start within the feasible boundary. */ -class ImageConstr : public ConstraintFunction +class TASKS_DLLAPI ImageConstr : public ConstraintFunction { public: /** From 9f8786552070d6be691c4f47b808ad618947e2a0 Mon Sep 17 00:00:00 2001 From: Andrew Hundt Date: Wed, 26 Apr 2017 00:19:00 -0400 Subject: [PATCH 5/5] BoundedSpeedConstr becomes BoundedCartesianMotionConstr with support for velocity and acceleration --- binding/python/generate.py | 22 ++--- src/QPConstr.cpp | 163 +++++++++++++++++++++++++++---------- src/QPConstr.h | 91 ++++++++++++++++++--- src/QPContactConstr.h | 4 +- tests/QPSolverTest.cpp | 6 +- 5 files changed, 212 insertions(+), 74 deletions(-) diff --git a/binding/python/generate.py b/binding/python/generate.py index fba4624b3..a0749aa5a 100644 --- a/binding/python/generate.py +++ b/binding/python/generate.py @@ -434,13 +434,13 @@ def build_qp(tasks): damperJointLimitsConstr = qp.add_class('DamperJointLimitsConstr', parent=[boundConstr, constr]) gripperTorqueConstr = qp.add_class('GripperTorqueConstr', parent=[ineqConstr, constr]) - boundedSpeedConstr = qp.add_class('BoundedSpeedConstr', parent=[genineqConstr, constr]) + BoundedCartesianMotionConstr = qp.add_class('BoundedCartesianMotionConstr', parent=[genineqConstr, constr]) imageConstr = qp.add_class('ImageConstr', parent=[ineqConstr, constr]) constrName = ['MotionConstr', 'MotionPolyConstr', 'ContactAccConstr', 'ContactSpeedConstr', 'CollisionConstr', 'JointLimitsConstr', 'DamperJointLimitsConstr', - 'MotionSpringConstr', 'GripperTorqueConstr', 'BoundedSpeedConstr', + 'MotionSpringConstr', 'GripperTorqueConstr', 'BoundedCartesianMotionConstr', 'CoMIncPlaneConstr', 'PositiveLambda', 'ContactPosConstr', 'ImageConstr'] eqConstrName = ['ContactAccConstr', 'ContactSpeedConstr', 'ContactPosConstr'] ineqConstrName = ['CollisionConstr', 'GripperTorqueConstr', 'CoMIncPlaneConstr', 'ImageConstr'] @@ -456,7 +456,7 @@ def build_qp(tasks): 'SurfaceTransformTask', 'RelativeDistTask', 'VectorOrientationTask'] constrList = [motionConstr, motionPolyConstr, contactAccConstr, contactSpeedConstr, collisionConstr, jointLimitsConstr, damperJointLimitsConstr, - motionSpringConstr, gripperTorqueConstr, boundedSpeedConstr, + motionSpringConstr, gripperTorqueConstr, BoundedCartesianMotionConstr, comIncPlaneConstr, positiveLambdaConstr, contactPosConstr, imageConstr] # build list type @@ -1536,29 +1536,29 @@ def addMotionDefault(motion): [param('const tasks::qp::ContactId&', 'contactId')]) gripperTorqueConstr.add_method('reset', None, []) - # BoundedSpeedConstr - boundedSpeedConstr.add_constructor([param('const std::vector&', 'mbs'), + # BoundedCartesianMotionConstr + BoundedCartesianMotionConstr.add_constructor([param('const std::vector&', 'mbs'), param('int', 'robotIndex'), param('double', 'timeStep')]) - boundedSpeedConstr.add_method('addBoundedSpeed', None, + BoundedCartesianMotionConstr.add_method('addBoundedSpeed', None, [param('const std::vector&', 'mbs'), param('const std::string&', 'bodyName'), param('const Eigen::Vector3d&', 'bodyPoint'), param('const Eigen::MatrixXd&', 'dof'), param('const Eigen::VectorXd&', 'speed')]) - boundedSpeedConstr.add_method('addBoundedSpeed', None, + BoundedCartesianMotionConstr.add_method('addBoundedSpeed', None, [param('const std::vector&', 'mbs'), param('const std::string&', 'bodyName'), param('const Eigen::Vector3d&', 'bodyPoint'), param('const Eigen::MatrixXd&', 'dof'), param('const Eigen::VectorXd&', 'lowerSpeed'), param('const Eigen::VectorXd&', 'upperSpeed')]) - boundedSpeedConstr.add_method('removeBoundedSpeed', retval('bool'), + BoundedCartesianMotionConstr.add_method('removeBoundedSpeed', retval('bool'), [param('const std::string&', 'bodyName')]) - boundedSpeedConstr.add_method('resetBoundedSpeeds', None, []) - boundedSpeedConstr.add_method('nrBoundedSpeeds', retval('int'), []) + BoundedCartesianMotionConstr.add_method('resetBoundedSpeeds', None, []) + BoundedCartesianMotionConstr.add_method('nrBoundedSpeeds', retval('int'), []) - boundedSpeedConstr.add_method('updateBoundedSpeeds', None, []) + BoundedCartesianMotionConstr.add_method('updateBoundedSpeeds', None, []) # ImageConstr imageConstr.add_constructor([param('const std::vector&', 'mbs'), diff --git a/src/QPConstr.cpp b/src/QPConstr.cpp index 55685ed78..6af735dec 100644 --- a/src/QPConstr.cpp +++ b/src/QPConstr.cpp @@ -1004,14 +1004,15 @@ const Eigen::VectorXd& GripperTorqueConstr::bInEq() const /** - * BoundedSpeedConstr + * BoundedCartesianMotionConstr */ -BoundedSpeedConstr::BoundedSpeedConstr(const std::vector& mbs, +BoundedCartesianMotionConstr::BoundedCartesianMotionConstr(const std::vector& mbs, int robotIndex, double timeStep): robotIndex_(robotIndex), - cont_(), + velConstr_(), + accConstr_(), fullJac_(6, mbs[robotIndex_].nrDof()), A_(), lower_(), @@ -1021,7 +1022,7 @@ BoundedSpeedConstr::BoundedSpeedConstr(const std::vector& mbs, {} -void BoundedSpeedConstr::addBoundedSpeed( +void BoundedCartesianMotionConstr::addBoundedSpeed( const std::vector& mbs, const std::string& bodyName, const Eigen::Vector3d& bodyPoint, const Eigen::MatrixXd& dof, const Eigen::VectorXd& speed) @@ -1030,53 +1031,87 @@ void BoundedSpeedConstr::addBoundedSpeed( } -void BoundedSpeedConstr::addBoundedSpeed( +void BoundedCartesianMotionConstr::addBoundedSpeed( const std::vector& mbs, const std::string& bodyName, const Eigen::Vector3d& bodyPoint, const Eigen::MatrixXd& dof, const Eigen::VectorXd& lowerSpeed, const Eigen::VectorXd& upperSpeed) { rbd::Jacobian jac(mbs[robotIndex_], bodyName, bodyPoint); - cont_.push_back({jac, dof, lowerSpeed, upperSpeed, bodyName}); + velConstr_.push_back({jac, dof, lowerSpeed, upperSpeed, bodyName}); } +bool BoundedCartesianMotionConstr::removeBound(std::vector& constr, const std::string& bodyName) +{ + auto it = std::find_if(constr.begin(), constr.end(), + [bodyName](const BoundedCartesianMotionData& data) + { + return data.bodyName == bodyName; + }); + + if(it != constr.end()) + { + constr.erase(it); + return true; + } + return false; +} -bool BoundedSpeedConstr::removeBoundedSpeed(const std::string& bodyName) +bool BoundedCartesianMotionConstr::removeBoundedSpeed(const std::string& bodyName) { - auto it = std::find_if(cont_.begin(), cont_.end(), - [bodyName](const BoundedSpeedData& data) - { - return data.bodyName == bodyName; - }); + return removeBound(velConstr_, bodyName); +} - if(it != cont_.end()) - { - cont_.erase(it); - return true; - } - return false; +bool BoundedCartesianMotionConstr::removeBoundedAcceleration(const std::string& bodyName) +{ + return removeBound(accConstr_, bodyName); +} + + +void BoundedCartesianMotionConstr::resetBoundedSpeeds() +{ + velConstr_.clear(); +} + + +void BoundedCartesianMotionConstr::resetBoundedAccelerations() +{ + accConstr_.clear(); } -void BoundedSpeedConstr::resetBoundedSpeeds() +void BoundedCartesianMotionConstr::reset() { - cont_.clear(); + resetBoundedSpeeds(); + resetBoundedAccelerations(); } -std::size_t BoundedSpeedConstr::nrBoundedSpeeds() const +std::size_t BoundedCartesianMotionConstr::nrBoundedSpeeds() const { - return cont_.size(); + return velConstr_.size(); } -void BoundedSpeedConstr::updateBoundedSpeeds() +std::size_t BoundedCartesianMotionConstr::nrBoundedMotions() const +{ + return velConstr_.size() + velConstr_.size(); +} + + +std::size_t BoundedCartesianMotionConstr::nrBoundedAccelerations() const +{ + return velConstr_.size() + velConstr_.size(); +} + + +void BoundedCartesianMotionConstr::updateBoundedMotions() { updateNrEq(); } -void BoundedSpeedConstr::updateNrVars(const std::vector& /* mbs */, +void BoundedCartesianMotionConstr::updateNrVars(const std::vector& /* mbs */, const SolverData& data) { alphaDBegin_ = data.alphaDBegin(robotIndex_); @@ -1085,7 +1120,7 @@ void BoundedSpeedConstr::updateNrVars(const std::vector& /* mbs } -void BoundedSpeedConstr::update(const std::vector& mbs, +void BoundedCartesianMotionConstr::update(const std::vector& mbs, const std::vector& mbcs, const SolverData& data) { @@ -1099,43 +1134,77 @@ void BoundedSpeedConstr::update(const std::vector& mbs, // (TargetSpeed - V_k)/dt - JD_k*alpha_k = J_k*alphaD_{k+1} int index = 0; - for(std::size_t i = 0; i < cont_.size(); ++i) + // Speed limits + for(std::size_t i = 0; i < velConstr_.size(); ++i) { - int rows = int(cont_[i].dof.rows()); + int rows = int(velConstr_[i].dof.rows()); // AEq - const MatrixXd& jac = cont_[i].jac.bodyJacobian(mb, mbc); - cont_[i].jac.fullJacobian(mb, jac, fullJac_); + const MatrixXd& jac = velConstr_[i].jac.bodyJacobian(mb, mbc); + velConstr_[i].jac.fullJacobian(mb, jac, fullJac_); A_.block(index, alphaDBegin_, rows, mb.nrDof()).noalias() = - cont_[i].dof*fullJac_; + velConstr_[i].dof*fullJac_; // BEq - Vector6d speed = cont_[i].jac.bodyVelocity(mb, mbc).vector(); - Vector6d normalAcc = cont_[i].jac.bodyNormalAcceleration( + Vector6d speed = velConstr_[i].jac.bodyVelocity(mb, mbc).vector(); + Vector6d normalAcc = velConstr_[i].jac.bodyNormalAcceleration( mb, mbc, data.normalAccB(robotIndex_)).vector(); - lower_.segment(index, rows).noalias() = cont_[i].dof*(-normalAcc - + lower_.segment(index, rows).noalias() = velConstr_[i].dof*(-normalAcc - (speed/timeStep_)); upper_.segment(index, rows).noalias() = lower_.segment(index, rows); - lower_.segment(index, rows).noalias() += (cont_[i].lSpeed/timeStep_); - upper_.segment(index, rows).noalias() += (cont_[i].uSpeed/timeStep_); + lower_.segment(index, rows).noalias() += (velConstr_[i].lowerLimit/timeStep_); + upper_.segment(index, rows).noalias() += (velConstr_[i].upperLimit/timeStep_); + index += rows; + } + + // Acceleration limits + for(std::size_t i = 0; i < accConstr_.size(); ++i) + { + int rows = int(accConstr_[i].dof.rows()); + + // AEq + const MatrixXd& jac = accConstr_[i].jac.bodyJacobian(mb, mbc); + accConstr_[i].jac.fullJacobian(mb, jac, fullJac_); + A_.block(index, alphaDBegin_, rows, mb.nrDof()).noalias() = + accConstr_[i].dof*fullJac_; + + // BEq + Vector6d acceleration = accConstr_[i].jac.bodyNormalAcceleration(mb, mbc).vector(); + Vector6d normalAcc = accConstr_[i].jac.bodyNormalAcceleration( + mb, mbc, data.normalAccB(robotIndex_)).vector(); + + lower_.segment(index, rows).noalias() = accConstr_[i].dof*(-normalAcc - + acceleration); + upper_.segment(index, rows).noalias() = lower_.segment(index, rows); + + lower_.segment(index, rows).noalias() += accConstr_[i].lowerLimit; + upper_.segment(index, rows).noalias() += accConstr_[i].upperLimit; index += rows; } } -std::string BoundedSpeedConstr::nameGenInEq() const +std::string BoundedCartesianMotionConstr::nameGenInEq() const { - return "BoundedSpeedConstr"; + return "BoundedCartesianMotionConstr"; } -std::string BoundedSpeedConstr::descGenInEq(const std::vector& mbs, +std::string BoundedCartesianMotionConstr::descGenInEq(const std::vector& mbs, int line) { int curRow = 0; - for(const BoundedSpeedData& c: cont_) + for(const BoundedCartesianMotionData& c: velConstr_) + { + curRow += int(c.dof.rows()); + if(line < curRow) + { + return std::string("Body: ") + mbs[robotIndex_].body(c.body).name(); + } + } + for(const BoundedCartesianMotionData& c: accConstr_) { curRow += int(c.dof.rows()); if(line < curRow) @@ -1147,34 +1216,38 @@ std::string BoundedSpeedConstr::descGenInEq(const std::vector& m } -int BoundedSpeedConstr::maxGenInEq() const +int BoundedCartesianMotionConstr::maxGenInEq() const { return int(A_.rows()); } -const Eigen::MatrixXd& BoundedSpeedConstr::AGenInEq() const +const Eigen::MatrixXd& BoundedCartesianMotionConstr::AGenInEq() const { return A_; } -const Eigen::VectorXd& BoundedSpeedConstr::LowerGenInEq() const +const Eigen::VectorXd& BoundedCartesianMotionConstr::LowerGenInEq() const { return lower_; } -const Eigen::VectorXd& BoundedSpeedConstr::UpperGenInEq() const +const Eigen::VectorXd& BoundedCartesianMotionConstr::UpperGenInEq() const { return upper_; } -void BoundedSpeedConstr::updateNrEq() +void BoundedCartesianMotionConstr::updateNrEq() { int nrEq = 0; - for(const BoundedSpeedData& c: cont_) + for(const BoundedCartesianMotionData& c: velConstr_) + { + nrEq += int(c.dof.rows()); + } + for(const BoundedCartesianMotionData& c: accConstr_) { nrEq += int(c.dof.rows()); } diff --git a/src/QPConstr.h b/src/QPConstr.h index b13c06eb3..cc41df350 100644 --- a/src/QPConstr.h +++ b/src/QPConstr.h @@ -519,7 +519,8 @@ class TASKS_DLLAPI GripperTorqueConstr : public ConstraintFunction /** - * Constraint some link velocity in link frame by direct integration. + * Constrain link velocity and acceleration in link frame. + * Velocity is constrained by direct integration. * \f[ * S (\underline{v} - v) * \leq S a \Delta_{dt} \leq @@ -530,7 +531,7 @@ class TASKS_DLLAPI GripperTorqueConstr : public ConstraintFunction * \f$ a \f$ the link acceleration in link frame * and \f$ S \f$ the axis selection matrix. */ -class TASKS_DLLAPI BoundedSpeedConstr : public ConstraintFunction +class TASKS_DLLAPI BoundedCartesianMotionConstr : public ConstraintFunction { public: /** @@ -538,7 +539,7 @@ class TASKS_DLLAPI BoundedSpeedConstr : public ConstraintFunction * @param robotIndex Constrained robot Index in mbs. * @param timeStep Time step in second. */ - BoundedSpeedConstr(const std::vector& mbs, + BoundedCartesianMotionConstr(const std::vector& mbs, int robotIndex, double timeStep); /** @@ -555,7 +556,7 @@ class TASKS_DLLAPI BoundedSpeedConstr : public ConstraintFunction * \f$ \underline{v} \in \mathbb{R}^{n} \f$ and * \f$ \overline{v} \in \mathbb{R}^{n} \f$. */ - void addBoundedSpeed(const std::vector& mbs, + void addBoundedSpeed(const std::vector& mbs, const std::string& bodyName, const Eigen::Vector3d& bodyPoint, const Eigen::MatrixXd& dof, const Eigen::VectorXd& speed); @@ -586,14 +587,74 @@ class TASKS_DLLAPI BoundedSpeedConstr : public ConstraintFunction */ bool removeBoundedSpeed(const std::string& bodyName); - /// Remove all bounded speed constraint. + /// Remove all bounded speed constraints. void resetBoundedSpeeds(); - /// @return Number of bounded speed constraint. + /** + * Add a targeted acceleration constraint. + * Don't forget to call updateBoundedAccelerations and QPSolver::updateConstrSize. + * You can also only call QPSolver::nrVars or QPSolver::updateConstrsNrVars + * or QPSolver::updateNrVars. + * @param mbs Multi-robot system (must be the same given in the constructor. + * @param bodyId Constrained body id in mbs[robotIndex]. + * @param bodyPoint static translation applied on the link + * \f$ v = xlt(bodyPoint) v_{bodyId} \f$. + * @param dof \f$ S \in \mathbb{R}^{n \times 6} \f$. + * @param acceleration Targeted velocity + * \f$ \underline{v} \in \mathbb{R}^{n} \f$ and + * \f$ \overline{v} \in \mathbb{R}^{n} \f$. + */ + void addBoundedAcceleration(const std::vector& mbs, + const std::string& bodyName, + const Eigen::Vector3d& bodyPoint, const Eigen::MatrixXd& dof, + const Eigen::VectorXd& acceleration); + + /** + * Add a bounded acceleration constraint. + * Don't forget to call updateBoundedAccelerations and QPSolver::updateConstrSize. + * You can also only call QPSolver::nrVars or QPSolver::updateConstrsNrVars + * or QPSolver::updateNrVars. + * @param mbs Multi-robot system (must be the same given in the constructor. + * @param bodyId Constrained body id in mbs[robotIndex]. + * @param bodyPoint static translation applied on the link + * \f$ v = xlt(bodyPoint) v_{bodyId} \f$. + * @param dof \f$ S \in \mathbb{R}^{n \times 6} \f$. + * @param lowerAcceleration Lower velocity \f$ \underline{v} \in \mathbb{R}^{n} \f$. + * @param upperAcceleration Upper velocity \f$ \overline{v} \in \mathbb{R}^{n} \f$. + */ + void addBoundedAcceleration(const std::vector& mbs, + const std::string& bodyName, + const Eigen::Vector3d& bodyPoint, const Eigen::MatrixXd& dof, + const Eigen::VectorXd& lowerAcceleration, const Eigen::VectorXd& upperAcceleration); + + /** + * Remove a bounded acceleration constraint. + * @param bodyName Remove the constraint applied on bodyName. + * @return true if the constraint has been removed false if bodyId + * was associated with no constraint. + */ + bool removeBoundedAcceleration(const std::string& bodyName); + + /// Remove all bounded acceleration constraints. + void resetBoundedAccelerations(); + + /// Remove all bounded motion constraints, + /// including both speed and acceleration. + void reset(); + + /// @return Number of bounded speed constraints. std::size_t nrBoundedSpeeds() const; + /// @return Number of bounded acceleration constraints. + std::size_t nrBoundedAccelerations() const; + + /// @return Number of bounded speed + acceleration constraints. + std::size_t nrBoundedMotions() const; + /// Reallocate A and b matrix. - void updateBoundedSpeeds(); + /// Applies to both speed and + /// acceleration. + void updateBoundedMotions(); // Constraint virtual void updateNrVars(const std::vector& mbs, @@ -614,16 +675,18 @@ class TASKS_DLLAPI BoundedSpeedConstr : public ConstraintFunction virtual const Eigen::VectorXd& UpperGenInEq() const; private: - struct BoundedSpeedData + struct BoundedCartesianMotionData { - BoundedSpeedData(rbd::Jacobian j, const Eigen::MatrixXd& d, + // Represents a limit placed on some type of cartesian motion + // can be both speed and acceleration + BoundedCartesianMotionData(rbd::Jacobian j, const Eigen::MatrixXd& d, const Eigen::VectorXd& ls, const Eigen::VectorXd& us, const std::string& bName): jac(j), bodyPoint(j.point()), dof(d), - lSpeed(ls), - uSpeed(us), + lowerLimit(ls), + upperLimit(us), body(j.jointsPath().back()), bodyName(bName) {} @@ -631,17 +694,19 @@ class TASKS_DLLAPI BoundedSpeedConstr : public ConstraintFunction rbd::Jacobian jac; sva::PTransformd bodyPoint; Eigen::MatrixXd dof; - Eigen::VectorXd lSpeed, uSpeed; + Eigen::VectorXd lowerLimit, upperLimit; int body; std::string bodyName; }; private: void updateNrEq(); + bool removeBound(std::vector& constr, const std::string& bodyName); private: int robotIndex_, alphaDBegin_; - std::vector cont_; + std::vector velConstr_; + std::vector accConstr_; Eigen::MatrixXd fullJac_; diff --git a/src/QPContactConstr.h b/src/QPContactConstr.h index 4939bf02d..de028f519 100644 --- a/src/QPContactConstr.h +++ b/src/QPContactConstr.h @@ -58,12 +58,12 @@ class TASKS_DLLAPI ContactConstrCommon void resetVirtualContacts(); /** - * Free some degree of freedom of a contact like in the BoundedSpeedConstr. + * Free some degree of freedom of a contact like in the BoundedCartesianMotionConstr. * \f[ \bar{v} = S v \f] * with \f$ v \f$ the velocity in the UnilateralContact and BilateralContact * \f$ cf \f$ frame. * @param dof \f$ S \in \mathbb{R}^{n \times 6} \f$ - * @see BoundedSpeedConstr + * @see BoundedCartesianMotionConstr * @see ContactConstr::updateDofContacts */ bool addDofContact(const ContactId& contactId, const Eigen::MatrixXd& dof); diff --git a/tests/QPSolverTest.cpp b/tests/QPSolverTest.cpp index c5bdcfec5..fc6700107 100644 --- a/tests/QPSolverTest.cpp +++ b/tests/QPSolverTest.cpp @@ -1340,7 +1340,7 @@ BOOST_AUTO_TEST_CASE(QPBoundedSpeedTest) int bodyIndex = mb.bodyIndexByName(bodyName); sva::PTransformd bodyPoint(Vector3d(0., 0.1, 0.)); - qp::BoundedSpeedConstr constSpeed(mbs, 0, 0.005); + qp::BoundedCartesianMotionConstr constSpeed(mbs, 0, 0.005); qp::PostureTask postureTask(mbs, 0, {{}, {0.}, {0.}, {0.}}, 1., 0.01); qp::PositionTask posTask(mbs, 0, bodyName, Vector3d(1., -1., 1.), bodyPoint.translation()); qp::SetPointTask posTaskSp(mbs, 0, &posTask, 20., 1.); @@ -1380,7 +1380,7 @@ BOOST_AUTO_TEST_CASE(QPBoundedSpeedTest) // same test but with Z axis BOOST_CHECK(constSpeed.removeBoundedSpeed(bodyName)); - constSpeed.updateBoundedSpeeds(); + constSpeed.updateBoundedMotions(); BOOST_CHECK_EQUAL(constSpeed.nrBoundedSpeeds(), 0); BOOST_CHECK_EQUAL(constSpeed.maxGenInEq(), 0); @@ -1388,7 +1388,7 @@ BOOST_AUTO_TEST_CASE(QPBoundedSpeedTest) solver.updateConstrSize(); dof << 0.,0.,0.,0.,0.,1.; constSpeed.addBoundedSpeed(mbs, bodyName, bodyPoint.translation(), dof, speed); - constSpeed.updateBoundedSpeeds(); + constSpeed.updateBoundedMotions(); BOOST_CHECK_EQUAL(constSpeed.nrBoundedSpeeds(), 1); BOOST_CHECK_EQUAL(constSpeed.maxGenInEq(), 1); solver.updateConstrSize();