Robotis-Manipulator库

小芽科技 2022-04-26 16:06:15
Categories： Tags：
注：本文参考和翻译于robotis的官方文档：Robotis Manipulator Library. 翻译版权和额外提供内容隶属于北京小芽科技有限公司。分享请注明出处。

概述

robotis_manipulator是一个用于控制机械手的库，该库提供了一个用于设置机械手参数的类，并提供了一些数学函数用于配置机械手控制器，以及一个使用minimum-jerk算法的轨迹生成器。用户可以自行设计一个继承RobotisManipulator的类，并通过使用提供的函数和虚拟类来设置该类。该类提供的功能包括：创建轨迹，接收关节位置信息，发送关节目标位置信息。open_manipulator_libs 可以作为一个参考例程。

安装

robotis_manipulator软件包支持ROS和OpenCR控制板。用户可以通过在ROS环境下使用robotis_manipulator软件包编写自己的程序来控制用户的机械手。如果你想在一个更简单的开发环境中使用robotis_manipulator，你可以使用OpenCR，这是一个强大的嵌入式系统。用户可以在支持OpenCR的arduino IDE环境中轻松使用robotis_manipulator。

ROS 软件包

注 : 该软件包已经在安装有ROS Kinetic Kame 的 Ubuntu 16.04做过测试。

在PC上安装Ubuntu

下载Ubuntu 16.04并在PC上安装。

可以按照以下方式安装Ubuntu。

在PC上安装 ROS

img

下面的脚本将为你简化ROS的安装流程。在一个终端窗口中运行以下命令。终端程序可以通过屏幕左上角的Ubuntu搜索图标找到。打开终端的快捷键是Ctrl+Alt+t。安装完ROS后，请重新启动电脑。

1
2
3
$ sudo apt-get update
$ sudo apt-get upgrade
$ wget https://raw.githubusercontent.com/ROBOTIS-GIT/robotis_tools/master/install_ros_kinetic.sh && chmod 755 ./install_ros_kinetic.sh && bash ./install_ros_kinetic.sh

注意。为了检查哪些软件包已经安装，请查看这个链接。 install_ros_kinetic.sh

如果您喜欢手动安装，请点击下面的链接。

ROS 相关软件包

安装robotis_manipulator 软件包。 请在终端中运行以下命令：

1
2
3
$ cd ~/catkin_ws/src/
$ git clone https://github.com/ROBOTIS-GIT/robotis_manipulator.git
$ cd ~/catkin_ws && catkin_make

If the catkin_make command has been completed without any errors, all the preparations for using the robotis_manipulator package are done.

OpenCR控制器

连接

如下图，把OPENCR的USB口通过microUSB线连接到电脑PC上。

img

注：请参考OpenCR的详细说明。

Arduino IDE 设置

注：本页仅解释linux环境下OpenCR的设置过程。如果您想在windows或mac环境中使用OpenCR，请参考[Arduino IDE了解OpenCR的使用](https://emanual.robotis.com/docs/en/parts/controller/opencr10/#arduino-ide）。

安装Arduino IDE(Linux)

从Arduino官方主页下载最新版本的Arduino IDE，然后安装。目前，OpenCR将在1.6.4或更高版本中提供服务。

https://www.arduino.cc/en/Main/Software

然后，将下载的文件解压缩到所需文件夹，并从终端执行安装文件。在本例中，下面的示例将tools文件夹置于用户文件夹的顶部（~/）中。此文件夹将用作Arduino IDE文件夹。

1
2
$ cd ~/tools/arduino-1.6.4
$ ./install.sh

Set the file path of installed Arduino IDE as an absolute path named PATH in the bashrc file. Here recommends to use gedit editor. (Use another editor, if necessary.) Finally, source it to apply the changes.

将安装的Arduino IDE的文件路径设置为bashrc文件中名为PATH的绝对路径。这里推荐使用gedit编辑器。（如有必要，请使用其他编辑器。）最后，使用source指令接受环境变量的更改。 

1
2
3
$ gedit ~/.bashrc
$ export PATH=$PATH:$HOME/tools/arduino-1.6.4
$ source ~/.bashrc

运行 Arduino IDE(Linux)

在终端中输入以下指令运行Arduino IDE：

1
$ arduino

img

运行Arduino IDE后，单击菜单栏中的File→ Preferences。当“首选项”窗口出现时，将以下链接复制并粘贴到“Additional Boards Manager URLs”文本框。（此步骤可能需要大约20分钟。）

1
https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/arduino/opencr_release/package_opencr_index.json

img

通过Boards Manager安装OpenCR 相关库

Click Tools → Board → Boards Manager.

img

在编辑框中输入OpenCR以查找ROBOTIS提供的OpenCR包。发现后，单击安装。

img

安装完毕之后，将出现 “INSTALLED” 字样。

img

在Tools → Board中查看是否出现OpenCR Board。 单击该OpenCR Board导入相关资源。

img

端口设置

此步骤显示程序上载的端口设置。OpenCR应通过USB端口连接到PC和OpenCR。

Select Tools → Port → /dev/ttyACM0.

警告：根据USB连接环境的不同，字符串“/dev/ttyACM0”中的最后一个数字值“0”可能不同。

img

注：请参考 Arduino IDE for using OpenCR(linux) 以了解相关使用方法。

API 相关

点击这里打开 API 相关手册

使用教程

robotis_manipulator是一个库，它存储控制操纵器所需的参数，并进行运动学求解、轨迹生成和执行器通信。本教程介绍如何使用robotis_manipulator。在本教程中，我们将使用为控制OpenMANIPULATOR-X RM-X52-TNM而创建的open_manipulator_libs作为例子。

步骤1： 设计OpenManipulator机械手类

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
#define CUSTOM_TRAJECTORY_SIZE 4

class OpenManipulator : public robotis_manipulator::RobotisManipulator
{
  
private:
  robotis_manipulator::Kinematics *kinematics_;
  robotis_manipulator::JointActuator *actuator_;
  robotis_manipulator::ToolActuator *tool_;
  robotis_manipulator::CustomTaskTrajectory *custom_trajectory_[CUSTOM_TRAJECTORY_SIZE];

public:
  OpenManipulator();
  virtual ~OpenManipulator();

  void initOpenManipulator(bool using_actual_robot_state, STRING usb_port = "/dev/ttyUSB0", STRING baud_rate = "1000000", float control_loop_time = 0.010);
  void processOpenManipulator(double present_time);
};

创建一个机械手类，继承robotis_manipulator::RobotisManipulator类。在这个例子中，我们创建了initOpenManipulator和processOpenManipulator作为该类的公有函数。

步骤2：初始化机械手

InitOpenManipulator功能设置机械手的DH（Denavit Hartenberg）参数、运动学解算器、执行器和轨迹生成器。下面是OpenManipulator类的InitOpenManipulator函数。 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
void OpenManipulator::initOpenManipulator(bool using_actual_robot_state, STRING usb_port, STRING baud_rate, float control_loop_time)
{
/*****************************************************************************
** Initialize Manipulator Parameter
*****************************************************************************/
  addWorld("world",   // world name
          "joint1"); // child name

  addJoint("joint1",  // my name
          "world",   // parent name
          "joint2",  // child name
          math::vector3(0.012, 0.0, 0.017),                // relative position
          math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
          Z_AXIS,    // axis of rotation
          11,        // actuator id
          M_PI,      // max joint limit (3.14 rad)
          -M_PI,     // min joint limit (-3.14 rad)
          1.0,       // coefficient
          9.8406837e-02,                                                        // mass
          math::inertiaMatrix(3.4543422e-05, -1.6031095e-08, -3.8375155e-07,
                              3.2689329e-05, 2.8511935e-08,
                              1.8850320e-05),                                   // inertial tensor
          math::vector3(-3.0184870e-04, 5.4043684e-04, 0.018 + 2.9433464e-02)   // COM
          );

  addJoint("joint2",  // my name
          "joint1",  // parent name
          "joint3",  // child name
          math::vector3(0.0, 0.0, 0.0595),                // relative position
          math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
          Y_AXIS,    // axis of rotation
          12,        // actuator id
          M_PI_2,    // max joint limit (1.67 rad)
          -2.05,     // min joint limit (-2.05 rad)
          1.0,       // coefficient
          1.3850917e-01,                                                        // mass
          math::inertiaMatrix(3.3055381e-04, 9.7940978e-08, -3.8505711e-05,
                              3.4290447e-04, -1.5717516e-06,
                              6.0346498e-05),                                   // inertial tensor
          math::vector3(1.0308393e-02, 3.7743363e-04, 1.0170197e-01)            // COM
          );

  addJoint("joint3",  // my name
          "joint2",  // parent name
          "joint4",  // child name
          math::vector3(0.024, 0.0, 0.128),               // relative position
          math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
          Y_AXIS,    // axis of rotation
          13,        // actuator id
          1.53,      // max joint limit (1.53 rad)
          -M_PI_2,   // min joint limit (-1.67 rad)
          1.0,       // coefficient
          1.3274562e-01,                                                        // mass
          math::inertiaMatrix(3.0654178e-05, -1.2764155e-06, -2.6874417e-07,
                              2.4230292e-04, 1.1559550e-08,
                              2.5155057e-04),                                   // inertial tensor
          math::vector3(9.0909590e-02, 3.8929816e-04, 2.2413279e-04)            // COM
          );

  addJoint("joint4",  // my name
          "joint3",  // parent name
          "gripper", // child name
          math::vector3(0.124, 0.0, 0.0),                 // relative position
          math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
          Y_AXIS,    // axis of rotation
          14,        // actuator id
          2.0,       // max joint limit (2.0 rad)
          -1.8,      // min joint limit (-1.8 rad)
          1.0,       // coefficient
          1.4327573e-01,                                                        // mass
          math::inertiaMatrix(8.0870749e-05, 0.0, -1.0157896e-06,
                              7.5980465e-05, 0.0,
                              9.3127351e-05),                                   // inertial tensor
          math::vector3(4.4206755e-02, 3.6839985e-07, 8.9142216e-03)            // COM
          );

  addTool("gripper",  // my name
          "joint4",   // parent name
          math::vector3(0.126, 0.0, 0.0),                 // relative position
          math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
          15,         // actuator id
          0.010,      // max gripper limit (0.01 m)
          -0.010,     // min gripper limit (-0.01 m)
          -0.015,     // Change unit from `meter` to `radian`
          3.2218127e-02 * 2,                                                    // mass
          math::inertiaMatrix(9.5568826e-06, 2.8424644e-06, -3.2829197e-10,
                              2.2552871e-05, -3.1463634e-10,
                              1.7605306e-05),                                   // inertial tensor
          math::vector3(0.028 + 8.3720668e-03, 0.0246, -4.2836895e-07)          // COM
        );
          
/*****************************************************************************
** Initialize Kinematics 
*****************************************************************************/
  kinematics_ = new kinematics::SolverCustomizedforOMChain();
  addKinematics(kinematics_);

  if(using_actual_robot_state)
  {
    /*****************************************************************************
    ** Initialize Joint Actuator
    *****************************************************************************/
    actuator_ = new dynamixel::JointDynamixel();

    // Set communication arguments
    STRING dxl_comm_arg[2] = {usb_port, baud_rate};
    void *p_dxl_comm_arg = &dxl_comm_arg;

    // Set joint actuator id
    std::vector<uint8_t> jointDxlId;
    jointDxlId.push_back(11);
    jointDxlId.push_back(12);
    jointDxlId.push_back(13);
    jointDxlId.push_back(14);
    addJointActuator(JOINT_DYNAMIXEL, actuator_, jointDxlId, p_dxl_comm_arg);

    // Set joint actuator control mode
    STRING joint_dxl_mode_arg = "position_mode";
    void *p_joint_dxl_mode_arg = &joint_dxl_mode_arg;
    setJointActuatorMode(JOINT_DYNAMIXEL, jointDxlId, p_joint_dxl_mode_arg);


    /*****************************************************************************
    ** Initialize Tool Actuator
    *****************************************************************************/
    tool_ = new dynamixel::GripperDynamixel();

    uint8_t gripperDxlId = 15;
    addToolActuator(TOOL_DYNAMIXEL, tool_, gripperDxlId, p_dxl_comm_arg);

    // Set gripper actuator control mode
    STRING gripper_dxl_mode_arg = "current_based_position_mode";
    void *p_gripper_dxl_mode_arg = &gripper_dxl_mode_arg;
    setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_mode_arg);

    // Set gripper actuator parameter
    STRING gripper_dxl_opt_arg[2];
    void *p_gripper_dxl_opt_arg = &gripper_dxl_opt_arg;
    gripper_dxl_opt_arg[0] = "Profile_Acceleration";
    gripper_dxl_opt_arg[1] = "20";
    setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_opt_arg);

    gripper_dxl_opt_arg[0] = "Profile_Velocity";
    gripper_dxl_opt_arg[1] = "200";
    setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_opt_arg);

    // Enable All Actuators 
    enableAllActuator();

    // Receive current angles from all actuators 
    receiveAllJointActuatorValue();
    receiveAllToolActuatorValue();
  }

  /*****************************************************************************
  ** Initialize Custom Trajectory
  *****************************************************************************/
  custom_trajectory_[0] = new custom_trajectory::Line();
  custom_trajectory_[1] = new custom_trajectory::Circle();
  custom_trajectory_[2] = new custom_trajectory::Rhombus();
  custom_trajectory_[3] = new custom_trajectory::Heart();

  addCustomTrajectory(CUSTOM_TRAJECTORY_LINE, custom_trajectory_[0]);
  addCustomTrajectory(CUSTOM_TRAJECTORY_CIRCLE, custom_trajectory_[1]);
  addCustomTrajectory(CUSTOM_TRAJECTORY_RHOMBUS, custom_trajectory_[2]);
  addCustomTrajectory(CUSTOM_TRAJECTORY_HEART, custom_trajectory_[3]);
}

设置机器人参数

在robotis_manipulator中，机械手由world、joint components和tool components组成，如下图所示。world是指机械手固定的基地（基板）。joint components和tool components分别有一个框架和坐标系。在joint component中，坐标位于关节的旋转轴上，而在tool component中，坐标是在夹具的中心位置。

img

在InitOpenManipulator 函数中， 机械手的DH 参数设置如下所示：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
/*****************************************************************************
** Initialize Manipulator Parameter
*****************************************************************************/
addWorld("world",   // world name
        "joint1"); // child name

addJoint("joint1",  // my name
        "world",   // parent name
        "joint2",  // child name
        math::vector3(0.012, 0.0, 0.017),                // relative position
        math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
        Z_AXIS,    // axis of rotation
        11,        // actuator id
        M_PI,      // max joint limit (3.14 rad)
        -M_PI,     // min joint limit (-3.14 rad)
        1.0,       // coefficient
        9.8406837e-02,                                                        // mass
        math::inertiaMatrix(3.4543422e-05, -1.6031095e-08, -3.8375155e-07,
                            3.2689329e-05, 2.8511935e-08,
                            1.8850320e-05),                                   // inertial tensor
        math::vector3(-3.0184870e-04, 5.4043684e-04, 0.018 + 2.9433464e-02)   // COM
        );

addJoint("joint2",  // my name
        "joint1",  // parent name
        "joint3",  // child name
        math::vector3(0.0, 0.0, 0.0595),                // relative position
        math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
        Y_AXIS,    // axis of rotation
        12,        // actuator id
        M_PI_2,    // max joint limit (1.67 rad)
        -2.05,     // min joint limit (-2.05 rad)
        1.0,       // coefficient
        1.3850917e-01,                                                        // mass
        math::inertiaMatrix(3.3055381e-04, 9.7940978e-08, -3.8505711e-05,
                            3.4290447e-04, -1.5717516e-06,
                            6.0346498e-05),                                   // inertial tensor
        math::vector3(1.0308393e-02, 3.7743363e-04, 1.0170197e-01)            // COM
        );

addJoint("joint3",  // my name
        "joint2",  // parent name
        "joint4",  // child name
        math::vector3(0.024, 0.0, 0.128),               // relative position
        math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
        Y_AXIS,    // axis of rotation
        13,        // actuator id
        1.53,      // max joint limit (1.53 rad)
        -M_PI_2,   // min joint limit (-1.67 rad)
        1.0,       // coefficient
        1.3274562e-01,                                                        // mass
        math::inertiaMatrix(3.0654178e-05, -1.2764155e-06, -2.6874417e-07,
                            2.4230292e-04, 1.1559550e-08,
                            2.5155057e-04),                                   // inertial tensor
        math::vector3(9.0909590e-02, 3.8929816e-04, 2.2413279e-04)            // COM
        );

addJoint("joint4",  // my name
        "joint3",  // parent name
        "gripper", // child name
        math::vector3(0.124, 0.0, 0.0),                 // relative position
        math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
        Y_AXIS,    // axis of rotation
        14,        // actuator id
        2.0,       // max joint limit (2.0 rad)
        -1.8,      // min joint limit (-1.8 rad)
        1.0,       // coefficient
        1.4327573e-01,                                                        // mass
        math::inertiaMatrix(8.0870749e-05, 0.0, -1.0157896e-06,
                            7.5980465e-05, 0.0,
                            9.3127351e-05),                                   // inertial tensor
        math::vector3(4.4206755e-02, 3.6839985e-07, 8.9142216e-03)            // COM
        );

addTool("gripper",  // my name
        "joint4",   // parent name
        math::vector3(0.126, 0.0, 0.0),                 // relative position
        math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
        15,         // actuator id
        0.010,      // max gripper limit (0.01 m)
        -0.010,     // min gripper limit (-0.01 m)
        -0.015,     // Change unit from `meter` to `radian`
        3.2218127e-02 * 2,                                                    // mass
        math::inertiaMatrix(9.5568826e-06, 2.8424644e-06, -3.2829197e-10,
                            2.2552871e-05, -3.1463634e-10,
                            1.7605306e-05),                                   // inertial tensor
        math::vector3(0.028 + 8.3720668e-03, 0.0246, -4.2836895e-07)          // COM
      );

设置机械手的DH参数可以使用 addWorld, addJoint, addTool, 和 addComponentChild 函数。

addWorld 函数

1
2
3
4
void RobotisManipulator::addWorld(Name world_name,
                                  Name child_name,
                                  Eigen::Vector3d world_position = Eigen::Vector3d::Zero(),
                                  Eigen::Matrix3d world_orientation = Eigen::Matrix3d::Identity());

在固定的机械手中设置一个world 。这个world指的是固定机械手的一个基础坐标点，这个点可以根据固定的机械手状态而改变。一个机械手只有一个世界坐标点。

addJoint 函数

1
2
3
4
5
6
7
8
9
10
11
12
13
void RobotisManipulator::addJoint(Name my_name,
                                  Name parent_name,
                                  Name child_name,
                                  Eigen::Vector3d relative_position,
                                  Eigen::Matrix3d relative_orientation,
                                  Eigen::Vector3d axis_of_rotation = Eigen::Vector3d::Zero(),
                                  int8_t joint_actuator_id = -1, 
                                  double max_position_limit = M_PI, 
                                  double min_position_limit = -M_PI,
                                  double coefficient = 1.0,
                                  double mass = 0.0,
                                  Eigen::Matrix3d inertia_tensor = Eigen::Matrix3d::Identity(),
                                  Eigen::Vector3d center_of_mass = Eigen::Vector3d::Zero());

增加一个机械手的joint component。joint component是指所有带或不带执行器运行的关节组件。位于机械手末端的末端执行器被排除在joint component之外。joint component的位置和方向是指关节所在的旋转轴的坐标位置和方向。

addTool 函数

1
2
3
4
5
6
7
8
9
10
11
void RobotisManipulator::addTool(Name my_name,
                                 Name parent_name,
                                 Eigen::Vector3d relative_position,
                                 Eigen::Matrix3d relative_orientation,
                                 int8_t tool_id = -1, 
                                 double max_position_limit =M_PI, 
                                 double min_position_limit = -M_PI,
                                 double coefficient = 1.0,
                                 double mass = 0.0,
                                 Eigen::Matrix3d inertia_tensor = Eigen::Matrix3d::Identity(),
                                 Eigen::Vector3d center_of_mass = Eigen::Vector3d::Zero());

添加一个机械手的工具组件。这个工具被称为机械手的末端执行器，被安装在机械手的末端，也就是机械爪，它没有任何子组件。一个机械手可以被配置有多个工具组件。

addComponentChild 函数

1
void RobotisManipulator::addComponentChild(Name my_name, Name child_name);

在addJoint函数之后，使用addComponentChild函数添加该组件的子组件。当关节组件除了在addJoint函数中已经指定的child_name外，还有其他子组件时使用。工具组件没有子组件，因为它位于操纵器的末端。

添加运动学对象 (Kinematics Object)

在initOpenManipulator函数中，添加了用于控制机械手的*运动学求解算法(kinematics solving algorithm)*，如下所示。

1
2
3
4
5
/*****************************************************************************
** Initialize Kinematics 
*****************************************************************************/
kinematics_ = new kinematics::SolverCustomizedforOMChain();
addKinematics(kinematics_);

*运动学求解算法(kinematics solving algorism)*取决于用户的机械手。因此，robotis_manipulator的配置是为了让用户可以配置运动学求解器并测试用户的算法。当然，关于串联结构机械手的通用求解算法，open_manipulator_libs中提供了以下运动学求解器类。

如果用户想把他的算法应用于自己的机械手，用户可以创建一个新的运动学求解器类。这个新的类是通过继承下面的robotis_manipulator::kinematics类来创建的。

OpenManipulator-X机械手使用的kinematics::SolverCustomizedforOMChain类包括以下内容。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
/*****************************************************************************
** Kinematics Solver Customized for OpenManipulator Chain
*****************************************************************************/
class SolverCustomizedforOMChain : public robotis_manipulator::Kinematics
{
private:
  void forwardSolverUsingChainRule(Manipulator *manipulator, Name component_name);
  bool chainCustomInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector<JointValue>* goal_joint_value);

public:
  SolverCustomizedforOMChain(){}
  virtual ~SolverCustomizedforOMChain(){}

  virtual void setOption(const void *arg);
  virtual MatrixXd jacobian(Manipulator *manipulator, Name tool_name);
  virtual void solveForwardKinematics(Manipulator *manipulator);
  virtual bool solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector<JointValue>* goal_joint_value);
};

为了使用 “robotis_manipulator “的所有API函数，如上面的 “kinematics::SolverCustomizedforOMChain “类中所示，必须将以下每个函数声明为 “虚函数”。

每个虚函数的配置如下所示：

添加制动器（舵机）对象

In the initOpenManipulator function, the actuator classes to be used to control actuators of the manipulator is added as shown below. 在 initOpenManipulator 函数中，添加了用于控制机械手执行器的actuator类，如下所示。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
if(using_actual_robot_state)
{
  /*****************************************************************************
  ** Initialize Joint Actuator
  *****************************************************************************/
  actuator_ = new dynamixel::JointDynamixel();
  
  // Set communication arguments
  STRING dxl_comm_arg[2] = {usb_port, baud_rate};
  void *p_dxl_comm_arg = &dxl_comm_arg;

  // Set joint actuator id
  std::vector<uint8_t> jointDxlId;
  jointDxlId.push_back(11);
  jointDxlId.push_back(12);
  jointDxlId.push_back(13);
  jointDxlId.push_back(14);
  addJointActuator(JOINT_DYNAMIXEL, actuator_, jointDxlId, p_dxl_comm_arg);

  // Set joint actuator control mode
  STRING joint_dxl_mode_arg = "position_mode";
  void *p_joint_dxl_mode_arg = &joint_dxl_mode_arg;
  setJointActuatorMode(JOINT_DYNAMIXEL, jointDxlId, p_joint_dxl_mode_arg);


  /*****************************************************************************
  ** Initialize Tool Actuator
  *****************************************************************************/
  tool_ = new dynamixel::GripperDynamixel();

  uint8_t gripperDxlId = 15;
  addToolActuator(TOOL_DYNAMIXEL, tool_, gripperDxlId, p_dxl_comm_arg);

  // Set gripper actuator control mode
  STRING gripper_dxl_mode_arg = "current_based_position_mode";
  void *p_gripper_dxl_mode_arg = &gripper_dxl_mode_arg;
  setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_mode_arg);

  // Set gripper actuator parameter
  STRING gripper_dxl_opt_arg[2];
  void *p_gripper_dxl_opt_arg = &gripper_dxl_opt_arg;
  gripper_dxl_opt_arg[0] = "Profile_Acceleration";
  gripper_dxl_opt_arg[1] = "20";
  setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_opt_arg);

  gripper_dxl_opt_arg[0] = "Profile_Velocity";
  gripper_dxl_opt_arg[1] = "200";
  setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_opt_arg);

  // Enable All Actuators 
  enableAllActuator();

  // Receive current angles from all actuators 
  receiveAllJointActuatorValue();
  receiveAllToolActuatorValue();
}

使用addJointActuator函数添加关节的执行器类，使用addToolActuator函数添加工具的执行器类。使用setJointActuatorMode和setToolActuatorMode函数设置这两个类别的控制模式。JointDynamixel 和 GripperDynamixel 类可以与DYNAMIXEL一起使用，否则用户需要为自己的执行器创建 JointActuator和 ToolActuator当在一个操纵器中使用不同种类的执行器时，可以添加复数的JointActuator和ToolActuator类。

关节制动器类（Joint Actuator Class）

新创建的关节执行器（joint actuator）类是通过继承以下robotis_manipulator::JointActuator类来创建的。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
class JointActuator
{
public:
  bool enabled_state_;

  JointActuator() : enabled_state_(false) {}
  virtual ~JointActuator() {}

  virtual void init(std::vector<uint8_t> actuator_id, const void *arg) = 0;
  virtual void setMode(std::vector<uint8_t> actuator_id, const void *arg) = 0;
  virtual std::vector<uint8_t> getId() = 0;

  virtual void enable() = 0;
  virtual void disable() = 0;

  virtual bool sendJointActuatorValue(std::vector<uint8_t> actuator_id, std::vector<ActuatorValue> value_vector) = 0;
  virtual std::vector<ActuatorValue> receiveJointActuatorValue(std::vector<uint8_t> actuator_id) = 0;

  bool findId(uint8_t actuator_id);
  bool getEnabledState();
};

OpenManipulator 使用的 JointDynamixel 类的结构如下所示：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
class JointDynamixel : public robotis_manipulator::JointActuator
{
private:
  DynamixelWorkbench *dynamixel_workbench_;
  Joint dynamixel_;
  std::vector<AccelerationCalculator> acceleration_calculator_;
  std::vector<LowPassFilter> velocity_filter_;
  double previous_time_;

public:
  JointDynamixel();
  virtual ~JointDynamixel(){}


  /*****************************************************************************
  ** Joint Dynamixel Control Functions
  *****************************************************************************/
  virtual void init(std::vector<uint8_t> actuator_id, const void *arg);
  virtual void setMode(std::vector<uint8_t> actuator_id, const void *arg);
  virtual std::vector<uint8_t> getId();

  virtual void enable();
  virtual void disable();

  virtual bool sendJointActuatorValue(std::vector<uint8_t> actuator_id, std::vector<robotis_manipulator::ActuatorValue> value_vector);
  virtual std::vector<robotis_manipulator::ActuatorValue> receiveJointActuatorValue(std::vector<uint8_t> actuator_id);


  /*****************************************************************************
  ** Functions called in Joint Dynamixel Control Functions
  *****************************************************************************/
  bool initialize(std::vector<uint8_t> actuator_id, STRING dxl_device_name, STRING dxl_baud_rate);
  bool setOperatingMode(std::vector<uint8_t> actuator_id, STRING dynamixel_mode = "position_mode");
  bool setSDKHandler(uint8_t actuator_id);
  bool writeProfileValue(std::vector<uint8_t> actuator_id, STRING profile_mode, uint32_t value);
  bool writeGoalPosition(std::vector<uint8_t> actuator_id, std::vector<double> radian_vector);
  std::vector<robotis_manipulator::ActuatorValue> receiveAllDynamixelValue(std::vector<uint8_t> actuator_id);
};

为了使用robotis_manipulator的所有API函数，必须编写以下每个虚拟函数，如上面的JointDynamixel类。

每个虚函数的配置如下所示：

Tool Actuator 类

新创建的tool actuator 类是继承了 robotis_manipulator::ToolActuator 类而创建的.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
class ToolActuator
{
public:
  bool enabled_state_;

  ToolActuator():enabled_state_(false){}
  virtual ~ToolActuator() {}

  virtual void init(uint8_t actuator_id, const void *arg) = 0;
  virtual void setMode(const void *arg) = 0;
  virtual uint8_t getId() = 0;

  virtual void enable() = 0;
  virtual void disable() = 0;

  virtual bool sendToolActuatorValue(ActuatorValue value) = 0;
  virtual ActuatorValue receiveToolActuatorValue() = 0;

  bool findId(uint8_t actuator_id);
  bool getEnabledState();
};

OpenManipulator使用的GripperDynamixel类有以下结构。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
class GripperDynamixel : public robotis_manipulator::ToolActuator
{
 private:
  DynamixelWorkbench *dynamixel_workbench_;
  Joint dynamixel_;

 public:
  GripperDynamixel() {}
  virtual ~GripperDynamixel() {}


  /*****************************************************************************
  ** Tool Dynamixel Control Functions
  *****************************************************************************/
  virtual void init(uint8_t actuator_id, const void *arg);
  virtual void setMode(const void *arg);
  virtual uint8_t getId();

  virtual void enable();
  virtual void disable();

  virtual bool sendToolActuatorValue(robotis_manipulator::ActuatorValue value);
  virtual robotis_manipulator::ActuatorValue receiveToolActuatorValue();


  /*****************************************************************************
  ** Functions called in Tool Dynamixel Profile Control Functions
  *****************************************************************************/
  bool initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate);
  bool setOperatingMode(STRING dynamixel_mode = "position_mode");
  bool writeProfileValue(STRING profile_mode, uint32_t value);
  bool setSDKHandler();
  bool writeGoalPosition(double radian);
  double receiveDynamixelValue();
};

为了使用robotis_manipulator的所有API函数，必须编写以下每个虚拟函数，如上面GripperDynamixel类所示。

每个虚函数的配置如下所示：

添加自定义轨迹对象

Even if you do not add this trajectory generator class, you can create a basic trajectory (TCP, joint) using a minimum jerk, but you need to add a custom_trajectory class to make the manipulator move along your own trajectory. In the initOpenManipulator function, add custom_trajectory to be used to create the manipulator’s path (trajectory) as shown below.

即使你不添加这个轨迹生成器类，你也可以用minimum jerk创建一个基本的轨迹（TCP，joint），但是你需要添加一个custom_trajectory类来使机械手沿着你自己的轨迹移动。在initOpenManipulator函数中，添加custom_trajectory，用来创建操纵器的路径（轨迹），如下所示。

1
2
3
4
5
6
7
8
9
10
11
12
/*****************************************************************************
** Initialize Custom Trajectory
*****************************************************************************/
custom_trajectory_[0] = new custom_trajectory::Line();
custom_trajectory_[1] = new custom_trajectory::Circle();
custom_trajectory_[2] = new custom_trajectory::Rhombus();
custom_trajectory_[3] = new custom_trajectory::Heart();

addCustomTrajectory(CUSTOM_TRAJECTORY_LINE, custom_trajectory_[0]);
addCustomTrajectory(CUSTOM_TRAJECTORY_CIRCLE, custom_trajectory_[1]);
addCustomTrajectory(CUSTOM_TRAJECTORY_RHOMBUS, custom_trajectory_[2]);
addCustomTrajectory(CUSTOM_TRAJECTORY_HEART, custom_trajectory_[3]);

custom trajectory类是通过addCustomTrajectory函数添加的，它有CustomJointTrajectory类的类型，返回JointWaypoint作为路径值，CustomTaskTrajectory类返回TaskWaypoint作为路径值。这些类都是通过继承以下robotis_manipulator::CustomJointTrajectory和robotis_manipulator::CustomTaskTrajectory类创建的。

1
2
3
4
5
6
7
8
9
10
class CustomJointTrajectory
{
public:
  CustomJointTrajectory() {}
  virtual ~CustomJointTrajectory() {}

  virtual void makeJointTrajectory(double move_time, JointWaypoint start, const void *arg) = 0; 
  virtual void setOption(const void *arg) = 0;
  virtual JointWaypoint getJointWaypoint(double tick) = 0;
};
1
2
3
4
5
6
7
8
9
10
class CustomTaskTrajectory
{
public:
  CustomTaskTrajectory() {}
  virtual ~CustomTaskTrajectory() {}

  virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) = 0; 
  virtual void setOption(const void *arg) = 0;
  virtual TaskWaypoint getTaskWaypoint(double tick) = 0;
};

当你创建一个继承自上述类的类时，你必须编写以下虚函数来使用通过robotis_manipulator的API函数添加的轨迹。

每个虚函数的配置如下所示：

在OpenManipulator-X中使用的 “Circle “轨迹类的结构如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
/*****************************************************************************
** Circle
*****************************************************************************/
class Circle : public robotis_manipulator::CustomTaskTrajectory
{
private:
  robotis_manipulator::MinimumJerk path_generator_;
  VectorXd coefficient_;

  TaskWaypoint start_pose_;
  TaskWaypoint goal_pose_;

  double radius_;
  double start_angular_position_;
  double revolution_;

public:
	Circle() {}
	virtual ~Circle() {}

  void initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position);
  TaskWaypoint drawCircle(double time_var);

  virtual void setOption(const void *arg);
  virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg);
  virtual TaskWaypoint getTaskWaypoint(double tick);
};

步骤3：设计控制流程函数

使机械手成为控制过程的一个函数。虽然可以直接从控制器软件包中写入，但创建一个控制器软件包是很有用的，它可以通过预先填充主要使用的进程来控制多个相同的机械手。

在 OpenManipulator-X中， 控制流程的函数配置如下：

1
2
3
4
5
6
7
8
9
10
11
void OpenManipulator::processOpenManipulator(double present_time)
{
  JointWaypoint goal_joint_value = getJointGoalValueFromTrajectory(present_time);
  JointWaypoint goal_tool_value  = getToolGoalValue();

  receiveAllJointActuatorValue();
  receiveAllToolActuatorValue();
  if(goal_joint_value.size() != 0) sendAllJointActuatorValue(goal_joint_value);
  if(goal_tool_value.size() != 0) sendAllToolActuatorValue(goal_tool_value);
  solveForwardKinematics();
}
  1. goal_joint_value和goal_tool_value的值是通过getJointGoalValueFromTrajectory和getToolGoalValue从make trajectory函数计算的轨迹中返回。如果你使用了makeTaskTrajectory函数，逆运动学将在getJointGoalValueFromTrajectory函数中得到解决。
  2. 通过receiveAllJointActuatorValue和receiveAllToolActuatorValue返回present joint values和tool value。这些值存储在RobotisManipulator类的manipulator_成员变量中。
  3. 通过sendAllJointActuatorValue和sendAllToolActuatorValue函数，将从getJointGoalValueFromTrajectory和getToolGoalValue得到的goal_joint_value和goal_tool_value值发送给推杆控制器。这将使执行器移动到目标位置。
  4. Solve the forward kinematics based on the present joint values received through receiveAllJointActuatorValue, receiveAllToolActuatorValue and stored the pose value of components calculated to the manipulator_. 根据通过receiveAllJointActuatorValue、receiveAllToolActuatorValue收到的present joint values和存储在*manipulator_*计算出的组件的姿势值，解决正向运动学问题。

步骤4：如何使用

使用你上面创建的类和函数，为机械手配置控制器包。关于控制器上可用的API，见这里。例如，在OpenCR上运行的OpenManipulator-X的控制器包组成如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
#include <open_manipulator_libs.h>
#include "processing.h"
#include "remote_controller.h"

OpenManipulator open_manipulator;
double control_time = 0.010;
double present_time = 0.0;
double previous_time = 0.0;
bool platform_state = true;

void setup()
{
  Serial.begin(57600);
  DEBUG.begin(57600);
  // while (!Serial)
  // ;

  connectProcessing(platform_state);
  connectRC100();
  
  open_manipulator.initOpenManipulator(platform_state);
  log::println("OpenManipulator Debugging Port");
}

void loop()
{
  present_time = millis()/1000.0;
  getData(100);
  playProcessingMotion(&open_manipulator);

  if(present_time-previous_time >= control_time)
  {
    open_manipulator.processOpenManipulator(millis()/1000.0);
    previous_time = millis()/1000.0;
    sendValueToProcessing(&open_manipulator);
  }
}

void getData(uint32_t wait_time)
{
  static uint8_t state = 0;
  static uint32_t tick = 0;

  bool rc100_state = false;
  bool processing_state = false;

  uint16_t get_rc100_data = 0;
  String get_processing_data = "";

  if (availableRC100())
  {
    get_rc100_data = readRC100Data();
    rc100_state = true;
  }

  if (availableProcessing())
  {
    get_processing_data = readProcessingData();
    processing_state = true;
  }

  switch (state)
  {
    case 0:
      if (rc100_state)
      {
        fromRC100(&open_manipulator, get_rc100_data);
        tick = millis();
        state = 1;
      }
      else if (processing_state)
      {
        fromProcessing(&open_manipulator, get_processing_data);
        tick = millis();
        state = 1;
      }
     break;

    case 1:
      if ((millis() - tick) >= wait_time)
      {
        state = 0;
      }
     break;

    default:
      state = 0;
     break;
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
#include <open_manipulator_libs.h>

#define DXL_SIZE 5

typedef struct _MotionWaypoint
{
  std::vector<double> angle;
  double path_time;
  double gripper_value;
} MotionWaypoint;

std::vector<MotionWaypoint> motion_way_point_buf;
bool processing_motion_state = false;
char hand_motion_cnt = 0;
bool hand_motion_repeat_state = false;
bool platform_state_processing = false;
String global_cmd[50];

void connectProcessing(bool platform)
{ 
  platform_state_processing = platform;
  for (int i = 0; i < DXL_SIZE; i++)
  {
    Serial.print(0.0);
    Serial.print(",");
  }

  Serial.println(0.0);
  delay(300);

  Serial.println("Init Processing");
}

int availableProcessing()
{
  return Serial.available();
}

String readProcessingData()
{
  return Serial.readStringUntil('\n');
}

void split(String data, char separator, String* temp)
{
  int cnt = 0;
  int get_index = 0;

  String copy = data;
  
  while(true)
  {
    get_index = copy.indexOf(separator);

    if(-1 != get_index)
    {
      temp[cnt] = copy.substring(0, get_index);
      copy = copy.substring(get_index + 1);
    }
    else
    {
      temp[cnt] = copy.substring(0, copy.length());
      break;
    }
	  ++cnt;
  }
}

String* parseDataFromProcessing(String get)
{
  get.trim();
  split(get, ',', global_cmd);
  
  return global_cmd;
}

void sendAngleToProcessing(JointWaypoint joint_states_vector)
{
  Serial.print("angle");
  for (int i = 0; i < (int)joint_states_vector.size(); i++)
  {
    Serial.print(",");
    Serial.print(joint_states_vector.at(i).position, 3);
  }
  Serial.print("\n");
}

void sendToolDataToProcessing(ToolValue value)
{
  Serial.print("tool");
  Serial.print(",");
  Serial.print(value.position * 10);
  Serial.print("\n");
}

void sendValueToProcessing(OpenManipulator *open_manipulator)
{
  sendAngleToProcessing(open_manipulator->getAllActiveJointValue());
  sendToolDataToProcessing(open_manipulator->getToolValue("gripper"));
}

void fromProcessing(OpenManipulator *open_manipulator, String data)
{
  String *cmd = parseDataFromProcessing(data);

  if (cmd[0] == "opm")
  {
    if (cmd[1] == "ready")
    {
      if(platform_state_processing)
      {
        open_manipulator->enableAllActuator();
        sendValueToProcessing(open_manipulator);
      }
    }
    else if (cmd[1] == "end")
    {
      if(platform_state_processing)
      {
        open_manipulator->disableAllActuator();
      }
    }
  }
  ////////// joint space control tab
  else if (cmd[0] == "joint")
  {
    std::vector<double> goal_position;
    for (uint8_t index = 0; index < DXL_SIZE; index++)
    {
      goal_position.push_back((double)cmd[index + 1].toFloat());
    }
    open_manipulator->makeJointTrajectory(goal_position, 1.0); // FIX TIME PARAM
  }
  else if (cmd[0] == "gripper")
  {
    open_manipulator->makeToolTrajectory("gripper", (double)cmd[1].toFloat());
  }
  else if (cmd[0] == "grip")
  {
    if (cmd[1] == "on")
      open_manipulator->makeToolTrajectory("gripper", -0.009);
    else if (cmd[1] == "off")
      open_manipulator->makeToolTrajectory("gripper", 0.009);
  }
  ////////// task space control tab
  else if (cmd[0] == "task")
  {
    if (cmd[1] == "forward")
      open_manipulator->makeTaskTrajectoryFromPresentPose("gripper", math::vector3(0.010, 0.0, 0.0), 0.2);
    else if (cmd[1] == "back")
      open_manipulator->makeTaskTrajectoryFromPresentPose("gripper", math::vector3(-0.010, 0.0, 0.0), 0.2);
    else if (cmd[1] == "left")
      open_manipulator->makeTaskTrajectoryFromPresentPose("gripper", math::vector3(0.0, 0.010, 0.0), 0.2);
    else if (cmd[1] == "right")
      open_manipulator->makeTaskTrajectoryFromPresentPose("gripper", math::vector3(0.0, -0.010, 0.0), 0.2);
    else if (cmd[1] == "up")
      open_manipulator->makeTaskTrajectoryFromPresentPose("gripper", math::vector3(0.0, 0.0, 0.010), 0.2);
    else if (cmd[1] == "down")
      open_manipulator->makeTaskTrajectoryFromPresentPose("gripper", math::vector3(0.0, 0.0, -0.010), 0.2);
    else
      open_manipulator->makeTaskTrajectoryFromPresentPose("gripper", math::vector3(0.0, 0.0, 0.0), 0.2);
  }
  else if (cmd[0] == "torque")
  {
    if(platform_state_processing)
    {
      if (cmd[1] == "on")
        open_manipulator->enableAllJointActuator();
      else if (cmd[1] == "off")
        open_manipulator->disableAllJointActuator();
    }
  }
  ////////// hand teaching tab
  else if (cmd[0] == "get")
  {
    if (cmd[1] == "clear")  // motion clear
    {
      processing_motion_state = false;
      motion_way_point_buf.clear();
      hand_motion_cnt = 0;
    }
    else if (cmd[1] == "pose")  // save pose
    {
      MotionWaypoint read_value;
      JointWaypoint present_states = open_manipulator->getAllActiveJointValue();
      for(uint32_t i = 0; i < present_states.size(); i ++)
        read_value.angle.push_back(present_states.at(i).position);  
      read_value.path_time = 2.0; // FIX TIME PARAM
      read_value.gripper_value = open_manipulator->getToolValue("gripper").position;
      motion_way_point_buf.push_back(read_value);  
      hand_motion_cnt = 0;
    }
    else if (cmd[1] == "on")  // save gripper on
    {
      open_manipulator->makeToolTrajectory("gripper", -0.009);
    }
    else if (cmd[1] == "off")  // save gripper off
    {
      open_manipulator->makeToolTrajectory("gripper", 0.009);
    }
  }
  else if (cmd[0] == "hand")
  {
    if (cmd[1] == "once") // play motion (once)
    {
      processing_motion_state = true;
    }
    else if (cmd[1] == "repeat") // play motion (repeat)
    {
      hand_motion_repeat_state = true;
    }
    else if (cmd[1] == "stop") // play motion (stop)
    {
      hand_motion_repeat_state = false;
      processing_motion_state = false;
      hand_motion_cnt = 0;
    }
  }
  ////////// motion tab
  else if (cmd[0] == "motion")
  {
    if (cmd[1] == "1")
    {
      TaskWaypoint draw_line_arg;
      draw_line_arg.kinematic.position(0) = 0.02;
      draw_line_arg.kinematic.position(1) = 0.02;
      draw_line_arg.kinematic.position(2) = -0.02;
      void *p_draw_line_arg = &draw_line_arg;
      open_manipulator->makeCustomTrajectory(CUSTOM_TRAJECTORY_LINE, "gripper", p_draw_line_arg, 1.0);
    }
    else if (cmd[1] == "2")
    {
      double draw_circle_arg[3];
      draw_circle_arg[0] = 0.03; // radius (m)
      draw_circle_arg[1] = 2;    // revolution
      draw_circle_arg[2] = 0.0;  // start angle position (rad)
      void* p_draw_circle_arg = &draw_circle_arg;
      open_manipulator->makeCustomTrajectory(CUSTOM_TRAJECTORY_CIRCLE, "gripper", p_draw_circle_arg, 4.0);
    }
  }
}

void playProcessingMotion(OpenManipulator *open_manipulator)
{
  if(!open_manipulator->getMovingState() && processing_motion_state)
  {
    if(motion_way_point_buf.size() == 0)
      return;

    open_manipulator->makeToolTrajectory("gripper", motion_way_point_buf.at(hand_motion_cnt).gripper_value);
    open_manipulator->makeJointTrajectory(motion_way_point_buf.at(hand_motion_cnt).angle, motion_way_point_buf.at(hand_motion_cnt).path_time); 
    hand_motion_cnt ++;
    if(hand_motion_cnt >= motion_way_point_buf.size())
    {
      hand_motion_cnt = 0;
      if(!hand_motion_repeat_state)
        processing_motion_state = false;
    }
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
#include <open_manipulator_libs.h>
#include <RC100.h>

RC100 rc100;
double grip_value = 0.0;

void connectRC100()
{
  rc100.begin(1);
}

int availableRC100()
{
  return rc100.available();
}

uint16_t readRC100Data()
{
  return rc100.readData();
}

void fromRC100(OpenManipulator* open_manipulator, uint16_t data)
{
  if (data & RC100_BTN_U) 
    open_manipulator->makeTaskTrajectoryFromPresentPose("gripper", math::vector3(0.007, 0.0, 0.0), 0.16);
  else if (data & RC100_BTN_D)
    open_manipulator->makeTaskTrajectoryFromPresentPose("gripper", math::vector3(-0.007, 0.0, 0.0), 0.16);
  else if (data & RC100_BTN_L)
    open_manipulator->makeTaskTrajectoryFromPresentPose("gripper", math::vector3(0.0, 0.007, 0.0), 0.16);
  else if (data & RC100_BTN_R)
    open_manipulator->makeTaskTrajectoryFromPresentPose("gripper", math::vector3(0.0, -0.007, 0.0), 0.16);
  else if (data & RC100_BTN_1)
    open_manipulator->makeTaskTrajectoryFromPresentPose("gripper", math::vector3(0.0, 0.0, 0.007), 0.16);
  else if (data & RC100_BTN_3)
    open_manipulator->makeTaskTrajectoryFromPresentPose("gripper", math::vector3(0.0, 0.0, -0.007), 0.16);
  else if (data & RC100_BTN_2)
  {
    grip_value += 0.0020;
    if (grip_value > 0.01f)
      grip_value = 0.01f;

    open_manipulator->makeToolTrajectory("gripper", grip_value);
  }
  else if (data & RC100_BTN_4)
  {
    grip_value -= 0.002;
    if (grip_value < -0.01f)
      grip_value = -0.01f;

    open_manipulator->makeToolTrajectory("gripper", grip_value);
  }
  else if (data & RC100_BTN_5)
  {
    std::vector<double> goal_position;
    goal_position.push_back(0.0);
    goal_position.push_back(-60.0 * DEG2RAD);
    goal_position.push_back(20.0 * DEG2RAD);
    goal_position.push_back(40.0 * DEG2RAD);
    open_manipulator->makeJointTrajectory(goal_position, 1.5);
  }
  else if (data & RC100_BTN_6)
  {
    std::vector<double> goal_position;
    goal_position.push_back(0.0);
    goal_position.push_back(0.0);
    goal_position.push_back(0.0);
    goal_position.push_back(0.0);
    open_manipulator->makeJointTrajectory(goal_position, 1.0);
  }
}
01_1_TurtleBot3概述
Dynamixel舵机问题合集一