franka_ros2
备注
franka_ros2
不支持 Windows。
franka_ros2存储库 包含 libfranka 的 ROS 2 集成。
小心
franka_ros2 目前正在快速开发中,预计会出现重大变化。欢迎在 GitHub 上报告错误。
安装
请参阅 README.md
MoveIt
要查看是否一切正常,可以尝试在机器人上运行 MoveIt 示例:
ros2 launch franka_fr3_moveit_config moveit.launch.py robot_ip:=<fci-ip>
然后激活 MotionPlanning
在 RViz 中的显示。
如果您没有机器人,仍然可以通过在虚拟硬件上运行来测试设置:
ros2 launch franka_fr3_moveit_config moveit.launch.py robot_ip:=dont-care use_fake_hardware:=true
等待直到可以在终端内看到来自 MoveIt 的绿色消息 You can start planning now!
。然后取消框选 PlanningScene
并再次框选打开。之后框选打开 MotionPlanning
。
示例控制器
这个 repo 附带了一些位于 franka_example_controllers
包中的示例控制器。
默认情况下,使用夹爪执行以下 launch 文件。如果没有连接夹爪,则可以在 launch 文件中使用 load_gripper:=false
.
移动到起始点
该控制器将机器人移动到其初始配置。
ros2 launch franka_bringup move_to_start_example_controller.launch.py arm_id:=fr3 robot_ip:=<fci-ip>
重力补偿
这是我们拥有的最简单的控制器,也是编写自己的控制器的一个很好的起点。它将零作为扭矩命令发送到所有关节,这意味着机器人只补偿自己的重量。
ros2 launch franka_bringup gravity_compensation_example_controller.launch.py arm_id:=fr3 robot_ip:=<fci-ip>
关节阻抗示例
该示例以非常柔性的周期性移动方式移动关节 4 和 5。可以尝试在运行时移动关节。
ros2 launch franka_bringup joint_impedance_example_controller.launch.py arm_id:=fr3 robot_ip:=<fci-ip>
带IK的关节阻抗示例
该示例使用 MoveIt 服务中的 LMA-Orocos 解算器来计算所需位姿的关节位置。所需位姿是周期性地沿 x 和 z 方向移动末端执行器。您可以在 franka_fr3_moveit_config 包 kinematics.yaml 文件中更改运动学解算器。
ros2 launch franka_bringup joint_impedance_with_ik_example_controller.launch.py arm_id:=fr3 robot_ip:=<fci-ip>
模型示例控制器
这是一个只读控制器,它会打印相对于基准框架的科里奥利力矢量、重力矢量、Joint4 的姿态矩阵、Joint4 主体雅可比矩阵和末端执行器雅可比矩阵。
ros2 launch franka_bringup model_example_controller.launch.py arm_id:=fr3 robot_ip:=<fci-ip>
关节位置示例
此示例向机器人发送周期性位置命令。
重要
位置轨迹需要从机器人的初始位置开始。
要读取机器人的起始位置,您可以在开始发送任何命令之前读取 initial_joint_position. state_interface值。
if (initialization_flag_) {
for (size_t i = 0; i < 7; ++i) {
initial_q_.at(i) = state_interface[i].get_value();
}
initialization_flag_ = false;
}
ros2 launch franka_bringup joint_position_example_controller.launch.py arm_id:=fr3 robot_ip:=<fci-ip>
关节速度示例
此示例向机器人的第 4 和第 5 关节发送周期性速度命令。
ros2 launch franka_bringup joint_velocity_example_controller.launch.py arm_id:=fr3 robot_ip:=<fci-ip>
笛卡尔位姿示例
此示例使用 CartesianPose 接口向机器人发送周期性位姿命令。
ros2 launch franka_bringup cartesian_pose_example_controller.launch.py arm_id:=fr3 robot_ip:=<fci-ip>
笛卡尔方向示例
此示例使用 CartesianOrientation 接口发送围绕机器人末端执行器 X 轴的周期性方向命令。
ros2 launch franka_bringup cartesian_orientation_example_controller.launch.py arm_id:=fr3 robot_ip:=<fci-ip>
笛卡尔位姿肘部示例
此示例发送周期性的肘部命令,同时保持末端执行器的位姿恒定。
ros2 launch franka_bringup cartesian_elbow_example_controller.launch.py arm_id:=fr3 robot_ip:=<fci-ip>
笛卡尔速度示例
此示例使用 CartesianVelocity 接口向机器人发送周期性速度命令。
ros2 launch franka_bringup cartesian_velocity_example_controller.launch.py arm_id:=fr3 robot_ip:=<fci-ip>
笛卡尔肘部示例
此示例使用 CartesianElbow 接口向机器人发送周期性肘部命令,同时保持末端执行器速度恒定。
ros2 launch franka_bringup elbow_example_controller.launch.py arm_id:=fr3 robot_ip:=<fci-ip>
包说明
本节包含对每个包的作用的更详细描述。一般来说,包结构会尽量遵守 此处 提出的结构。
franka_bringup
该软件包包含示例的 launch 文件以及基本的 franka.launch.py
launch 文件,可用于在没有任何控制器的情况下启动机器人。
当启动机器人时:
ros2 launch franka_bringup franka.launch.py arm_id:=fr3 robot_ip:=<fci-ip> use_rviz:=true
除了 joint_state_broadcaster
之外,没有任何控制器在运行。然而,与机器人的连接仍然被建立,并且当前机器人位姿可在 RViz 中可视化。在这种模式下,当用户按下 stop 按钮时,机器人可以被示教。但是,一旦使用了 effort_command_interface
的控制器被启动,机器人将使用来自 libfranka 的扭矩接口。例如,可以通过运行下面的命令启动 gravity_compensation_example_controller
:
ros2 control load_controller --set-state active gravity_compensation_example_controller
这相当于运行 重力补偿 中提到的 launch 文件 gravity_compensation_example_controller.launch.py
。
当控制器通过使用以下命令被停止时:
ros2 control set_controller_state gravity_compensation_example_controller inactive
机器人将停止扭矩控制并仅通过 FCI 发送其当前状态。
现在可以选择再次启动同一个控制器通过:
ros2 control set_controller_state gravity_compensation_example_controller active
或加载并启动另一个:
ros2 control load_controller --set-state active joint_impedance_example_controller
franka_description
警告
从 0.1.14 版本开始,franka_description 包不再在 franka_ros2 存储库中提供。它可在单独的存储库 Franka Description 中找到。
该软件包包含用于可视化机器人的 xacro 文件和 meshes。此外,它包含一个 launch 文件,可以在不访问真实机器人的情况下可视化机器人模型:
ros2 launch franka_description visualize_franka.launch.py load_gripper:=<true|false>
franka_example_controllers
这个包包含一些控制器,可以看作是如何在 ROS 2 中编写控制器的示例。目前,控制器只能访问测量的关节位置和关节速度。基于此信息,控制器可以发送扭矩命令。目前无法使用例如关节位置接口等其他接口。
franka_gripper
此包包含 franka_gripper_node
用于与 Franka Hand
交互。
franka_gripper_node
提供以下操作:
homing
- 将夹爪归零并根据已安装的手指更新最大宽度。move
- 以定义的速度移动到目标宽度。grasp
- 尝试在以给定速度闭合的同时以所期望的力在所期望的宽度抓取。如果夹爪两指之间的距离d
处于width - epsilon.inner < d < width + epsilon.outer
的范围,则该操作是成功的。gripper_action
- 一个 MoveIt 的特殊抓取 action。
此外,还有一项 stop
service 可以中止抓取 action 并停止抓取。
使用以下 launch 文件启动夹爪:
ros2 launch franka_gripper gripper.launch.py robot_ip:=<fci-ip>
您现在可以在另一个的tab中执行homing并发送抓取命令。
ros2 action send_goal /fr3_gripper/homing franka_msgs/action/Homing {}
ros2 action send_goal -f /fr3_gripper/grasp franka_msgs/action/Grasp "{width: 0.00, speed: 0.03, force: 50}"
默认情况下,内部和外部容差(epsilon)为 0.005 米。还可以显式设置容差:
ros2 action send_goal -f /fr3_gripper/grasp franka_msgs/action/Grasp "{width: 0.00, speed: 0.03, force: 50, epsilon: {inner: 0.01, outer: 0.01}}"
要停止抓取,可以使用 stop
service。
ros2 service call /fr3_gripper/stop std_srvs/srv/Trigger {}
franka_hardware
重要
自 0.1.14 版本起的重大变化: franka_hardware
robot_state 和 robot_model 将以 arm_id
作为前缀。
panda/robot_model -> ${arm_id}/robot_model
panda/robot_state -> ${arm_id}/robot_state
状态和命令接口的命名没有变化。它们以 URDF 中的关节名称作为前缀。
该软件包包含 ros2_control 所需的 franka_hardware
插件。该插件从机器人的 URDF 加载,并通过机器人描述传递给控制器管理器。它为每个关节提供:
一个包含测量的关节位置的
position state interface
。一个包含测量的关节速度的
velocity state interface
。一个包含测量的连杆侧关节扭矩的
effort state interface
一个包含机器人初始关节位置的
initial_position state interface
。一个包含期望的无重力的关节扭矩的
effort command interface
。一个包含期望的关节位置的
position command interface
。一个包含期望的关节速度的
velocity command interface
。
此外
一个包含机器人状态信息的
franka_robot_state
, franka_robot_state 。一个包含指向模型对象的指针的
ranka_robot_model_interface
。
重要
franka_robot_state
和 franka_robot_model_interface
状态接口不应直接从硬件状态接口使用。相反,它们应该由 franka_semantic_components 接口使用。
机器人的 IP 通过来自 URDF 的参数读取。
franka_semantic_components
该包包含franka_robot_model、franka_robot_state和cartesian命令类。这些类用于转换franka_robot_model对象和franka_robot_state对象,它们以双指针形式存储在hardware_state_interface中。
有关如何使用这些类的更多参考: Franka Robot State Broadcaster 和 Franka 示例控制器(model_example_controller)
笛卡尔位姿接口:
此接口用于使用借用的命令接口向机器人发送笛卡尔位姿命令。FrankaSemanticComponentInterface 类负责处理借用的命令接口和状态接口。在启动笛卡尔位姿接口时,用户需要向构造函数传递一个布尔标志,以指示该接口是否用于肘部。
auto is_elbow_active = false;
CartesianPoseInterface cartesian_pose_interface(is_elbow_active);
该接口允许用户读取由franka hardware接口设置的当前位姿命令接口值。
std::array<double, 16> pose;
pose = cartesian_pose_interface.getInitialPoseMatrix();
还可以读取 Eigen 格式的四元数和平移值。
Eigen::Quaterniond quaternion;
Eigen::Vector3d translation;
std::tie(quaternion, translation) = cartesian_pose_interface.getInitialOrientationAndTranslation();
设置好笛卡尔接口后,您需要在控制器中 assign_loaned_command_interfaces
和 assign_loaned_state_interfaces
。这需要在控制器的 on_activate() 函数中完成。示例可以在 assign loaned comamand interface example 中找到。
cartesian_pose_interface.assign_loaned_command_interfaces(command_interfaces_);
cartesian_pose_interface.assign_loaned_state_interfaces(state_interfaces_);
在控制器的更新功能中,您可以向机器人发送位姿命令。
std::array<double, 16> pose;
pose = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0.5, 0, 0.5, 1};
cartesian_pose_interface.setCommanded(pose);
或者您可以以 Eigen 格式发送四元数、平移值。
Eigen::Quaterniond quaternion(1, 0, 0, 0);
Eigen::Vector3d translation(0.5, 0, 0.5);
cartesian_pose_interface.setCommand(quaternion, translation);
笛卡尔速度接口:
此接口用于使用借用的命令接口向机器人发送笛卡尔速度命令。FrankaSemanticComponentInterface 类负责处理借用的命令接口和状态接口。
auto is_elbow_active = false;
CartesianVelocityInterface cartesian_velocity_interface(is_elbow_active);
要将速度命令发送给机器人,您需要在自定义控制器中assign_loaned_command_interface。
cartesian_velocity_interface.assign_loaned_command_interface(command_interfaces_);
在控制器的更新功能中,您可以向机器人发送笛卡尔速度命令。
std::array<double, 6> cartesian_velocity;
cartesian_velocity = {0, 0, 0, 0, 0, 0.1};
cartesian_velocity_interface.setCommand(cartesian_velocity);
franka_robot_state_broadcaster
此软件包包含只读的 franka_robot_state_broadcaster 控制器。它将 franka_robot_state 主题发布到名为 /franka_robot_state_broadcaster/robot_state 的主题。此控制器节点由 franka_bringup 中的 franka_launch.py 生成。因此,所有包含 franka_launch.py 的示例都会发布 robot_state 主题。
franka_fr3_moveit_config
此包包含 MoveIt2 的配置。有一个名为 panda_manipulator
的新移动规划组 ,其尖端位于夹爪的指尖,并且其 Z 轴旋转了 -45 度, 因此 X 轴现在朝前,使其更易于使用。移动组 panda_arm
仍可用于向后兼容。新的应用程序应该使用新的 panda_manipulator
移动组替换。
franka_msgs
此包包含不同夹爪动作和机器人状态消息的定义。
joint_effort_trajectory_controller
此包包含经过修改的 joint_trajectory_controller,可以使用 franka_hardware::FrankaHardwareInterface
的扭矩接口。它基于此 Pull request 。
备注
由于修复程序已在 ros2_controllers 主分支中可用,因此该包将很快被删除。一旦将其反向移植到 Humble,它将从 franka_ros2 存储库中删除。
franka_gazebo
重要
franka_description 最低版本要求为 0.3.0。你可以从 https://github.com/frankaemika/franka_description 克隆 franka_description 包。
将 Franka ROS 2 与 Gazebo 仿真器集成的项目。
启动 RVIZ + Gazebo
启动一个生成 RVIZ 和 Gazebo 来显示机器人的示例:
ros2 launch franka_gazebo_bringup visualize_franka_robot.launch.py
如果要显示另一个机器人,可以定义arm_id:
ros2 launch franka_gazebo_bringup visualize_franka_robot.launch.py arm_id:=fp3
如果您想启动含有 franka_hand 的仿真:
ros2 launch franka_gazebo_bringup visualize_franka_robot.launch.py load_gripper:=true franka_hand:='franka_hand'
使用 Gazebo 的关节速度控制示例
开始之前,请确保构建了 franka_example_controllers 和 franka_description 包。 franka_description 的最低版本必须是 0.3.0。
colcon build --packages-select franka_example_controllers
现在您可以使用 Gazebo 仿真器启动速度示例。
ros2 launch franka_gazebo_bringup gazebo_joint_velocity_controller_example.launch.py load_gripper:=true franka_hand:='franka_hand'
请记住,夹爪关节与关节速度控制器存在bug。如果您有兴趣控制夹爪,请使用关节位置接口。
使用 Gazebo 的关节位置控制示例
要运行关节位置控制示例,您需要安装了在关节速度控制部分列出所需的软件。
然后您可以使用以下命令运行。
ros2 launch franka_gazebo_bringup gazebo_joint_position_controller_example.launch.py load_gripper:=true franka_hand:='franka_hand'
使用 Gazebo 的关节阻抗控制示例
对于运行扭矩示例。您必须编译位于 franka_gazebo 下的 franka_ign_ros2_control 包。您可以使用以下命令编译 franka_ign_ros2_control 。
colcon build --packages-select franka_ign_ros2_control
然后source你的工作区。
source install/setup.sh
然后您就可以运行阻抗控制示例了。
ros2 launch franka_gazebo_bringup gazebo_joint_impedance_controller_example.launch.py load_gripper:=true franka_hand:='franka_hand'
故障排除
如果您遇到 Gazebo 找不到模型文件的情况,请尝试包含工作区。例如:
export GZ_SIM_RESOURCE_PATH=${GZ_SIM_RESOURCE_PATH}:/workspaces/src/
franka_ros 和 franka_ros2 的区别
本节概述了 franka_ros
和 franka_ros2
之间的基本变化。
franka_gripper
所有 topics和 actions 之前都以
franka_gripper
作为前缀。此前缀已重命名为panda_gripper
,以便在未来启用所有前缀都基于arm_id
的工作流程,从而轻松启用多臂设置。该
stop
action 现在是一个 service action,因为它不可抢占。所有 actions(除了
gripper_action
)都以当前夹爪宽度作为反馈。
franka_visualization
这个包已经不存在了。但是, franka_description 提供了一个launch文件来可视化机器人模型,而无需连接到机器人
franka_control
这个包已经不存在了。与机器人的连接由 franka_hardware 包中的硬件插件提供。它提供的 actions 和 services 目前未出现在 franka_ros2
中。
编写控制器
与 franka_ros
相比我们目前提供了简化版的控制器接口:
Joint positions - 关节位置
Joint velocities - 关节速度
Measured torques - 测量扭矩
Franka robot state - Franka 机器人状态
Franka robot model - Franka 机器人模型
重要
Franka 机器人状态通过 franka_robot_state_broadcaster 包发布到名为 /franka_robot_state_broadcaster/robot_state 的主题。
重要
建议通过 franka_semantic_components 类使用 Franka robot state 和 Franka robot model。它们作为双指针存储在 state_interface 中,并转换回 franka_semantic_component 类中的原始对象。
可以在 franka_example_controllers 包中找到使用 franka_model 的示例:model_example_controller 。
您可以基于 franka_example_controllers 之一来构建自己的控制器。要计算机器人的运动学和动力学,可以在 KDL 等库中使用机器人的关节状态和 URDF(其中还有一个可用的 ROS 2 包)。
非实时机器人参数设置
非实时机器人参数设置可以通过 ROS 2 服务进行。它们在机器人硬件初始化后发布。
服务名称如下:
* /service_server/set_cartesian_stiffness
* /service_server/set_force_torque_collision_behavior
* /service_server/set_full_collision_behavior
* /service_server/set_joint_stiffness
* /service_server/set_load
* /service_server/set_parameters
* /service_server/set_parameters_atomically
* /service_server/set_stiffness_frame
* /service_server/set_tcp_frame
服务消息描述如下:
franka_msgs::srv::SetJointStiffness
指定内部控制器的关节刚度(阻尼自动从刚度得出)。
franka_msgs::srv::SetCartesianStiffness
指定内部控制器的笛卡尔刚度(阻尼自动从刚度得出)。
franka_msgs::srv::SetTCPFrame
指定从 <arm_id>_EE(末端执行器)到 <arm_id>_NE(标称末端执行器)坐标系的转换。从法兰到末端执行器坐标系的转换分为两个转换:<arm_id>_EE 到 <arm_id>_NE 坐标系和 <arm_id>_NE 到 <arm_id>_link8 坐标系。从 <arm_id>_NE 到 <arm_id>_link8 坐标系的转换只能通过管理员界面进行设置。
franka_msgs::srv::SetStiffnessFrame
指定从 <arm_id>_K 到 <arm_id>_EE 框架的转换。
franka_msgs::srv::SetForceTorqueCollisionBehavior
设置外部笛卡尔力旋量的阈值来配置碰撞反射。
franka_msgs::srv::SetFullCollisionBehavior
设置笛卡尔和关节层的外力阈值来配置碰撞反射。
franka_msgs::srv::SetLoad
设置外部负载来补偿(例如抓取的物体)。
启动 franka_bringup/franka.launch.py 文件来初始化机器人硬件:
ros2 launch franka_bringup franka.launch.py robot_ip:=<fci-ip>
这是一个简单的例子:
ros2 service call /service_server/set_joint_stif
fness franka_msgs/srv/SetJointStiffness "{joint_stiffness: [1000.0, 1000.0, 10
00.0, 1000.0, 1000.0, 1000.0, 1000.0]}"
重要
非实时参数设置只能在机器人硬件处于 idle(空闲) 模式时进行。如果控制器处于活动状态并声明命令接口,这将使机器人处于 move(移动) 模式。在 move(移动) 模式下,无法进行非实时参数设置。
重要
<arm_id>_EE 坐标系表示可配置末端执行器坐标系的一部分,可在运行时通过 franka_ros 进行调整。<arm_id>_K 坐标系标记内部笛卡尔阻抗的中心。它还可用作外部力旋量的参考坐标系。 <arm_id>_EE 和 <arm_id>_K 都不包含在 URDF 中,因为它们可以在运行时更改 。默认情况下,<arm_id> 设置为 “panda”。
非实时 ROS 2 操作
非实时 ROS 2 操作可以通过 ActionServer 完成。以下操作可用:
/action_server/error_recovery
- 从机器人错误中自动恢复。
使用的消息是:
franka_msgs::action::ErrorRecovery
- 没有参数。
使用示例:
ros2 action send_goal /action_server/error_recovery franka_msgs/action/ErrorRecovery {}
已知的问题
当使用
fake_hardware
和 MoveIt 时,需要一些时间直到默认位置被应用。