29 #ifndef HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH 30 #define HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH 36 #include <hpp/pinocchio/device.hh> 37 #include <hpp/pinocchio/liegroup-element.hh> 40 namespace constraints {
46 template <
typename Expression>
49 typedef shared_ptr<SymbolicFunction>
Ptr_t;
50 typedef weak_ptr<SymbolicFunction>
WkPtr_t;
52 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57 std::vector<bool> mask(expr->value().size(),
true);
58 return create(name, robot, expr, mask);
64 std::vector<bool> mask) {
65 assert(mask.size() == (std::size_t)expr->value().size());
75 std::vector<bool> mask(sizeOutput,
true);
77 sizeOutput, expr, mask));
86 std::vector<bool> mask)
92 size_type d = robot_->extraConfigSpace().dimension();
93 activeParameters_.tail(d).setConstant(
false);
94 activeDerivativeParameters_.tail(d).setConstant(
false);
100 std::vector<bool> mask)
115 robot_->currentConfiguration(argument);
116 robot_->computeForwardKinematics(pinocchio::JOINT_POSITION);
119 expr_->computeValue(argument);
121 for (std::size_t i = 0; i < mask_.size(); i++) {
122 if (mask_[i]) result.vector()[index++] = expr_->value()[i];
129 robot_->currentConfiguration(arg);
130 robot_->computeForwardKinematics(pinocchio::JOINT_POSITION |
131 pinocchio::JACOBIAN);
134 expr_->computeJacobian(arg);
137 const typename Expression::JacobianType_t& Je(expr_->jacobian());
140 size_type d = robot_->extraConfigSpace().dimension();
142 assert(robot_->numberDof() == Je.cols() +
d);
143 jacobian.rightCols(d).setZero();
145 nv = jacobian.cols();
148 for (std::size_t i = 0; i < mask_.size(); i++) {
149 if (mask_[i]) jacobian.row(index++).head(nv) = Je.row(i);
153 void init(
const Ptr_t&
self) { wkPtr_ =
self; }
160 if (robot_ != castother.robot_)
return false;
161 if (expr_ != castother.expr_)
return false;
162 if (mask_ != castother.mask_)
return false;
171 std::vector<bool> mask_;
175 #endif // HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH void init(const Ptr_t &self)
Definition: symbolic-function.hh:153
virtual ~SymbolicFunction()
Definition: symbolic-function.hh:82
virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const
Definition: symbolic-function.hh:126
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:97
const Derived & d
Definition: matrix-view-operation.hh:138
Definition: active-set-differentiable-function.hh:36
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:109
SymbolicFunction(const std::string &name, const DevicePtr_t &robot, const typename Traits< Expression >::Ptr_t expr, std::vector< bool > mask)
Definition: symbolic-function.hh:84
pinocchio::LiegroupSpace LiegroupSpace
Definition: fwd.hh:68
virtual bool isEqual(const DifferentiableFunction &other) const
Definition: differentiable-function.hh:206
shared_ptr< SymbolicFunction > Ptr_t
Definition: symbolic-function.hh:49
Definition: symbolic-function.hh:47
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
assert(d.lhs()._blocks()==d.rhs()._blocks())
bool isEqual(const DifferentiableFunction &other) const
Definition: symbolic-function.hh:155
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:55
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:62
Definition: differentiable-function.hh:63
virtual void impl_compute(LiegroupElementRef result, ConfigurationIn_t argument) const
Definition: symbolic-function.hh:112
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:88
HPP_CONSTRAINTS_CB_REF< Class > Ptr_t
Definition: symbolic-calculus.hh:127
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:66
pinocchio::size_type size_type
Definition: fwd.hh:47
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:58
weak_ptr< SymbolicFunction > WkPtr_t
Definition: symbolic-function.hh:50
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:72