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 25102b5dc..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 BoundedCartesianMotionConstr::reset() +{ + resetBoundedSpeeds(); + resetBoundedAccelerations(); +} + + +std::size_t BoundedCartesianMotionConstr::nrBoundedSpeeds() const +{ + return velConstr_.size(); } -void BoundedSpeedConstr::resetBoundedSpeeds() +std::size_t BoundedCartesianMotionConstr::nrBoundedMotions() const { - cont_.clear(); + return velConstr_.size() + velConstr_.size(); } -std::size_t BoundedSpeedConstr::nrBoundedSpeeds() const +std::size_t BoundedCartesianMotionConstr::nrBoundedAccelerations() const { - return cont_.size(); + return velConstr_.size() + velConstr_.size(); } -void BoundedSpeedConstr::updateBoundedSpeeds() +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()); } @@ -1217,17 +1290,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 +1309,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 +1431,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 +1446,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 +1499,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 +1516,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..cc41df350 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 @@ -515,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 @@ -526,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: /** @@ -534,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); /** @@ -551,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); @@ -582,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, @@ -610,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) {} @@ -627,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_; @@ -656,7 +725,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: /** @@ -672,6 +741,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 +811,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 +819,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 +834,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/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/src/QPTasks.cpp b/src/QPTasks.cpp index 5a6cb0b73..64aa70ac7 100644 --- a/src/QPTasks.cpp +++ b/src/QPTasks.cpp @@ -1089,7 +1089,6 @@ GazeTask::GazeTask(const std::vector& mbs, robotIndex_(robotIndex) {} - int GazeTask::dim() { return 2; @@ -1141,7 +1140,6 @@ PositionBasedVisServoTask::PositionBasedVisServoTask(const 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 f375bc34c..90cbbaa9e 100644 --- a/src/Tasks.h +++ b/src/Tasks.h @@ -29,6 +29,8 @@ #include +#include + // forward declarations // RBDyn namespace rbd @@ -290,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, @@ -306,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_; @@ -332,6 +342,14 @@ class TASKS_DLLAPI PositionBasedVisServoTask 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, @@ -351,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_; 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) 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();