当前位置:   article > 正文

在自己的工作空间中调用libfranka包运行Franka机械臂的代码_libfranka moveit

libfranka moveit

  在安装好libfranka和franka_ros包之后,可以直接运行libfranka工作空间中的devel->lib->功能包里面的可执行文件,就可以直接控制机械臂了。如果有还没有配置好libfranka和franka_ros的小伙伴,可以去查看Franka环境配置

  那么如何自己的工作空间下调用libfranka包成功编译自己的代码呢,以官方给出的例程为例,一步步展示,下面会有很多图,保证你学会。

目录

一.创建自己的工作空间

二.在c_cpp_properties.json文件中添加路径

三.编写CMakeList文件

四.编译文件

 五.执行文件


   我们使用的是官网例程generate_joint_position_motion.cpp,这个例程是设计机械臂的七个关节角控制机械臂的运动。显然在我们自己的工作空间中是运行不了的,因为我们缺少头文件,下面我们就演示一下怎么调用libfranka包里面的头文件来编译这个例程。

  1. // Copyright (c) 2017 Franka Emika GmbH
  2. // Use of this source code is governed by the Apache-2.0 license, see LICENSE
  3. #include <cmath>
  4. #include <iostream>
  5. #include <franka/exception.h>
  6. #include <franka/robot.h>
  7. #include "examples_common.h"
  8. /**
  9. * @example generate_joint_position_motion.cpp
  10. * An example showing how to generate a joint position motion.
  11. *
  12. * @warning Before executing this example, make sure there is enough space in front of the robot.
  13. */
  14. int main(int argc, char** argv) {
  15. if (argc != 2) {
  16. std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
  17. return -1;
  18. }
  19. try {
  20. franka::Robot robot(argv[1]);
  21. setDefaultBehavior(robot);
  22. // First move the robot to a suitable joint configuration
  23. std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
  24. MotionGenerator motion_generator(0.5, q_goal);
  25. std::cout << "WARNING: This example will move the robot! "
  26. << "Please make sure to have the user stop button at hand!" << std::endl
  27. << "Press Enter to continue..." << std::endl;
  28. std::cin.ignore();
  29. robot.control(motion_generator);
  30. std::cout << "Finished moving to initial joint configuration." << std::endl;
  31. // Set additional parameters always before the control loop, NEVER in the control loop!
  32. // Set collision behavior.
  33. robot.setCollisionBehavior(
  34. {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
  35. {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
  36. {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
  37. {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
  38. std::array<double, 7> initial_position;
  39. double time = 0.0;
  40. robot.control([&initial_position, &time](const franka::RobotState& robot_state,
  41. franka::Duration period) -> franka::JointPositions {
  42. time += period.toSec();
  43. if (time == 0.0) {
  44. initial_position = robot_state.q_d;
  45. }
  46. double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time));
  47. franka::JointPositions output = {{initial_position[0], initial_position[1],
  48. initial_position[2], initial_position[3] + delta_angle,
  49. initial_position[4] + delta_angle, initial_position[5],
  50. initial_position[6] + delta_angle}};
  51. if (time >= 5.0) {
  52. std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
  53. return franka::MotionFinished(output);
  54. }
  55. return output;
  56. });
  57. } catch (const franka::Exception& e) {
  58. std::cout << e.what() << std::endl;
  59. return -1;
  60. }
  61. return 0;
  62. }

一.创建自己的工作空间

在终端中输入

  1. mkdir -p position_generator/src
  2. cd position_generator/
  3. catkin_make
  4. code .

进入VScode之后,ctrl+shift+B打开小窗口,点击catkin_make:buil右边的小齿轮,把下面程序替换进{}tasks.json文件中:

  1. {
  2. // 有关 tasks.json 格式的文档,请参见
  3. // https://go.microsoft.com/fwlink/?LinkId=733558
  4. "version": "2.0.0",
  5. "tasks": [
  6. {
  7. "label": "catkin_make:debug", //代表提示的描述性信息
  8. "type": "shell", //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令,如果是process代表作为一个进程来运行
  9. "command": "catkin_make",//这个是我们需要运行的命令
  10. "args": [],//如果需要在命令后面加一些后缀,可以写在这里,比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
  11. "group": {"kind":"build","isDefault":true},
  12. "presentation": {
  13. "reveal": "always"//可选always或者silence,代表是否输出信息
  14. },
  15. "problemMatcher": "$msCompile"
  16. }
  17. ]
  18. }

配置好之后ctrl+shift+B会自动编译。然后新建功能包叫“generate_position”。在include目录下创建“common.h”头文件,在src目录下创建源文件“position.cpp”和“common.cpp”。其中position.cpp里面放主函数。这里操作不太会的小伙伴可以参考VScode 使用教程——ros下编译C/C++代码。建好的文件夹如下:

 编写头文件和源文件的代码,直接复制下面的代码即可(下面的三个代码分别来自于libfranka->examples文件夹里面的例程examples_common.h         examples_common.cpp        generate_joint_position_motion.cpp)

注意:

这里的代码和例程中的代码略微有些不同。

  • common.cpp和position.cpp中都将例程中的#include "examples_common.h"改成了#include "generate_position/common.h"

因为我们这里common.h头文件是在include/generate_position目录下的

  • “common.h”头文件代码

  1. // Copyright (c) 2017 Franka Emika GmbH
  2. // Use of this source code is governed by the Apache-2.0 license, see LICENSE
  3. #pragma once
  4. #include <array>
  5. #include <Eigen/Core>
  6. #include <franka/control_types.h>
  7. #include <franka/duration.h>
  8. #include <franka/robot.h>
  9. #include <franka/robot_state.h>
  10. /**
  11. * @file examples_common.h
  12. * Contains common types and functions for the examples.
  13. */
  14. /**
  15. * Sets a default collision behavior, joint impedance, Cartesian impedance, and filter frequency.
  16. *
  17. * @param[in] robot Robot instance to set behavior on.
  18. */
  19. void setDefaultBehavior(franka::Robot& robot);
  20. /**
  21. * An example showing how to generate a joint pose motion to a goal position. Adapted from:
  22. * Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots
  23. * (Kogan Page Science Paper edition).
  24. */
  25. class MotionGenerator {
  26. public:
  27. /**
  28. * Creates a new MotionGenerator instance for a target q.
  29. *
  30. * @param[in] speed_factor General speed factor in range [0, 1].
  31. * @param[in] q_goal Target joint positions.
  32. */
  33. MotionGenerator(double speed_factor, const std::array<double, 7> q_goal);
  34. /**
  35. * Sends joint position calculations
  36. *
  37. * @param[in] robot_state Current state of the robot.
  38. * @param[in] period Duration of execution.
  39. *
  40. * @return Joint positions for use inside a control loop.
  41. */
  42. franka::JointPositions operator()(const franka::RobotState& robot_state, franka::Duration period);
  43. private:
  44. using Vector7d = Eigen::Matrix<double, 7, 1, Eigen::ColMajor>;
  45. using Vector7i = Eigen::Matrix<int, 7, 1, Eigen::ColMajor>;
  46. bool calculateDesiredValues(double t, Vector7d* delta_q_d) const;
  47. void calculateSynchronizedValues();
  48. static constexpr double kDeltaQMotionFinished = 1e-6;
  49. const Vector7d q_goal_;
  50. Vector7d q_start_;
  51. Vector7d delta_q_;
  52. Vector7d dq_max_sync_;
  53. Vector7d t_1_sync_;
  54. Vector7d t_2_sync_;
  55. Vector7d t_f_sync_;
  56. Vector7d q_1_;
  57. double time_ = 0.0;
  58. Vector7d dq_max_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished();
  59. Vector7d ddq_max_start_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();
  60. Vector7d ddq_max_goal_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();
  61. };
  • “common.cpp”代码

  1. // Copyright (c) 2017 Franka Emika GmbH
  2. // Use of this source code is governed by the Apache-2.0 license, see LICENSE
  3. #include "generate_position/common.h"
  4. #include <algorithm>
  5. #include <array>
  6. #include <cmath>
  7. #include <franka/exception.h>
  8. #include <franka/robot.h>
  9. void setDefaultBehavior(franka::Robot& robot) {
  10. robot.setCollisionBehavior(
  11. {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
  12. {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
  13. {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
  14. {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});
  15. robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
  16. robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
  17. }
  18. MotionGenerator::MotionGenerator(double speed_factor, const std::array<double, 7> q_goal)
  19. : q_goal_(q_goal.data()) {
  20. dq_max_ *= speed_factor;
  21. ddq_max_start_ *= speed_factor;
  22. ddq_max_goal_ *= speed_factor;
  23. dq_max_sync_.setZero();
  24. q_start_.setZero();
  25. delta_q_.setZero();
  26. t_1_sync_.setZero();
  27. t_2_sync_.setZero();
  28. t_f_sync_.setZero();
  29. q_1_.setZero();
  30. }
  31. bool MotionGenerator::calculateDesiredValues(double t, Vector7d* delta_q_d) const {
  32. Vector7i sign_delta_q;
  33. sign_delta_q << delta_q_.cwiseSign().cast<int>();
  34. Vector7d t_d = t_2_sync_ - t_1_sync_;
  35. Vector7d delta_t_2_sync = t_f_sync_ - t_2_sync_;
  36. std::array<bool, 7> joint_motion_finished{};
  37. for (size_t i = 0; i < 7; i++) {
  38. if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) {
  39. (*delta_q_d)[i] = 0;
  40. joint_motion_finished[i] = true;
  41. } else {
  42. if (t < t_1_sync_[i]) {
  43. (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] *
  44. (0.5 * t - t_1_sync_[i]) * std::pow(t, 3.0);
  45. } else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) {
  46. (*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i];
  47. } else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) {
  48. (*delta_q_d)[i] =
  49. delta_q_[i] + 0.5 *
  50. (1.0 / std::pow(delta_t_2_sync[i], 3.0) *
  51. (t - t_1_sync_[i] - 2.0 * delta_t_2_sync[i] - t_d[i]) *
  52. std::pow((t - t_1_sync_[i] - t_d[i]), 3.0) +
  53. (2.0 * t - 2.0 * t_1_sync_[i] - delta_t_2_sync[i] - 2.0 * t_d[i])) *
  54. dq_max_sync_[i] * sign_delta_q[i];
  55. } else {
  56. (*delta_q_d)[i] = delta_q_[i];
  57. joint_motion_finished[i] = true;
  58. }
  59. }
  60. }
  61. return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(),
  62. [](bool x) { return x; });
  63. }
  64. void MotionGenerator::calculateSynchronizedValues() {
  65. Vector7d dq_max_reach(dq_max_);
  66. Vector7d t_f = Vector7d::Zero();
  67. Vector7d delta_t_2 = Vector7d::Zero();
  68. Vector7d t_1 = Vector7d::Zero();
  69. Vector7d delta_t_2_sync = Vector7d::Zero();
  70. Vector7i sign_delta_q;
  71. sign_delta_q << delta_q_.cwiseSign().cast<int>();
  72. for (size_t i = 0; i < 7; i++) {
  73. if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
  74. if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_start_[i]) +
  75. 3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_goal_[i]))) {
  76. dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] *
  77. (ddq_max_start_[i] * ddq_max_goal_[i]) /
  78. (ddq_max_start_[i] + ddq_max_goal_[i]));
  79. }
  80. t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i];
  81. delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i];
  82. t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i];
  83. }
  84. }
  85. double max_t_f = t_f.maxCoeff();
  86. for (size_t i = 0; i < 7; i++) {
  87. if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
  88. double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]);
  89. double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i];
  90. double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i];
  91. double delta = b * b - 4.0 * a * c;
  92. if (delta < 0.0) {
  93. delta = 0.0;
  94. }
  95. dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a);
  96. t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i];
  97. delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i];
  98. t_f_sync_[i] =
  99. (t_1_sync_)[i] / 2.0 + delta_t_2_sync[i] / 2.0 + std::abs(delta_q_[i] / dq_max_sync_[i]);
  100. t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i];
  101. q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]);
  102. }
  103. }
  104. }
  105. franka::JointPositions MotionGenerator::operator()(const franka::RobotState& robot_state,
  106. franka::Duration period) {
  107. time_ += period.toSec();
  108. if (time_ == 0.0) {
  109. q_start_ = Vector7d(robot_state.q_d.data());
  110. delta_q_ = q_goal_ - q_start_;
  111. calculateSynchronizedValues();
  112. }
  113. Vector7d delta_q_d;
  114. bool motion_finished = calculateDesiredValues(time_, &delta_q_d);
  115. std::array<double, 7> joint_positions;
  116. Eigen::VectorXd::Map(&joint_positions[0], 7) = (q_start_ + delta_q_d);
  117. franka::JointPositions output(joint_positions);
  118. output.motion_finished = motion_finished;
  119. return output;
  120. }
  • “position.cpp”代码
  1. // Copyright (c) 2017 Franka Emika GmbH
  2. // Use of this source code is governed by the Apache-2.0 license, see LICENSE
  3. #include <cmath>
  4. #include <iostream>
  5. #include <franka/exception.h>
  6. #include <franka/robot.h>
  7. #include "generate_position/common.h"
  8. /**
  9. * @example generate_joint_position_motion.cpp
  10. * An example showing how to generate a joint position motion.
  11. *
  12. * @warning Before executing this example, make sure there is enough space in front of the robot.
  13. */
  14. int main(int argc, char** argv) {
  15. if (argc != 2) {
  16. std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
  17. return -1;
  18. }
  19. try {
  20. franka::Robot robot(argv[1]);
  21. setDefaultBehavior(robot);
  22. // First move the robot to a suitable joint configuration
  23. std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
  24. MotionGenerator motion_generator(0.5, q_goal);
  25. std::cout << "WARNING: This example will move the robot! "
  26. << "Please make sure to have the user stop button at hand!" << std::endl
  27. << "Press Enter to continue..." << std::endl;
  28. std::cin.ignore();
  29. robot.control(motion_generator);
  30. std::cout << "Finished moving to initial joint configuration." << std::endl;
  31. // Set additional parameters always before the control loop, NEVER in the control loop!
  32. // Set collision behavior.
  33. robot.setCollisionBehavior(
  34. {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
  35. {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
  36. {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
  37. {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
  38. std::array<double, 7> initial_position;
  39. double time = 0.0;
  40. robot.control([&initial_position, &time](const franka::RobotState& robot_state,
  41. franka::Duration period) -> franka::JointPositions {
  42. time += period.toSec();
  43. if (time == 0.0) {
  44. initial_position = robot_state.q_d;
  45. }
  46. double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time));
  47. franka::JointPositions output = {{initial_position[0], initial_position[1],
  48. initial_position[2], initial_position[3] + delta_angle,
  49. initial_position[4] + delta_angle, initial_position[5],
  50. initial_position[6] + delta_angle}};
  51. if (time >= 5.0) {
  52. std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
  53. return franka::MotionFinished(output);
  54. }
  55. return output;
  56. });
  57. } catch (const franka::Exception& e) {
  58. std::cout << e.what() << std::endl;
  59. return -1;
  60. }
  61. return 0;
  62. }

二.在c_cpp_properties.json文件中添加路径

打开.vscod下面的c_cpp_properties.json文件

在“includePath”里面添加上libfranka包里面的头文件的路径,还有一个是自己编写的头文件路径。

(每个人的路径都不一样)

注意,每条路径最后有一个英文逗号,最后一个路径后面没有逗号。

 在这一块我们详细说一下路径的事

因为我们在代码用到了#include <franka/exception.h>  ,#include <franka/robot.h>这些头文件,那么这些头文件在哪里呢。我们vscode中打开libfranka,可以在include下看到我们引用的这些头文件。

那怎么快速获得include下头文件的路径呢,我们右键include,选择Open in Integrated Terminal,在打开的终端中输入

pwd

就可以得到路径了,然后稍微修改一下,在include后面加上/** ,表示包括include下面的所有文件。

 

 自己编写的头文件路径也可以同理获得。

 三.编写CMakeList文件

 CMakeList代码如下,我们解释一下每一部分分别是什么。

  1. cmake_minimum_required(VERSION 3.0.2)
  2. project(generate_position)
  3. find_package(catkin REQUIRED COMPONENTS
  4. roscpp
  5. rospy
  6. std_msgs
  7. )
  8. catkin_package(
  9. )
  10. include_directories(
  11. include
  12. ${catkin_INCLUDE_DIRS}
  13. )
  14. include_directories("/usr/include/eigen3") # Eigen3库的路径
  15. add_library(position_lib # 自己定义库的名字position_lib
  16. include/${PROJECT_NAME}/common.h # 调用的自定义头文件,${PROJECT_NAME}表示功能包名
  17. src/common.cpp # 调用的自定义源文件
  18. )
  19. add_executable(position src/position.cpp) # position是自己定义的映射节点名字,一般就是源文件名字
  20. # src/position.cpp就是main函数所在源文件路径
  21. find_package(Franka 0.7.0 REQUIRED) # 0.7.0改成自己的Franka机械臂版本
  22. # add_dependencies要在add_library后面,得先定义了position_lib才能添加依赖
  23. add_dependencies(position_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # 之前add_library里面自己定义的库名字add_library
  24. add_dependencies(position ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # add_executable中映射的节点名字position
  25. target_link_libraries(position_lib # 之前add_library里面自己定义的库名字add_library
  26. ${Franka_LIBRARIES} # 这里要添加${Franka_LIBRARIES}
  27. ${catkin_LIBRARIES}
  28. )
  29. target_link_libraries(position # position是自己定义的映射节点名字
  30. position_lib # 之前add_library里面自己定义的库名字add_library
  31. ${Franka_LIBRARIES}
  32. ${catkin_LIBRARIES}
  33. )

四.编译文件

启动roscore

roscore

 新开一个终端,进入工作空间,在终端输入:

catkin_make -DCMAKE_BUILD_TYPE=Release -DFranka_DIR:PATH=~/catkin_franka/libfranka/build

其中PATH=后面跟着的是你的libfranka包编译的路径 

这样就是编译成功了。

 五.执行文件

电脑连接机械臂,然后进入工作空间->devel->lib->功能包,就可以看到可执行文件了,在这里打开终端运行即可。

 如果编译出现问题,说fatal error: Eigen/Core: 没有那个文件或目录,那么就是Eigen3库的问题,可以参考fatal error: Eigen/Core: 没有那个文件或目录_啵啵鱼爱吃小猫咪的博客-CSDN博客

注意:有一个会报错但是不容易发现的问题,就是include/generate_position一定要保证在统一行,如果generate_position变成include的子目录,就会报错

声明:本文内容由网友自发贡献,转载请注明出处:【wpsshop】
推荐阅读
  

闽ICP备14008679号