C++ (Cpp) RobotPtr::getManipulator 예제들

예제 #1

파일: 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

파일: test.cpp 프로젝트: ChefOtter/ahl_ros_pkg

int main(int argc, char** argv)
{
  ros::init(argc, argv, "lwr_server");
  ros::NodeHandle nh;

  ros::Timer timer_update_model = nh.createTimer(ros::Duration(0.01), updateModel);
  ros::Timer timer_control = nh.createTimer(ros::Duration(0.001), control);

  robot = RobotPtr(new Robot("lwr"));
  ParserPtr parser = ParserPtr(new Parser());

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

  controller = RobotControllerPtr(new RobotController());
  controller->init(robot);

  using namespace ahl_gazebo_if;
  gazebo_interface = GazeboInterfacePtr(new GazeboInterface());
  gazebo_interface->setDuration(0.010);
  gazebo_interface->addJoint("lwr::joint1");
  gazebo_interface->addJoint("lwr::joint2");
  gazebo_interface->addJoint("lwr::joint3");
  gazebo_interface->addJoint("lwr::joint4");
  gazebo_interface->addJoint("lwr::joint5");
  gazebo_interface->addJoint("lwr::joint6");
  gazebo_interface->addJoint("lwr::joint7");
  gazebo_interface->connect();

  tf_pub = TfPublisherPtr(new TfPublisher());

  ManipulatorPtr mnp = robot->getManipulator("mnp");

  gravity_compensation = TaskPtr(new GravityCompensation(robot));
  damping = TaskPtr(new Damping(mnp));
  joint_control = TaskPtr(new JointControl(mnp));
  joint_limit = TaskPtr(new JointLimit(mnp, 0.087));
  position_control = TaskPtr(new PositionControl(mnp, "gripper", 0.001));
  orientation_control = TaskPtr(new OrientationControl(mnp, "gripper", 0.001));

  controller->addTask(gravity_compensation, 0);
  controller->addTask(damping, 0);
  controller->addTask(joint_control, 0);
  controller->addTask(joint_limit, 100);

  ros::MultiThreadedSpinner spinner;
  spinner.spin();

  return 0;
}

예제 #3

파일: 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;
}