赞
踩
在安装好libfranka和franka_ros包之后,可以直接运行libfranka工作空间中的devel->lib->功能包里面的可执行文件,就可以直接控制机械臂了。如果有还没有配置好libfranka和franka_ros的小伙伴,可以去查看Franka环境配置
那么如何自己的工作空间下调用libfranka包成功编译自己的代码呢,以官方给出的例程为例,一步步展示,下面会有很多图,保证你学会。
我们使用的是官网例程generate_joint_position_motion.cpp,这个例程是设计机械臂的七个关节角控制机械臂的运动。显然在我们自己的工作空间中是运行不了的,因为我们缺少头文件,下面我们就演示一下怎么调用libfranka包里面的头文件来编译这个例程。
- // Copyright (c) 2017 Franka Emika GmbH
- // Use of this source code is governed by the Apache-2.0 license, see LICENSE
- #include <cmath>
- #include <iostream>
-
- #include <franka/exception.h>
- #include <franka/robot.h>
-
- #include "examples_common.h"
-
- /**
- * @example generate_joint_position_motion.cpp
- * An example showing how to generate a joint position motion.
- *
- * @warning Before executing this example, make sure there is enough space in front of the robot.
- */
-
- int main(int argc, char** argv) {
- if (argc != 2) {
- std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
- return -1;
- }
- try {
- franka::Robot robot(argv[1]);
- setDefaultBehavior(robot);
-
- // First move the robot to a suitable joint configuration
- std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
- MotionGenerator motion_generator(0.5, q_goal);
- std::cout << "WARNING: This example will move the robot! "
- << "Please make sure to have the user stop button at hand!" << std::endl
- << "Press Enter to continue..." << std::endl;
- std::cin.ignore();
- robot.control(motion_generator);
- std::cout << "Finished moving to initial joint configuration." << std::endl;
-
- // Set additional parameters always before the control loop, NEVER in the control loop!
- // Set collision behavior.
- robot.setCollisionBehavior(
- {{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}},
- {{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}},
- {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
- {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
-
- std::array<double, 7> initial_position;
- double time = 0.0;
- robot.control([&initial_position, &time](const franka::RobotState& robot_state,
- franka::Duration period) -> franka::JointPositions {
- time += period.toSec();
-
- if (time == 0.0) {
- initial_position = robot_state.q_d;
- }
-
- double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time));
-
- franka::JointPositions output = {{initial_position[0], initial_position[1],
- initial_position[2], initial_position[3] + delta_angle,
- initial_position[4] + delta_angle, initial_position[5],
- initial_position[6] + delta_angle}};
-
- if (time >= 5.0) {
- std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
- return franka::MotionFinished(output);
- }
- return output;
- });
- } catch (const franka::Exception& e) {
- std::cout << e.what() << std::endl;
- return -1;
- }
-
- return 0;
- }
在终端中输入
- mkdir -p position_generator/src
- cd position_generator/
- catkin_make
- code .
进入VScode之后,ctrl+shift+B打开小窗口,点击catkin_make:buil右边的小齿轮,把下面程序替换进{}tasks.json文件中:
- {
- // 有关 tasks.json 格式的文档,请参见
- // https://go.microsoft.com/fwlink/?LinkId=733558
- "version": "2.0.0",
- "tasks": [
- {
- "label": "catkin_make:debug", //代表提示的描述性信息
- "type": "shell", //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令,如果是process代表作为一个进程来运行
- "command": "catkin_make",//这个是我们需要运行的命令
- "args": [],//如果需要在命令后面加一些后缀,可以写在这里,比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
- "group": {"kind":"build","isDefault":true},
- "presentation": {
- "reveal": "always"//可选always或者silence,代表是否输出信息
- },
- "problemMatcher": "$msCompile"
- }
- ]
- }
配置好之后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目录下的
- // Copyright (c) 2017 Franka Emika GmbH
- // Use of this source code is governed by the Apache-2.0 license, see LICENSE
- #pragma once
-
- #include <array>
-
- #include <Eigen/Core>
-
- #include <franka/control_types.h>
- #include <franka/duration.h>
- #include <franka/robot.h>
- #include <franka/robot_state.h>
-
- /**
- * @file examples_common.h
- * Contains common types and functions for the examples.
- */
-
- /**
- * Sets a default collision behavior, joint impedance, Cartesian impedance, and filter frequency.
- *
- * @param[in] robot Robot instance to set behavior on.
- */
- void setDefaultBehavior(franka::Robot& robot);
-
- /**
- * An example showing how to generate a joint pose motion to a goal position. Adapted from:
- * Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots
- * (Kogan Page Science Paper edition).
- */
- class MotionGenerator {
- public:
- /**
- * Creates a new MotionGenerator instance for a target q.
- *
- * @param[in] speed_factor General speed factor in range [0, 1].
- * @param[in] q_goal Target joint positions.
- */
- MotionGenerator(double speed_factor, const std::array<double, 7> q_goal);
-
- /**
- * Sends joint position calculations
- *
- * @param[in] robot_state Current state of the robot.
- * @param[in] period Duration of execution.
- *
- * @return Joint positions for use inside a control loop.
- */
- franka::JointPositions operator()(const franka::RobotState& robot_state, franka::Duration period);
-
- private:
- using Vector7d = Eigen::Matrix<double, 7, 1, Eigen::ColMajor>;
- using Vector7i = Eigen::Matrix<int, 7, 1, Eigen::ColMajor>;
-
- bool calculateDesiredValues(double t, Vector7d* delta_q_d) const;
- void calculateSynchronizedValues();
-
- static constexpr double kDeltaQMotionFinished = 1e-6;
- const Vector7d q_goal_;
-
- Vector7d q_start_;
- Vector7d delta_q_;
-
- Vector7d dq_max_sync_;
- Vector7d t_1_sync_;
- Vector7d t_2_sync_;
- Vector7d t_f_sync_;
- Vector7d q_1_;
-
- double time_ = 0.0;
-
- Vector7d dq_max_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished();
- Vector7d ddq_max_start_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();
- Vector7d ddq_max_goal_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();
- };
- // Copyright (c) 2017 Franka Emika GmbH
- // Use of this source code is governed by the Apache-2.0 license, see LICENSE
- #include "generate_position/common.h"
-
- #include <algorithm>
- #include <array>
- #include <cmath>
-
- #include <franka/exception.h>
- #include <franka/robot.h>
-
- void setDefaultBehavior(franka::Robot& robot) {
- robot.setCollisionBehavior(
- {{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}},
- {{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}},
- {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.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}});
- robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
- robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
- }
-
- MotionGenerator::MotionGenerator(double speed_factor, const std::array<double, 7> q_goal)
- : q_goal_(q_goal.data()) {
- dq_max_ *= speed_factor;
- ddq_max_start_ *= speed_factor;
- ddq_max_goal_ *= speed_factor;
- dq_max_sync_.setZero();
- q_start_.setZero();
- delta_q_.setZero();
- t_1_sync_.setZero();
- t_2_sync_.setZero();
- t_f_sync_.setZero();
- q_1_.setZero();
- }
-
- bool MotionGenerator::calculateDesiredValues(double t, Vector7d* delta_q_d) const {
- Vector7i sign_delta_q;
- sign_delta_q << delta_q_.cwiseSign().cast<int>();
- Vector7d t_d = t_2_sync_ - t_1_sync_;
- Vector7d delta_t_2_sync = t_f_sync_ - t_2_sync_;
- std::array<bool, 7> joint_motion_finished{};
-
- for (size_t i = 0; i < 7; i++) {
- if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) {
- (*delta_q_d)[i] = 0;
- joint_motion_finished[i] = true;
- } else {
- if (t < t_1_sync_[i]) {
- (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] *
- (0.5 * t - t_1_sync_[i]) * std::pow(t, 3.0);
- } else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) {
- (*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i];
- } else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) {
- (*delta_q_d)[i] =
- delta_q_[i] + 0.5 *
- (1.0 / std::pow(delta_t_2_sync[i], 3.0) *
- (t - t_1_sync_[i] - 2.0 * delta_t_2_sync[i] - t_d[i]) *
- std::pow((t - t_1_sync_[i] - t_d[i]), 3.0) +
- (2.0 * t - 2.0 * t_1_sync_[i] - delta_t_2_sync[i] - 2.0 * t_d[i])) *
- dq_max_sync_[i] * sign_delta_q[i];
- } else {
- (*delta_q_d)[i] = delta_q_[i];
- joint_motion_finished[i] = true;
- }
- }
- }
- return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(),
- [](bool x) { return x; });
- }
-
- void MotionGenerator::calculateSynchronizedValues() {
- Vector7d dq_max_reach(dq_max_);
- Vector7d t_f = Vector7d::Zero();
- Vector7d delta_t_2 = Vector7d::Zero();
- Vector7d t_1 = Vector7d::Zero();
- Vector7d delta_t_2_sync = Vector7d::Zero();
- Vector7i sign_delta_q;
- sign_delta_q << delta_q_.cwiseSign().cast<int>();
-
- for (size_t i = 0; i < 7; i++) {
- if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
- if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_start_[i]) +
- 3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_goal_[i]))) {
- dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] *
- (ddq_max_start_[i] * ddq_max_goal_[i]) /
- (ddq_max_start_[i] + ddq_max_goal_[i]));
- }
- t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i];
- delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i];
- t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i];
- }
- }
- double max_t_f = t_f.maxCoeff();
- for (size_t i = 0; i < 7; i++) {
- if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
- double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]);
- double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i];
- double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i];
- double delta = b * b - 4.0 * a * c;
- if (delta < 0.0) {
- delta = 0.0;
- }
- dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a);
- t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i];
- delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i];
- t_f_sync_[i] =
- (t_1_sync_)[i] / 2.0 + delta_t_2_sync[i] / 2.0 + std::abs(delta_q_[i] / dq_max_sync_[i]);
- t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i];
- q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]);
- }
- }
- }
-
- franka::JointPositions MotionGenerator::operator()(const franka::RobotState& robot_state,
- franka::Duration period) {
- time_ += period.toSec();
-
- if (time_ == 0.0) {
- q_start_ = Vector7d(robot_state.q_d.data());
- delta_q_ = q_goal_ - q_start_;
- calculateSynchronizedValues();
- }
-
- Vector7d delta_q_d;
- bool motion_finished = calculateDesiredValues(time_, &delta_q_d);
-
- std::array<double, 7> joint_positions;
- Eigen::VectorXd::Map(&joint_positions[0], 7) = (q_start_ + delta_q_d);
- franka::JointPositions output(joint_positions);
- output.motion_finished = motion_finished;
- return output;
- }
- // Copyright (c) 2017 Franka Emika GmbH
- // Use of this source code is governed by the Apache-2.0 license, see LICENSE
- #include <cmath>
- #include <iostream>
-
- #include <franka/exception.h>
- #include <franka/robot.h>
-
- #include "generate_position/common.h"
-
- /**
- * @example generate_joint_position_motion.cpp
- * An example showing how to generate a joint position motion.
- *
- * @warning Before executing this example, make sure there is enough space in front of the robot.
- */
-
- int main(int argc, char** argv) {
- if (argc != 2) {
- std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
- return -1;
- }
- try {
- franka::Robot robot(argv[1]);
- setDefaultBehavior(robot);
-
- // First move the robot to a suitable joint configuration
- std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
- MotionGenerator motion_generator(0.5, q_goal);
- std::cout << "WARNING: This example will move the robot! "
- << "Please make sure to have the user stop button at hand!" << std::endl
- << "Press Enter to continue..." << std::endl;
- std::cin.ignore();
- robot.control(motion_generator);
- std::cout << "Finished moving to initial joint configuration." << std::endl;
-
- // Set additional parameters always before the control loop, NEVER in the control loop!
- // Set collision behavior.
- robot.setCollisionBehavior(
- {{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}},
- {{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}},
- {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
- {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
-
- std::array<double, 7> initial_position;
- double time = 0.0;
- robot.control([&initial_position, &time](const franka::RobotState& robot_state,
- franka::Duration period) -> franka::JointPositions {
- time += period.toSec();
-
- if (time == 0.0) {
- initial_position = robot_state.q_d;
- }
-
- double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time));
-
- franka::JointPositions output = {{initial_position[0], initial_position[1],
- initial_position[2], initial_position[3] + delta_angle,
- initial_position[4] + delta_angle, initial_position[5],
- initial_position[6] + delta_angle}};
-
- if (time >= 5.0) {
- std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
- return franka::MotionFinished(output);
- }
- return output;
- });
- } catch (const franka::Exception& e) {
- std::cout << e.what() << std::endl;
- return -1;
- }
-
- return 0;
- }
打开.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代码如下,我们解释一下每一部分分别是什么。
- cmake_minimum_required(VERSION 3.0.2)
- project(generate_position)
-
-
- find_package(catkin REQUIRED COMPONENTS
- roscpp
- rospy
- std_msgs
- )
-
-
- catkin_package(
-
- )
-
-
- include_directories(
- include
- ${catkin_INCLUDE_DIRS}
- )
- include_directories("/usr/include/eigen3") # Eigen3库的路径
-
-
- add_library(position_lib # 自己定义库的名字position_lib
- include/${PROJECT_NAME}/common.h # 调用的自定义头文件,${PROJECT_NAME}表示功能包名
- src/common.cpp # 调用的自定义源文件
- )
-
- add_executable(position src/position.cpp) # position是自己定义的映射节点名字,一般就是源文件名字
- # src/position.cpp就是main函数所在源文件路径
- find_package(Franka 0.7.0 REQUIRED) # 0.7.0改成自己的Franka机械臂版本
-
- # add_dependencies要在add_library后面,得先定义了position_lib才能添加依赖
- add_dependencies(position_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # 之前add_library里面自己定义的库名字add_library
- add_dependencies(position ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # add_executable中映射的节点名字position
-
-
- target_link_libraries(position_lib # 之前add_library里面自己定义的库名字add_library
- ${Franka_LIBRARIES} # 这里要添加${Franka_LIBRARIES}
- ${catkin_LIBRARIES}
- )
-
- target_link_libraries(position # position是自己定义的映射节点名字
- position_lib # 之前add_library里面自己定义的库名字add_library
- ${Franka_LIBRARIES}
- ${catkin_LIBRARIES}
- )
-
启动roscore
roscore
新开一个终端,进入工作空间,在终端输入:
catkin_make -DCMAKE_BUILD_TYPE=Release -DFranka_DIR:PATH=~/catkin_franka/libfranka/build
这样就是编译成功了。
电脑连接机械臂,然后进入工作空间->devel->lib->功能包,就可以看到可执行文件了,在这里打开终端运行即可。
如果编译出现问题,说fatal error: Eigen/Core: 没有那个文件或目录,那么就是Eigen3库的问题,可以参考fatal error: Eigen/Core: 没有那个文件或目录_啵啵鱼爱吃小猫咪的博客-CSDN博客
注意:有一个会报错但是不容易发现的问题,就是include/generate_position一定要保证在统一行,如果generate_position变成include的子目录,就会报错
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。