这这例程算是比较基础的例程,就是你给出机械臂的七个关节角,然后机械臂就会自动运行到那个关节角的姿态。
// 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 joint_point_to_point_motion.cpp
* An example that moves the robot to a target position by commanding joint positions.
*
* @warning Before executing this example, make sure there is enough space in front of the robot.
*/
// 给定七个关节角让机械臂运动到该姿态
// 输入的十个参数包括[代码文件路径][机械臂hostname][七个关节角][速度因子]
int main(int argc, char** argv) {
if (argc != 10) {
std::cerr << "Usage: " << argv[0] << " <robot-hostname> "
<< "<joint0> <joint1> <joint2> <joint3> <joint4> <joint5> <joint6> "
<< "<speed-factor>" << std::endl
<< "joint0 to joint6 are joint angles in [rad]." << std::endl
<< "speed-factor must be between zero and one." << std::endl;
return -1;
}
try {
franka::Robot robot(argv[1]); // 创建机器人实例
setDefaultBehavior(robot); // 设置机械臂的默认碰撞行为
std::array<double, 7> q_goal;
for (size_t i = 0; i < 7; i++) {
q_goal[i] = std::stod(argv[i + 2]);
}
double speed_factor = std::stod(argv[9]);
// Set additional parameters always before the control loop, NEVER in the control loop!
// 设置机械臂的默认碰撞行为
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}});
MotionGenerator motion_generator(speed_factor, q_goal); // 创建一个motion generator,参数包括速度因子,目标关节角
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); // 让机械臂根据motion generator运动
std::cout << "Motion finished" << std::endl;
} catch (const franka::Exception& e) {
std::cout << e.what() << std::endl; // 如果有异常则抛出异常
return -1;
}
return 0;
}