17 #ifndef HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH 18 # define HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH 20 # include <boost/assign/list_of.hpp> 27 # include <hpp/pinocchio/device.hh> 28 # include <hpp/pinocchio/liegroup-element.hh> 31 namespace constraints {
36 template <
typename Expression>
40 typedef boost::shared_ptr<SymbolicFunction>
Ptr_t;
41 typedef boost::weak_ptr<SymbolicFunction>
WkPtr_t;
43 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50 std::vector <bool> mask (expr->value ().size (),
true);
51 return create (name, robot, expr, mask);
58 std::vector <bool> mask)
60 assert (mask.size() == (std::size_t) expr->value().size());
72 std::vector<bool> mask (sizeOutput,
true);
73 Ptr_t ptr (
new SymbolicFunction (name,sizeInput,sizeInputDerivative,sizeOutput,expr,mask));
82 std::vector <bool> mask) :
86 robot_ (robot), expr_ (expr), mask_ (mask)
88 size_type d = robot_->extraConfigSpace().dimension();
89 activeParameters_ .tail(d).setConstant(
false);
90 activeDerivativeParameters_.tail(d).setConstant(
false);
97 std::vector <bool> mask) :
100 robot_ (), expr_ (expr), mask_ (mask) {}
111 robot_->currentConfiguration (argument);
112 robot_->computeForwardKinematics ();
114 expr_->invalidate ();
115 expr_->computeValue (argument);
117 for (std::size_t i = 0; i < mask_.size (); i++) {
119 result.vector () [index++] = expr_->value () [i];
127 robot_->currentConfiguration (arg);
128 robot_->computeForwardKinematics ();
130 expr_->invalidate ();
131 expr_->computeJacobian (arg);
134 const typename Expression::JacobianType_t& Je (expr_->jacobian());
137 size_type d = robot_->extraConfigSpace().dimension();
139 assert(robot_->numberDof() == Je.cols() +
d);
140 jacobian.rightCols(d).setZero();
142 nv = jacobian.cols();
145 for (std::size_t i = 0; i < mask_.size (); i++) {
147 jacobian.row(index++).head(nv) = Je.row (i);
151 void init (
const Ptr_t&
self) {
158 std::vector <bool> mask_;
162 #endif // HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH void init(const Ptr_t &self)
Definition: symbolic-function.hh:151
virtual ~SymbolicFunction()
Definition: symbolic-function.hh:78
virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const
Definition: symbolic-function.hh:123
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:93
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:80
pinocchio::LiegroupSpace LiegroupSpace
Definition: fwd.hh:56
Definition: symbolic-function.hh:37
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:46
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:55
Definition: differentiable-function.hh:52
boost::weak_ptr< SymbolicFunction > WkPtr_t
Definition: symbolic-function.hh:41
virtual void impl_compute(LiegroupElementRef result, ConfigurationIn_t argument) const
Definition: symbolic-function.hh:107
#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
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:67
boost::shared_ptr< SymbolicFunction > Ptr_t
Definition: symbolic-function.hh:40