JointsSelector.h

Go to the documentation of this file.

 
#pragma once
 
#include <tvm/defs.h>
 
#include <tvm/Robot.h>
#include <tvm/function/abstract/Function.h>
 
namespace tvm
{
 
namespace robot
{
 
class TVM_DLLAPI JointsSelector : public function::abstract::Function
{
public:
  SET_UPDATES(JointsSelector, Jacobian, JDot)
 
  
  static std::unique_ptr<JointsSelector> ActiveJoints(FunctionPtr f,
                                                      RobotPtr robot,
                                                      const std::vector<std::string> & activeJoints);
 
  static std::unique_ptr<JointsSelector> InactiveJoints(FunctionPtr f,
                                                        RobotPtr robot,
                                                        const std::vector<std::string> & inactiveJoints);
 
  const Eigen::VectorXd & value() const override { return f_->value(); }
  const Eigen::VectorXd & velocity() const override { return f_->velocity(); }
  const Eigen::VectorXd & normalAcceleration() const override { return f_->normalAcceleration(); }
 
protected:
  JointsSelector(FunctionPtr f,
                 RobotPtr robot,
                 bool ffActive,
                 const std::vector<std::pair<Eigen::DenseIndex, Eigen::DenseIndex>> & activeIndex);
 
protected:
  void updateJacobian();
  void updateJDot();
 
  FunctionPtr f_;
  RobotPtr robot_;
  bool ffActive_;
  std::vector<std::pair<Eigen::DenseIndex, Eigen::DenseIndex>> activeIndex_;
};
 
} // namespace robot
 
} // namespace tvm