Обновлены параметры планирования и добавлены новые файлы для модуля планирования движения, включая C++ и Python реализации.
This commit is contained in:
Даниил Грабарь
@@ -13,7 +13,7 @@ planning_pipelines:
|
|||||||
plan_request_params:
|
plan_request_params:
|
||||||
planning_attempts: 1
|
planning_attempts: 1
|
||||||
planning_pipeline: ompl
|
planning_pipeline: ompl
|
||||||
max_velocity_scaling_factor: 1.0
|
max_velocity_scaling_factor: 0.5
|
||||||
max_acceleration_scaling_factor: 1.0
|
max_acceleration_scaling_factor: 1.0
|
||||||
|
|
||||||
ompl_rrtc: # Namespace for individual plan request
|
ompl_rrtc: # Namespace for individual plan request
|
||||||
|
|||||||
@@ -0,0 +1,295 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- | This document was autogenerated by xacro from iiwa7.urdf.xacro | -->
|
||||||
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<robot name="iiwa7">
|
||||||
|
<link name="world"/>
|
||||||
|
<link name="base_link">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.012856764088336 7.05371084939836E-07 0.0679655592591469"/>
|
||||||
|
<mass value="30.2763517543289"/>
|
||||||
|
<inertia ixx="0.111239565538359" ixy="-3.81097437253422e-07" ixz="0.00486659750522703" iyy="0.139017104291389" iyz="7.87871531503135e-07" izz="0.133028621926226"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/visual/base_link.DAE"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="iiwa_material">
|
||||||
|
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/collision/base_link.STL"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<link name="link1">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-4.78790441849064E-07 -0.0348192058752328 -0.0692021315991948"/>
|
||||||
|
<mass value="21.1627406257303"/>
|
||||||
|
<inertia ixx="0.133321610552378" ixy="-5.3830316300325e-07" ixz="9.16989471201082e-07" iyy="0.127213841326705" iyz="0.0236704924620788" izz="0.047155320135412"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/visual/link1.DAE"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="iiwa_material">
|
||||||
|
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/collision/link1.STL"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<link name="link2">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-5.2906167189587E-07 -0.0869460684741495 0.0287906957294968"/>
|
||||||
|
<mass value="25.1403147192251"/>
|
||||||
|
<inertia ixx="0.197631241693207" ixy="9.50708464039702e-08" ixz="-3.51963818841194e-07" iyy="0.0602720613821397" iyz="-0.0367196702125505" izz="0.187557269753206"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/visual/link2.DAE"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="iiwa_material">
|
||||||
|
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/collision/link2.STL"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<link name="link3">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="8.94264666317923E-07 0.0343188244798595 -0.0692020521907963"/>
|
||||||
|
<mass value="21.1627874292289"/>
|
||||||
|
<inertia ixx="0.133322417523041" ixy="-2.72387851582384e-07" ixz="-2.31579646685682e-07" iyy="0.127213838333813" iyz="-0.0236710393468367" izz="0.0471557500559207"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/visual/link3.DAE"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="iiwa_material">
|
||||||
|
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/collision/link3.STL"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<link name="link4">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="9.9343785475714E-08 -0.0869462342636022 -0.0292907044657978"/>
|
||||||
|
<mass value="25.140328983289"/>
|
||||||
|
<inertia ixx="0.197630839812683" ixy="-3.9082887807781e-07" ixz="-3.24564289064112e-07" iyy="0.0602723071447973" iyz="0.0367195588674331" izz="0.187556898167347"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/visual/link4.DAE"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="iiwa_material">
|
||||||
|
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/collision/link4.STL"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<link name="link5">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-1.5059645121912E-07 -0.0298223596156005 -0.116233672352317"/>
|
||||||
|
<mass value="9.53466992761605"/>
|
||||||
|
<inertia ixx="0.0529038960536523" ixy="6.86274869084724e-08" ixz="3.57472026918582e-07" iyy="0.0444942713885967" iyz="0.0174914068345989" izz="0.0239916848880951"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/visual/link5.DAE"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="iiwa_material">
|
||||||
|
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/collision/link5.STL"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<link name="link6">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="3.09820422451923E-06 -0.000588422963829549 0.000348569863276505"/>
|
||||||
|
<mass value="14.2007599322815"/>
|
||||||
|
<inertia ixx="0.0384515762635731" ixy="-6.70279497724119e-08" ixz="2.19173188415509e-06" iyy="0.0280010940276735" iyz="-0.00192249677468735" izz="0.0368124806983828"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/visual/link6.DAE"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="iiwa_material">
|
||||||
|
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/collision/link6.STL"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<link name="link7">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="5.28878934317328E-07 3.37199922245852E-07 -0.0274928177649973"/>
|
||||||
|
<mass value="2.47156969383589"/>
|
||||||
|
<inertia ixx="0.00177365937026673" ixy="-1.2410521430667e-07" ixz="-2.95409038849194e-08" iyy="0.00177328722902027" iyz="-1.57567641084958e-08" izz="0.00272748710623445"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/visual/link7.DAE"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="iiwa_material">
|
||||||
|
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://iiwa_description/resource/meshes/collision/link7.STL"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<link name="link_ee"/>
|
||||||
|
<joint name="world_base_joint" type="fixed">
|
||||||
|
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
|
||||||
|
<parent link="world"/>
|
||||||
|
<child link="base_link"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="joint1" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0.3375"/>
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="link1"/>
|
||||||
|
<axis xyz="0 0 -1"/>
|
||||||
|
<limit effort="500" lower="-2.97" upper="2.97" velocity="1.71"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="joint2" type="revolute">
|
||||||
|
<origin rpy="-1.5708 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="link1"/>
|
||||||
|
<child link="link2"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="500" lower="-2.09" upper="2.09" velocity="1.71"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="joint3" type="revolute">
|
||||||
|
<origin rpy="1.5708 0 0" xyz="0 -0.3993 0"/>
|
||||||
|
<parent link="link2"/>
|
||||||
|
<child link="link3"/>
|
||||||
|
<axis xyz="0 0 -1"/>
|
||||||
|
<limit effort="500" lower="-2.97" upper="2.97" velocity="1.75"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="joint4" type="revolute">
|
||||||
|
<origin rpy="-1.5708 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="link3"/>
|
||||||
|
<child link="link4"/>
|
||||||
|
<axis xyz="0 0 -1"/>
|
||||||
|
<limit effort="500" lower="-2.09" upper="2.09" velocity="2.27"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="joint5" type="revolute">
|
||||||
|
<origin rpy="1.5708 0 0" xyz="0 -0.3993 0"/>
|
||||||
|
<parent link="link4"/>
|
||||||
|
<child link="link5"/>
|
||||||
|
<axis xyz="0 0 -1"/>
|
||||||
|
<limit effort="500" lower="-2.97" upper="2.97" velocity="2.44"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="joint6" type="revolute">
|
||||||
|
<origin rpy="-1.5708 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="link5"/>
|
||||||
|
<child link="link6"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="500" lower="-2.09" upper="2.09" velocity="3.14"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="joint7" type="revolute">
|
||||||
|
<origin rpy="1.5708 0 0" xyz="0 -0.126 0"/>
|
||||||
|
<parent link="link6"/>
|
||||||
|
<child link="link7"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="500" lower="-3.05" upper="3.05" velocity="3.14"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="tools_joint" type="fixed">
|
||||||
|
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
|
||||||
|
<parent link="link7"/>
|
||||||
|
<child link="link_ee"/>
|
||||||
|
</joint>
|
||||||
|
<webots>
|
||||||
|
<plugin type="webots_ros2_control::Ros2Control"/>
|
||||||
|
</webots>
|
||||||
|
<ros2_control name="iiwaWebotsControl" type="system">
|
||||||
|
<hardware>
|
||||||
|
<plugin>webots_ros2_control::Ros2ControlSystem</plugin>
|
||||||
|
<!-- <plugin>mock_components/GenericSystem</plugin> -->
|
||||||
|
</hardware>
|
||||||
|
<joint name="joint1">
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">0</param>
|
||||||
|
</state_interface>
|
||||||
|
</joint>
|
||||||
|
<joint name="joint2">
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">0</param>
|
||||||
|
</state_interface>
|
||||||
|
</joint>
|
||||||
|
<joint name="joint3">
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">0</param>
|
||||||
|
</state_interface>
|
||||||
|
</joint>
|
||||||
|
<joint name="joint4">
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">0</param>
|
||||||
|
</state_interface>
|
||||||
|
</joint>
|
||||||
|
<joint name="joint5">
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">0</param>
|
||||||
|
</state_interface>
|
||||||
|
</joint>
|
||||||
|
<joint name="joint6">
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">0</param>
|
||||||
|
</state_interface>
|
||||||
|
</joint>
|
||||||
|
<joint name="joint7">
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">0</param>
|
||||||
|
</state_interface>
|
||||||
|
</joint>
|
||||||
|
</ros2_control>
|
||||||
|
</robot>
|
||||||
@@ -12,43 +12,50 @@
|
|||||||
<!-- joint1 -->
|
<!-- joint1 -->
|
||||||
<xacro:revolute_joint jname="joint1" parent="base_link" child="link1"
|
<xacro:revolute_joint jname="joint1" parent="base_link" child="link1"
|
||||||
xyz="0 0 0.3375" rpy="0 0 0" axis="0 0 -1"
|
xyz="0 0 0.3375" rpy="0 0 0" axis="0 0 -1"
|
||||||
lower="-2.97" upper="2.97" effort="500" velocity="1.71"/>
|
lower="-2.97" upper="2.97" effort="500"
|
||||||
|
velocity="1.71" damping="0.5"/>
|
||||||
|
|
||||||
|
|
||||||
<!-- joint2 -->
|
<!-- joint2 -->
|
||||||
<xacro:revolute_joint jname="joint2" parent="link1" child="link2"
|
<xacro:revolute_joint jname="joint2" parent="link1" child="link2"
|
||||||
xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
|
xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
|
||||||
lower="-2.09" upper="2.09" effort="500" velocity="1.71"/>
|
lower="-2.09" upper="2.09" effort="500"
|
||||||
|
velocity="1.71" damping="0.5"/>
|
||||||
|
|
||||||
|
|
||||||
<!-- joint3 -->
|
<!-- joint3 -->
|
||||||
<xacro:revolute_joint jname="joint3" parent="link2" child="link3"
|
<xacro:revolute_joint jname="joint3" parent="link2" child="link3"
|
||||||
xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 -1"
|
xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 -1"
|
||||||
lower="-2.97" upper="2.97" effort="500" velocity="1.75"/>
|
lower="-2.97" upper="2.97" effort="500"
|
||||||
|
velocity="1.75" damping="0.5"/>
|
||||||
|
|
||||||
|
|
||||||
<!-- joint4 -->
|
<!-- joint4 -->
|
||||||
<xacro:revolute_joint jname="joint4" parent="link3" child="link4"
|
<xacro:revolute_joint jname="joint4" parent="link3" child="link4"
|
||||||
xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 -1"
|
xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 -1"
|
||||||
lower="-2.09" upper="2.09" effort="500" velocity="2.27"/>
|
lower="-2.09" upper="2.09" effort="500"
|
||||||
|
velocity="2.27" damping="0.5"/>
|
||||||
|
|
||||||
|
|
||||||
<!-- joint5 -->
|
<!-- joint5 -->
|
||||||
<xacro:revolute_joint jname="joint5" parent="link4" child="link5"
|
<xacro:revolute_joint jname="joint5" parent="link4" child="link5"
|
||||||
xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 -1"
|
xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 -1"
|
||||||
lower="-2.97" upper="2.97" effort="500" velocity="2.44"/>
|
lower="-2.97" upper="2.97" effort="500"
|
||||||
|
velocity="2.44" damping="0.5"/>
|
||||||
|
|
||||||
|
|
||||||
<!-- joint6 -->
|
<!-- joint6 -->
|
||||||
<xacro:revolute_joint jname="joint6" parent="link5" child="link6"
|
<xacro:revolute_joint jname="joint6" parent="link5" child="link6"
|
||||||
xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
|
xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
|
||||||
lower="-2.09" upper="2.09" effort="500" velocity="3.14"/>
|
lower="-2.09" upper="2.09" effort="500"
|
||||||
|
velocity="3.14" damping="0.5"/>
|
||||||
|
|
||||||
|
|
||||||
<!-- joint7 -->
|
<!-- joint7 -->
|
||||||
<xacro:revolute_joint jname="joint7" parent="link6" child="link7"
|
<xacro:revolute_joint jname="joint7" parent="link6" child="link7"
|
||||||
xyz="0 -0.126 0" rpy="1.5708 0 0" axis="0 0 1"
|
xyz="0 -0.126 0" rpy="1.5708 0 0" axis="0 0 1"
|
||||||
lower="-3.05" upper="3.05" effort="500" velocity="3.14"/>
|
lower="-3.05" upper="3.05" effort="500"
|
||||||
|
velocity="3.14" damping="0.5"/>
|
||||||
|
|
||||||
<joint name="tools_joint" type="fixed">
|
<joint name="tools_joint" type="fixed">
|
||||||
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
|
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
|
||||||
|
|||||||
@@ -30,13 +30,14 @@
|
|||||||
</xacro:macro>
|
</xacro:macro>
|
||||||
|
|
||||||
<!-- Макрос для описания соединений -->
|
<!-- Макрос для описания соединений -->
|
||||||
<xacro:macro name="revolute_joint" params="jname parent child xyz rpy axis lower upper effort velocity">
|
<xacro:macro name="revolute_joint" params="jname parent child xyz rpy axis lower upper effort velocity damping">
|
||||||
<joint name="${jname}" type="revolute">
|
<joint name="${jname}" type="revolute">
|
||||||
<origin xyz="${xyz}" rpy="${rpy}"/>
|
<origin xyz="${xyz}" rpy="${rpy}"/>
|
||||||
<parent link="${parent}"/>
|
<parent link="${parent}"/>
|
||||||
<child link="${child}"/>
|
<child link="${child}"/>
|
||||||
<axis xyz="${axis}"/>
|
<axis xyz="${axis}"/>
|
||||||
<limit lower="${lower}" upper="${upper}" effort="${effort}" velocity="${velocity}"/>
|
<limit lower="${lower}" upper="${upper}" effort="${effort}" velocity="${velocity}"/>
|
||||||
|
<dynamics damping="${damping}"/>
|
||||||
</joint>
|
</joint>
|
||||||
</xacro:macro>
|
</xacro:macro>
|
||||||
|
|
||||||
|
|||||||
@@ -2,14 +2,14 @@
|
|||||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" xacro:version="1.0">
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" xacro:version="1.0">
|
||||||
|
|
||||||
<!-- Массы звеньев -->
|
<!-- Массы звеньев -->
|
||||||
<xacro:property name="mass_base" value="30.2763517543289"/>
|
<xacro:property name="mass_base" value="0.1"/>
|
||||||
<xacro:property name="mass_link1" value="21.1627406257303"/>
|
<xacro:property name="mass_link1" value="4"/>
|
||||||
<xacro:property name="mass_link2" value="25.1403147192251"/>
|
<xacro:property name="mass_link2" value="4"/>
|
||||||
<xacro:property name="mass_link3" value="21.1627874292289"/>
|
<xacro:property name="mass_link3" value="3"/>
|
||||||
<xacro:property name="mass_link4" value="25.140328983289"/>
|
<xacro:property name="mass_link4" value="2.7"/>
|
||||||
<xacro:property name="mass_link5" value="9.53466992761605"/>
|
<xacro:property name="mass_link5" value="1.7"/>
|
||||||
<xacro:property name="mass_link6" value="14.2007599322815"/>
|
<xacro:property name="mass_link6" value="1.8"/>
|
||||||
<xacro:property name="mass_link7" value="2.47156969383589"/>
|
<xacro:property name="mass_link7" value="0.3"/>
|
||||||
|
|
||||||
<!-- Общие цвета / материал -->
|
<!-- Общие цвета / материал -->
|
||||||
<xacro:property name="color_r" value="0.647058823529412"/>
|
<xacro:property name="color_r" value="0.647058823529412"/>
|
||||||
|
|||||||
@@ -10,11 +10,32 @@ find_package(ament_cmake REQUIRED)
|
|||||||
find_package(ament_cmake_python REQUIRED)
|
find_package(ament_cmake_python REQUIRED)
|
||||||
find_package(rclcpp REQUIRED)
|
find_package(rclcpp REQUIRED)
|
||||||
find_package(rclpy REQUIRED)
|
find_package(rclpy REQUIRED)
|
||||||
|
find_package(moveit_ros_planning_interface REQUIRED)
|
||||||
|
find_package(moveit_core REQUIRED)
|
||||||
|
find_package(moveit_ros_planning REQUIRED)
|
||||||
|
find_package(moveit_msgs REQUIRED)
|
||||||
|
find_package(geometry_msgs REQUIRED)
|
||||||
|
|
||||||
|
add_executable(motion_planning_cpp src/motion_planning_cpp.cpp)
|
||||||
|
target_include_directories(motion_planning_cpp PUBLIC include)
|
||||||
|
|
||||||
|
ament_target_dependencies(motion_planning_cpp
|
||||||
|
rclcpp
|
||||||
|
moveit_ros_planning_interface
|
||||||
|
moveit_core
|
||||||
|
moveit_ros_planning
|
||||||
|
moveit_msgs
|
||||||
|
geometry_msgs
|
||||||
|
)
|
||||||
|
|
||||||
include_directories(include)
|
|
||||||
|
|
||||||
ament_python_install_package(${PROJECT_NAME})
|
ament_python_install_package(${PROJECT_NAME})
|
||||||
|
|
||||||
|
install(TARGETS
|
||||||
|
motion_planning_cpp
|
||||||
|
DESTINATION lib/${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
|
||||||
install(PROGRAMS
|
install(PROGRAMS
|
||||||
# scripts/motion_planning_test.py
|
# scripts/motion_planning_test.py
|
||||||
scripts/motion_planning.py
|
scripts/motion_planning.py
|
||||||
|
|||||||
@@ -14,9 +14,10 @@ class MotionPlaning(Node):
|
|||||||
super().__init__("motion_planning_node")
|
super().__init__("motion_planning_node")
|
||||||
|
|
||||||
self.moveit = MoveItPy(node_name="motion_planning_node")
|
self.moveit = MoveItPy(node_name="motion_planning_node")
|
||||||
self.robot_arm = self.moveit.get_planning_component("iiwa_arm")
|
self.robot_arm: PlanningComponent = self.moveit.get_planning_component("iiwa_arm")
|
||||||
self.planning_scene_monitor = self.moveit.get_planning_scene_monitor()
|
self.planning_scene_monitor = self.moveit.get_planning_scene_monitor()
|
||||||
|
|
||||||
|
|
||||||
self.get_logger().warn("***********************************************************")
|
self.get_logger().warn("***********************************************************")
|
||||||
self.get_logger().info("MotionPlanning node is ready...")
|
self.get_logger().info("MotionPlanning node is ready...")
|
||||||
self.get_logger().warn("***********************************************************")
|
self.get_logger().warn("***********************************************************")
|
||||||
|
|||||||
@@ -0,0 +1,58 @@
|
|||||||
|
#include <chrono>
|
||||||
|
#include <memory>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include <pluginlib/class_loader.hpp>
|
||||||
|
|
||||||
|
#include <rclcpp/rclcpp.hpp>
|
||||||
|
#include <moveit/robot_model_loader/robot_model_loader.hpp>
|
||||||
|
#include <moveit/planning_pipeline/planning_pipeline.hpp>
|
||||||
|
#include <moveit/planning_scene/planning_scene.hpp>
|
||||||
|
#include <moveit/kinematic_constraints/utils.hpp>
|
||||||
|
#include <moveit_msgs/msg/display_trajectory.hpp>
|
||||||
|
#include <moveit_msgs/msg/planning_scene.hpp>
|
||||||
|
#include <moveit/move_group_interface/move_group_interface.hpp>
|
||||||
|
|
||||||
|
|
||||||
|
class MotionPlanning: public rclcpp::Node
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
MotionPlanning() : Node("motion_planning_node",
|
||||||
|
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true))
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(get_logger(), "***************");
|
||||||
|
RCLCPP_INFO(get_logger(), "MotionPlaning node created");
|
||||||
|
RCLCPP_INFO(get_logger(), "***************");
|
||||||
|
|
||||||
|
timer_ = create_wall_timer(
|
||||||
|
std::chrono::milliseconds(200),
|
||||||
|
std::bind(&MotionPlanning::runOnce, this)
|
||||||
|
);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
void runOnce()
|
||||||
|
{
|
||||||
|
timer_->cancel();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
rclcpp::TimerBase::SharedPtr timer_;
|
||||||
|
|
||||||
|
const std::string planning_group_ = "iiwa_arm";
|
||||||
|
const std::string base_frame_ = "base_link";
|
||||||
|
const std::string ee_link = "link_ee";
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
int main(int argc, char * argv[])
|
||||||
|
{
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
rclcpp::spin(std::make_shared<MotionPlanning>());
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user