赞
踩
目的:将moveit的规划好的路径传给gazebo
主要的难点添加控制器命名要相对应。
moveit处配置
1、新建gazebo中的控制器
在moveit内新建controllers_gazebo.yaml ,其与controllers.yaml类似;(主要是命名空间)
- controller_manager_ns: controller_manager
- controller_list:
- - name: arm/left_rm65_controller
- action_ns: follow_joint_trajectory
- type: FollowJointTrajectory
- default: true
- joints:
- - L1_joint
- - L2_joint
- - L3_joint
- - L4_joint
- - L5_joint
- - L6_joint
-
- - name: arm/right_rm65_controller
- action_ns: follow_joint_trajectory
- type: FollowJointTrajectory
- default: true
- joints:
- - R1_joint
- - R2_joint
- - R3_joint
- - R4_joint
- - R5_joint
- - R6_joint
-
- - name: arm/dh_gripper_controller
- action_ns: follow_joint_trajectory
- type: FollowJointTrajectory
- default: true
- joints:
- - L1_tool_trans
- - L2_tool_trans
-
2、去改moveit的double_arm_moveit_controller_manager.launch.xml添加controllers配置
- <launch>
- <!-- Define the controller manager plugin to use for trajectory execution -->
- <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
-
- <!-- loads controller list to the param server -->
-
- <!--moveit单独使用,采用这个控制器-->
- <!--<rosparam file="$(find double_arm_moveit_config)/config/ros_controllers.yaml"/>-->
-
- <!--moveit与真实机械臂联用,采用这个控制器-->
- <!-- <rosparam file="$(find double_arm_moveit_config)/config/controllers.yaml"/>-->
-
- <!--moveit与gazebo联合仿真,采用这个控制器-->
- <rosparam file="$(find double_arm_moveit_config)/config/controllers_gazebo.yaml"/>
- </launch>
3、在moveit中创建moveit_planning_execution.launch(中文标注处需要修改)加/
- <launch>
- # The planning and execution components of MoveIt! configured to
- # publish the current configuration of the robot (simulated or real)
- # and the current state of the world as seen by the planner
- <include file="$(find double_arm_moveit_config)/launch/move_group.launch">
- <arg name="publish_monitored_planning_scene" value="true" />
- </include>
- # The visualization component of MoveIt!
- <include file="$(find double_arm_moveit_config)/launch/moveit_rviz.launch"/>
-
- <!-- We do not have a robot connected, so publish fake joint states -->
- <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
- <param name="/use_gui" value="false"/>
-
- <!--根据控制器的不同设置不同-->
- <rosparam param="/source_list">[/arm/joint_states]</rosparam>
- </node>
-
- </launch>
robot端配置
4、gazebo/config 目录下的double_arm_trajectory_control.yaml 配置文件(对应上文3中文处)
- arm:
- left_rm65_controller:
- type: "position_controllers/JointTrajectoryController"
- joints:
- - L1_joint
- - L2_joint
- - L3_joint
- - L4_joint
- - L5_joint
- - L6_joint
-
- gains:
- L1_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
- L2_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
- L3_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
- L4_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
- L5_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
- L6_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
-
- right_rm65_controller:
- type: "position_controllers/JointTrajectoryController"
- joints:
- - R1_joint
- - R2_joint
- - R3_joint
- - R4_joint
- - R5_joint
- - R6_joint
-
- gains:
- R1_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
- R2_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
- R3_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
- R4_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
- R5_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
- R6_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
-
- dh_gripper_controller:
- type: "position_controllers/JointTrajectoryController"
- joints:
- - L1_tool_trans
- - L2_tool_trans
- gains:
- L1_tool_trans: {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0}
- L2_tool_trans: {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0}
5、在gazebo中创建launch文件运行4中建立的yaml(double_arm_trajectory_controller.launch)
- <launch>
-
-
- <rosparam file="$(find double_arm_gazebo)/config/double_arm_trajectory_control.yaml" command="load"/>
-
- <node name="arm_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns = "/arm" args="left_arm_controller gripper_controller right_arm_controller"/>
-
- </launch>
6、创建关节状态控制器(发布机器人的关节状态和TF变换)
config内创建double_arm_gazebo_joint_states.yaml
- arm:
- joint_state_controller:
- type: joint_state_controller/JointStateController
- publish_rate: 50
-
launch文件创建double_arm_gazebo_states.launch 去运行yaml文件(remap映射从moveit映射到gazebo)
- <launch>
- <!-- 将关节控制器的配置参数加载到参数服务器中 -->
- <rosparam file="$(find double_arm_gazebo)/config/double_arm_gazebo_joint_states.yaml" command="load"/>
-
- <node name="joint_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns = "arm" args = "joint_state_controller"/>
-
- <!-- 运行robot_state_publisher节点,发布tf -->
- <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
- respawn="false" output="screen">
- <remap from="/joint_states" to="/arm/joint_states" />
- </node>
-
- </launch>
7、创建arm_world.launch文件去运行gazebo
- <launch>
-
- <!-- these are the arguments you can pass this launch file, for example paused:=true -->
- <arg name="paused" default="false"/>
- <arg name="use_sim_time" default="true"/>
- <arg name="gui" default="true"/>
- <arg name="headless" default="false"/>
- <arg name="debug" default="false"/>
-
- <!-- We resume the logic in empty_world.launch -->
- <include file="$(find gazebo_ros)/launch/empty_world.launch">
- <arg name="debug" value="$(arg debug)" />
- <arg name="gui" value="$(arg gui)" />
- <arg name="paused" value="$(arg paused)"/>
- <arg name="use_sim_time" value="$(arg use_sim_time)"/>
- <arg name="headless" value="$(arg headless)"/>
- </include>
-
- <!-- Load the URDF into the ROS Parameter Server -->
- <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find double_arm_description)/urdf/double_arm_description.urdf.xacro'" />
-
-
- <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
- <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
- args="-urdf -model dual_arm -param robot_description"/>
-
-
- </launch>
8、建立一个launch文件(arm_bringup_moveit.launch)去驱动所有的launch文件
- <launch>
-
- <!--arg name="rvizconfig" default="$(find dual_arm_gazebo)/config/rviz_gazebo.rviz"/-->
-
- <!-- Launch Gazebo -->
- <include file="$(find dual_arm_gazebo)/launch/arm_world.launch" />
-
- <!-- ros_control arm launch file -->
- <include file="$(find dual_arm_gazebo)/launch/arm_gazebo_states.launch" />
-
- <!-- ros_control trajectory control dof arm launch file -->
- <include file="$(find dual_arm_gazebo)/launch/arm_trajectory_controller.launch" />
-
- <!-- moveit launch file -->
- <include file="$(find dual_arm_moveit_config)/launch/moveit_planning_execution.launch" />
-
- </launch>
二、出现的问题有:
1、
sudo apt install ros-noetic-ros-controllers
2、
sudo apt install ros-noetic-joint-trajectory-controller
3、#include <rm_75_msgs/JointPos.h>有波浪线
在json文件上包含文件路径
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。