17 #ifndef HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH 18 # define HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH 25 # include <hpp/pinocchio/device.hh> 26 # include <hpp/pinocchio/liegroup-element.hh> 29 namespace constraints {
34 template <
typename Expression>
38 typedef shared_ptr<SymbolicFunction>
Ptr_t;
39 typedef weak_ptr<SymbolicFunction>
WkPtr_t;
41 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48 std::vector <bool> mask (expr->value ().size (),
true);
49 return create (name, robot, expr, mask);
56 std::vector <bool> mask)
58 assert (mask.size() == (std::size_t) expr->value().size());
70 std::vector<bool> mask (sizeOutput,
true);
80 std::vector <bool> mask) :
84 robot_ (robot), expr_ (expr), mask_ (mask)
86 size_type d = robot_->extraConfigSpace().dimension();
87 activeParameters_ .tail(
d).setConstant(
false);
88 activeDerivativeParameters_.tail(
d).setConstant(
false);
95 std::vector <bool> mask) :
98 robot_ (), expr_ (expr), mask_ (mask) {}
109 robot_->currentConfiguration (argument);
110 robot_->computeForwardKinematics ();
112 expr_->invalidate ();
113 expr_->computeValue (argument);
115 for (std::size_t i = 0; i < mask_.size (); i++) {
117 result.vector () [index++] = expr_->value () [i];
125 robot_->currentConfiguration (arg);
126 robot_->computeForwardKinematics ();
128 expr_->invalidate ();
129 expr_->computeJacobian (arg);
132 const typename Expression::JacobianType_t& Je (expr_->jacobian());
135 size_type d = robot_->extraConfigSpace().dimension();
137 assert(robot_->numberDof() == Je.cols() +
d);
138 jacobian.rightCols(
d).setZero();
140 nv = jacobian.cols();
143 for (std::size_t i = 0; i < mask_.size (); i++) {
145 jacobian.row(index++).head(nv) = Je.row (i);
156 std::vector <bool> mask_;
160 #endif // HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH void init(const Ptr_t &self)
Definition: symbolic-function.hh:149
virtual ~SymbolicFunction()
Definition: symbolic-function.hh:76
virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const
Definition: symbolic-function.hh:121
SymbolicFunction(const std::string &name, size_type sizeInput, size_type sizeInputDerivative, size_type sizeOutput, const typename Traits< Expression >::Ptr_t expr, std::vector< bool > mask)
Definition: symbolic-function.hh:91
const Derived & d
Definition: matrix-view-operation.hh:126
Definition: active-set-differentiable-function.hh:24
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:97
SymbolicFunction(const std::string &name, const DevicePtr_t &robot, const typename Traits< Expression >::Ptr_t expr, std::vector< bool > mask)
Definition: symbolic-function.hh:78
pinocchio::LiegroupSpace LiegroupSpace
Definition: fwd.hh:56
shared_ptr< SymbolicFunction > Ptr_t
Definition: symbolic-function.hh:38
Definition: symbolic-function.hh:35
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
assert(d.lhs()._blocks()==d.rhs()._blocks())
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW Ptr_t create(const std::string &name, const DevicePtr_t &robot, const typename Traits< Expression >::Ptr_t expr)
Return a shared pointer to a new instance.
Definition: symbolic-function.hh:44
static Ptr_t create(const std::string &name, const DevicePtr_t &robot, const typename Traits< Expression >::Ptr_t expr, std::vector< bool > mask)
Return a shared pointer to a new instance.
Definition: symbolic-function.hh:53
Definition: differentiable-function.hh:52
virtual void impl_compute(LiegroupElementRef result, ConfigurationIn_t argument) const
Definition: symbolic-function.hh:105
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
HPP_CONSTRAINTS_CB_REF< Class > Ptr_t
Definition: symbolic-calculus.hh:107
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:54
pinocchio::size_type size_type
Definition: fwd.hh:36
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
weak_ptr< SymbolicFunction > WkPtr_t
Definition: symbolic-function.hh:39
static Ptr_t create(const std::string &name, size_type sizeInput, size_type sizeInputDerivative, size_type sizeOutput, const typename Traits< Expression >::Ptr_t expr)
Return a shared pointer to a new instance.
Definition: symbolic-function.hh:65