所用测试工作空间test_ws：包含官网最原始的功能包
一、使用Kinova官方Development center控制真实机械臂
0.在ubuntu系统安装Kinova机械臂的Development center，这一步自行安装，很简单。
1.使用USB连接机械臂和电脑
2.Development center路径：文件夹Other Locations—Computer—opt—JACO-SDK—GUI—Development center(双击打开即可)
3.我们实验室自己搭建的双臂kinova机器人，通过Virtual Joystick窗口控制各个关节角度或直接控制笛卡尔空间下的位置和姿态。如果我们的kinova机械臂运动起来像乌龟一样慢，在这里还可以调节机械臂的移动速度。
left-arm左臂序列号：PJ00650019162710001
right-arm右臂序列号：PJ00650019003483-0
4.使用Development center控制真实机械臂的优势
之前我在做机械臂和相机的手眼标定时，需要调整末端执行器夹取标定版的位姿，以获取相机和机械臂的标定矩阵。用过kinova机械臂的都知道，用他的Joystick来控制机械臂到达目标位置是非常困难的，所以这里使用Development center控制机械臂笛卡尔空间的位置，简单方便到达目标位置。
二、使用Kinova官方kinova-ros包控制一个真实机械臂之关节空间控制
1.下载kinova-ros功能包到工作空间的src文件夹
这里注意版本，我的系统是ubuntu18.04+ROS melodic，所下载kinova-ros-melodic-devel
地址为：https://github.com/Kinovarobotics/kinova-ros/tree/melodic-devel
该网页包括一些基础的使用说明，如：
a.要通过usb访问arm，请将udev规则文件10-kinova-arm.rules从~/catkin_ws/src/kinova-ros/kinova_driver/udev复制到/etc/udev/rules.d/：
sudo cp kinova_driver/udev/10-kinova-arm.rules /etc/udev/rules.d/
b. kinova_control文件夹是Gazebo使用的文件
2.关节空间下控制kinova真实机械臂方法一：命令行方式
a.启动launch文件
roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=m1n6s300 kinova_robotSerial:=PJ00650019003483-0
b.打开rviz可视化机械臂
rosrun rviz rviz
Fixed Frame:选root
Add：RobotModel
注意：这里如果满屏幕报错[rospack] Error: package 'kinova_description' not found [librospack]: error while executing command [ERROR] [1693814462.315590353]: Could not load resource [package://kinova_description/meshes/shoulder.dae]: Unable to open file "package://kinova_description/meshes/shoulder.dae".
原因：没有sorce工作空间
总之，如果出现[rospack] Error: package 'kinova_description' not found [librospack]: error while executing command ，也就是找不到包。回到工作空间source一下就可
source devel/setup.bash
c. 通过关节控制机械臂：Joint position control
例如:
rosrun kinova_demo joints_action_client.py -v -r m1n6s300 degree -- 0 0 0 0 0 10
这里也是，如果报错找不到kinova_demo的package，原因是没有source工作空间。
3.控制kinova真实机械臂方法二：rviz交互方式
控制关节位置的另一种方法是在Rviz中使用交互式Marker。可按照以下步骤进行交互式控制：
#1. launch the drivers:
roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=m1n6s300 kinova_robotSerial:=PJ00650019003483-0
#2. start the node of interactive conrol:
rosrun kinova_driver kinova_interactive_control m1n6s300
#3. open Rviz:
rosrun rviz rviz
#4. in RViz (in the display section) change Global Options -> Fixed Frame to world
#5. add robot's model with Add -> RobotModel (in rviz folder)
#6. add interactive markers with Add -> InteractiveMarkers (in rviz folder)
#7. change InteractiveMarkers -> Updated Topic to /m1n6s300_interactive_control_Joint/update
#8. And then, A ring should appear around each joint, you can move the robot by movings those rings.
三、使用Kinova官方kinova-ros包控制一个真实机械臂之笛卡尔位置控制
1.笛卡尔空间下使用终端指令控制Kinova机械臂
a. 启动launch文件
roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=m1n6s300 kinova_robotSerial:=PJ00650019003483-0
rosrun kinova_demo pose_action_client.py -v -r j2n6s300 mdeg -- 0.01 0 0 0 0 10
解释： pose_action_client.py函数共有三个参数
参数一：kinova_robotType (eg. m1n6s300)
参数二：unit {mq | mdeg | mrad} (单位参数，分别指的是米和四元数、米和度以及米和弧度)
参数三：pose_value：（坐标为x、y、z），然后是方向（基于不同单位可能是3个值，也可能是4个值）
该函数使用选项-r来告诉机器人角度值是相对的还是绝对的。它还有选项-v用于更详细的输出。
上述指令表示：驱动mico机器人沿+x轴移动1cm，并沿手轴旋转+10度。
重要重要重要：机器人root frame的笛卡尔坐标系由以下规则定义：
原点：原点是底部平面和圆柱体中心线的交点。
+x方向：当面对基板（电源开关和电缆插座所在的位置）时，+x轴指向左侧。
+y方向：当面对基板时，+y轴朝向用户。
+z方向：当机器人站在平面上时，+z轴向上。
2.笛卡尔空间下使用rviz交互方式控制Kinova机械臂
同样，笛卡尔空间下控制机械臂末端运动的另一种方法是在Rviz中使用交互式Marker。可按照以下步骤进行交互式控制：
#1. launch the drivers:
roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=m1n6s300 kinova_robotSerial:=PJ00650019003483-0
#2. start the node of interactive conrol:
rosrun kinova_driver kinova_interactive_control m1n6s300
#3. open Rviz:
rosrun rviz rviz
#4. in RViz (in the display section) change Global Options -> Fixed Frame to world
#5. add robot's model with Add -> RobotModel (in rviz folder)
#6. add interactive markers with Add -> InteractiveMarkers (in rviz folder)
#7. change InteractiveMarkers -> Updated Topic to /m1n6s300_interactive_control_Cart/update
#8. And then, a cubic with 3 axis (translation) and 3 rings(rotation) should appear at the end-effector, you can move the robot by dragging the axis or rings.
3.笛卡尔空间下，在不停止运行的情况下，平滑到达多个路径点：
不停地执行多个笛卡尔航路点：动作客户端一次执行一个目标。如果用户想在不停在每个航路点的情况下为机器人提供多个航路点，可以使用AddPoseToCartesianTrajectories服务。该服务将命令的位姿添加到由机器人维护的缓冲区中。机器人按照添加的顺序执行该缓冲区中的位姿，而不会在位姿之间停止。
服务ClearTrajections可用于清除轨迹缓冲区。
四、kinova机械臂手指控制
a. 启动launch文件
roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=m1n6s300 kinova_robotSerial:=PJ00650019003483-0
b.启动手指控制指令
rosrun kinova_demo fingers_action_client.py m1n6s300 percent -- 100 100 100
解释：fingers_action_client.py函数有三个参数：
第一个参数：kinova_robotType（例如m1n6s300）
第二个参数：unit｛turn|mm|percent｝
第三个参数：finger_value。
重点说明1：当第二个单位参数为percent时，代表手指张开和闭合的百分比，很好理解，当为100时，则表示手指完全闭合，如上面的控制指令；当为50时，则手指处于张开一半状态；当为0是，则表示手指完全张开；
重点说明2：当第二个单位参数为turn时，代表手指张开的程度，取值区间为0-6800，0表示完全打开，6800表示完全关闭，一般用turn作为单位，如下参考指令：
rosrun kinova_demo fingers_action_client.py m1n6s300 turn -- 0 0 0
五、笛卡尔导纳模式（力控/拖动示教）
该模式下，我们可以手动（手动）控制机器人。导纳力控制可以通过以下命令启用和停用：
rosservice call /'m1n6s300_driver'/in/start_force_control
rosservice call /'m1n6s300_driver'/in/stop_force_control
用户可以通过向末端执行器/关节施加力/扭矩来移动机器人。当存在笛卡尔/关节位置命令时，结果运动将是力和位置命令的组合。
六、7自由度机械臂零空间控制（鸡头模式：末端执行器位姿不变，机械臂可正常运动）
kinova有7自由度冗余机械臂，虽然我们实验室没有，但还是记录一下。该模式下，末端执行器位姿不变，机械臂可正常运动
${kinova_robotType}_driver /in/set_null_space_mode_state
七、力矩控制：暂时用不到，省略
八、以太网连接
考虑到使用USB连接机械臂时，笔记本电脑和机械臂距离不能太远（USB线长有限），限制了机械臂的使用，所以考虑使用以太网连接连接，官网给出步骤如下：就不翻译了，很容易看懂。
Setup a static IP address for your ethernet network say - 192.168.100.100
With the robot connected to your PC via USB open kinova's Develepment Center
Open tab General/Ethernet - Set robot IP Address to something like - 192.168.100.xxx
Make sure MAC address is not all zero. If so contact support@kinova.ca
Press 'Update' and restart robot
In a terminal ping your robot's IP, your robot is setup for ethernet
要通过ROS中的以太网连接到机器人，只需在robot_parameters.yaml中设置这些参数：
connection_type: Ethernet
local_machine_IP: [your PC network IP]
subnet_mask: [your network subnet mask]
