예제 #1
0
파일: test4.cpp 프로젝트: bilynbk/ahl_wbc
int main(int argc, char** argv)
{
  ros::init(argc, argv, "parser_test");
  ros::NodeHandle nh;

  try
  {
    std::string name = "red_arm";
    RobotPtr robot = std::make_shared<Robot>(name);

    ParserPtr parser = std::make_shared<Parser>();
    std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/red_arm.yaml";
    parser->load(path, robot);

    ros::MultiThreadedSpinner spinner;

    TfPublisherPtr tf_publisher = std::make_shared<TfPublisher>();

    const std::string mnp_name = "mnp";
    unsigned long cnt = 0;
    ros::Rate r(10.0);

    while(ros::ok())
    {
      Eigen::VectorXd q = Eigen::VectorXd::Zero(robot->getDOF(mnp_name));
      ManipulatorPtr mnp = robot->getManipulator(mnp_name);

      double goal = sin(2.0 * M_PI * 0.2 * cnt * 0.1);
      ++cnt;
      for(uint32_t i = 0; i < q.rows(); ++i)
      {
        q.coeffRef(6) = M_PI / 4.0 * goal;
      }
      std::cout << M_PI / 4.0 * goal << std::endl;
      //q = Eigen::VectorXd::Constant(q.rows(), 0.0 * M_PI / 4.0);

      robot->update(q);
      robot->computeJacobian(mnp_name);
      robot->computeMassMatrix(mnp_name);
      M = robot->getMassMatrix(mnp_name);
      J = robot->getJacobian(mnp_name, "gripper");

      calc();
      tf_publisher->publish(robot, false);
      r.sleep();
    }
  }
  catch(ahl_utils::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }

  return 0;
}
예제 #2
0
void updateModel(const ros::TimerEvent&)
{
  try
  {
    boost::mutex::scoped_lock lock(mutex);
    if(joint_updated)
    {
      robot->computeBasicJacobian("mnp");
      robot->computeMassMatrix("mnp");
      controller->updateModel();
      updated = true;
      tf_pub->publish(robot);
    }
  }
  catch(ahl_ctrl::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
  catch(ahl_gazebo_if::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
}
예제 #3
0
파일: test6.cpp 프로젝트: skohlbr/ahl_wbc
int main(int argc, char** argv)
{
  ros::init(argc, argv, "parser_test");
  ros::NodeHandle nh;

  try
  {
    std::string name = "personal_robot";
    RobotPtr robot = RobotPtr(new Robot(name));

    ParserPtr parser = ParserPtr(new Parser());
    std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/pr2.yaml";
    parser->load(path, robot);

    ros::MultiThreadedSpinner spinner;

    TfPublisherPtr tf_publisher = TfPublisherPtr(new TfPublisher());

    const std::string mnp_name1 = "left_mnp";
    const std::string mnp_name2 = "right_mnp";
    unsigned long cnt = 0;
    ros::Rate r(10.0);

    while(ros::ok())
    {
      Eigen::VectorXd q = Eigen::VectorXd::Zero(robot->getDOF());
      ManipulatorPtr left_mnp = robot->getManipulator(mnp_name1);

      double goal = sin(2.0 * M_PI * 0.2 * cnt * 0.1);
      ++cnt;

      //std::cout << M_PI / 4.0 * goal << std::endl;
      q = Eigen::VectorXd::Constant(q.rows(), 0.0);

      q[10] = goal;

      robot->update(q);
      //robot->update(mnp_name2, q);

      robot->computeJacobian(mnp_name1);
      robot->computeJacobian(mnp_name2);
      robot->computeMassMatrix(mnp_name1);
      robot->computeMassMatrix(mnp_name2);

      M1 = robot->getMassMatrix(mnp_name1);
      M2 = robot->getMassMatrix(mnp_name2);
      J1 = robot->getJacobian(mnp_name1, "gripper_l_link");
      J2 = robot->getJacobian(mnp_name2, "gripper_r_link");

      calc();
      tf_publisher->publish(robot, false);
      r.sleep();
    }
  }
  catch(ahl_utils::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }

  return 0;
}
예제 #4
0
int main(int argc, char** argv)
{
  ros::init(argc, argv, "parser_test");
  ros::NodeHandle nh;

  try
  {
    std::string name = "youbot";
    RobotPtr robot = RobotPtr(new Robot(name));

    ParserPtr parser = ParserPtr(new Parser());
    std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/youbot.yaml";
    parser->load(path, robot);

    robot->getMobility()->print();

    ros::MultiThreadedSpinner spinner;

    TfPublisherPtr tf_publisher = TfPublisherPtr(new TfPublisher());

    const std::string mnp_name = "mnp";
    unsigned long cnt = 0;
    const double period = 0.001;
    ros::Rate r(1 / period);

    ahl_filter::DifferentiatorPtr differentiator = ahl_filter::DifferentiatorPtr(new ahl_filter::PseudoDifferentiator(period, 1.0));

    Eigen::VectorXd q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 0.0);
    Eigen::VectorXd dq = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 0.0);
    differentiator->init(q, dq);

    Eigen::VectorXd pre_q = q;

    //std::ofstream ofs1;
    //ofs1.open("result1.hpp");
    //std::ofstream ofs2;
    //ofs2.open("result2.hpp");
    //std::ofstream ofs3;
    //ofs3.open("result3.hpp");

    while(ros::ok())
    {
      q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 1.0);
      double coeff = 1.0 * sin(2.0 * M_PI * 0.1 * cnt * period);
      ++cnt;

      q = coeff * q;

      //q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), M_PI / 2.0);

      q.coeffRef(0) = 0.0;
      q.coeffRef(1) = 0.0;
      q.coeffRef(2) = 0.0;

      robot->update(mnp_name, q);
      robot->computeBasicJacobian(mnp_name);
      robot->computeMassMatrix(mnp_name);

      differentiator->apply(q);

      Eigen::VectorXd dq1 = robot->getJointVelocity(mnp_name);

      Eigen::VectorXd dq2;
      differentiator->copyDerivativeValueTo(dq2);

      std::cout << "p     : " << q.transpose() << std::endl;
      std::cout << "pre_p : " << pre_q.transpose() << std::endl;

      Eigen::VectorXd dq3 = (q - pre_q) / period;
      pre_q = q;

      //std::cout << "dq1 : " << dq1.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl;
      //std::cout << "dq2 : " << dq2.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl;
      //std::cout << "dq3 : " << dq3.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl;
      //std::cout << dq << std::endl << std::endl;
      //std::cout << cos(2.0 * M_PI * 0.1 * cnt * 0.1) << std::endl;

      tf_publisher->publish(robot, false);

/*
      ofs1 << cnt * period << " ";
      ofs2 << cnt * period << " ";
      ofs3 << cnt * period << " ";
      for(unsigned int i = 0; i < dq.rows() - 3; ++i)
      {
        ofs1 << dq1[i + 3] << " ";
        ofs2 << dq2[i + 3] << " ";
        ofs3 << dq3[i + 3] << " ";
      }
      ofs1 << std::endl;
      ofs2 << std::endl;
      ofs3 << std::endl;
*/
      r.sleep();
    }
  }
  catch(ahl_robot::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }

  return 0;
}