FrictionCompensation::FrictionCompensation(const ahl_robot::RobotPtr& robot)
{
  robot_ = robot;
  mnp_name_ = robot_->getManipulatorName();
  N_ = Eigen::MatrixXd::Identity(robot->getDOF(), robot->getDOF());
  b_ = Eigen::VectorXd::Zero(robot->getDOF()).asDiagonal();
}
GravityCompensation::GravityCompensation(const ahl_robot::RobotPtr& robot)
{
  robot_ = robot;
  mnp_name_ = robot_->getManipulatorName();
  N_ = Eigen::MatrixXd::Identity(robot->getDOF(), robot->getDOF());
}