remove old robot model
This commit is contained in:
parent
388692d941
commit
96ef9cb69c
4 changed files with 46 additions and 136 deletions
|
@ -8,7 +8,7 @@
|
|||
|
||||
<xacro:arg name="x" default="-0.10" />
|
||||
<xacro:arg name="y" default="0.0" />
|
||||
<xacro:arg name="z" default="0.03" />
|
||||
<xacro:arg name="z" default="0.0" />
|
||||
|
||||
<xacro:arg name="roll" default="0.0" />
|
||||
<xacro:arg name="pitch" default="0.0" />
|
||||
|
|
|
@ -160,7 +160,7 @@
|
|||
<child
|
||||
link="Link2" />
|
||||
<axis
|
||||
xyz="-1 0 0" />
|
||||
xyz="1 0 0" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="1.7453"
|
||||
|
@ -276,7 +276,7 @@
|
|||
<child
|
||||
link="Link4" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.2832"
|
||||
upper="6.2832"
|
||||
|
@ -505,17 +505,45 @@
|
|||
<axis
|
||||
xyz="0 0 0" />
|
||||
</joint>
|
||||
<link name="grasp_point"/>
|
||||
<joint
|
||||
name="grasp_point_joint"
|
||||
type="fixed">
|
||||
<origin
|
||||
xyz="0.0 -0.095 0.05"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="Link6" />
|
||||
<child
|
||||
link="grasp_point" />
|
||||
<axis
|
||||
xyz="0 0 0" />
|
||||
</joint>
|
||||
|
||||
<xacro:macro name="gazebo_reference" params="joint_name">
|
||||
<gazebo reference='${joint_name}'>
|
||||
<preserveFixedJoint>true</preserveFixedJoint>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:gazebo_reference joint_name="Right_point"/>
|
||||
<xacro:gazebo_reference joint_name="Left_point"/>
|
||||
|
||||
|
||||
<xacro:joint_hardware joint_name="ax1" hardware="${hardware}" initial_joint_position="0.0" />
|
||||
<xacro:joint_hardware joint_name="ax2" hardware="${hardware}" initial_joint_position="0.0" />
|
||||
<xacro:joint_hardware joint_name="ax3" hardware="${hardware}" initial_joint_position="0.0" />
|
||||
<xacro:joint_hardware joint_name="ax2" hardware="${hardware}" initial_joint_position="0.85" />
|
||||
<xacro:joint_hardware joint_name="ax3" hardware="${hardware}" initial_joint_position="1.0" />
|
||||
<xacro:joint_hardware joint_name="ax4" hardware="${hardware}" initial_joint_position="0.0" />
|
||||
<xacro:joint_hardware joint_name="ax5" hardware="${hardware}" initial_joint_position="0.0" />
|
||||
<xacro:joint_hardware joint_name="ax6" hardware="${hardware}" initial_joint_position="0.0" />
|
||||
<xacro:joint_hardware joint_name="ax5" hardware="${hardware}" initial_joint_position="1.0" />
|
||||
<xacro:joint_hardware joint_name="ax6" hardware="${hardware}" initial_joint_position="3.14159" />
|
||||
|
||||
|
||||
<xacro:if value="${hardware=='gazebo'}">
|
||||
<!-- <xacro:fts link="tool0" name="fts_sensor" tf_prefix="${tf_prefix}"></xacro:fts> -->
|
||||
|
||||
<xacro:rgbd parent="Link1" tf_prefix="${tf_prefix}">
|
||||
<origin rpy="0.0 0.0 -1.57" xyz="0.0 -0.035 0.03"></origin>
|
||||
</xacro:rgbd>
|
||||
<gazebo>
|
||||
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
|
||||
<parameters>$(arg simulation_controllers)</parameters>
|
||||
|
@ -524,14 +552,21 @@
|
|||
</ros>
|
||||
</plugin>
|
||||
<plugin filename="VacuumGripper" name="rbs_mill_assist::VacuumGripper">
|
||||
<link_name>Link6</link_name>
|
||||
<link_name>Link8</link_name>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</xacro:if>
|
||||
<gazebo reference="Link6">
|
||||
<sensor name='sensor_contact' type='contact'>
|
||||
<gazebo reference="Link8">
|
||||
<sensor name='left_sensor_contact' type='contact'>
|
||||
<contact>
|
||||
<collision>Link6_collision</collision>
|
||||
<collision>Link8_collision</collision>
|
||||
</contact>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
<gazebo reference="Link7">
|
||||
<sensor name='right_sensor_contact' type='contact'>
|
||||
<contact>
|
||||
<collision>Link7_collision</collision>
|
||||
</contact>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
|
|
@ -1,62 +0,0 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rbs_arm">
|
||||
<xacro:arg name="gripper_name" default="" />
|
||||
<xacro:arg name="hardware" default="gazebo" />
|
||||
<xacro:arg name="simulation_controllers" default="" />
|
||||
<xacro:arg name="tf_prefix" default="" />
|
||||
<xacro:arg name="namespace" default="arm0"/>
|
||||
|
||||
<xacro:arg name="x" default="-0.10" />
|
||||
<xacro:arg name="y" default="0.0" />
|
||||
<xacro:arg name="z" default="0.03" />
|
||||
|
||||
<xacro:arg name="roll" default="0.0" />
|
||||
<xacro:arg name="pitch" default="0.0" />
|
||||
<xacro:arg name="yaw" default="0.0" />
|
||||
|
||||
<xacro:include filename="$(find rbs_mill_assist)/urdf/rbs_arm_modular_macro.xacro" />
|
||||
<link name="world" />
|
||||
|
||||
<xacro:property name="machine_length" value="0.4"/>
|
||||
<xacro:property name="machine_width" value="0.5"/>
|
||||
<xacro:property name="machine_height" value="0.115"/>
|
||||
|
||||
<link name="machine_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="${machine_length} ${machine_width} ${machine_height}"/>
|
||||
</geometry>
|
||||
<material name="gray">
|
||||
<color rgba="0 1 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="${machine_length} ${machine_width} ${machine_height}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="40.0"/>
|
||||
<inertia
|
||||
ixx="${(1/12)*30*(machine_width**2 + machine_height**2)}"
|
||||
iyy="${(1/12)*30*(machine_length**2 + machine_height**2)}"
|
||||
izz="${(1/12)*30*(machine_length**2 + machine_width**2)}"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyz="0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="machine_joint" type="fixed">
|
||||
<parent link="world"/>
|
||||
<child link="machine_link"/>
|
||||
<origin xyz="0.25 0 ${0.115/2}" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<!-- Arm-->
|
||||
<xacro:rbs_arm namespace="$(arg namespace)" parent="world" tf_prefix="$(arg tf_prefix)" hardware="$(arg hardware)"
|
||||
gripper_name="$(arg gripper_name)" controllers="$(arg simulation_controllers)">
|
||||
<origin xyz="$(arg x) $(arg y) $(arg z)" rpy="$(arg roll) $(arg pitch) $(arg yaw)" />
|
||||
</xacro:rbs_arm>
|
||||
</robot>
|
|
@ -1,63 +0,0 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="rbs_arm" params="namespace parent *origin tf_prefix hardware gripper_name controllers">
|
||||
<xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_base_link.xacro"></xacro:include>
|
||||
<xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_ee_link.xacro"></xacro:include>
|
||||
<xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_fork_link.xacro"></xacro:include>
|
||||
<xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_main_link.xacro"></xacro:include>
|
||||
<xacro:include filename="$(find rbs_arm)/urdf/sensors.xacro"></xacro:include>
|
||||
<!-- BEGIN robot description -->
|
||||
<!-- link 0-->
|
||||
<xacro:base_link hardware="${hardware}" parent="${parent}" tf_prefix="${tf_prefix}">
|
||||
<xacro:insert_block name="origin"></xacro:insert_block>
|
||||
</xacro:base_link>
|
||||
|
||||
<!-- link 1-->
|
||||
<xacro:fork_link d="0.2" hardware="${hardware}" name="fork0_link" p="10000" parent="base_link" tf_prefix="${tf_prefix}" initial_joint_position="1.57"></xacro:fork_link>
|
||||
<!-- link 2-->
|
||||
<xacro:main_link d="0.2" hardware="${hardware}" name="main0_link" p="10000" parent="fork0_link" tf_prefix="${tf_prefix}" initial_joint_position="0.5"></xacro:main_link>
|
||||
<!-- link 3-->
|
||||
<xacro:fork_link d="0.2" hardware="${hardware}" name="fork1_link" p="10000" parent="main0_link" tf_prefix="${tf_prefix}" initial_joint_position="0.0"></xacro:fork_link>
|
||||
<!-- link 4-->
|
||||
<xacro:main_link d="0.2" hardware="${hardware}" name="main1_link" p="1000" parent="fork1_link" tf_prefix="${tf_prefix}" initial_joint_position="1.5"></xacro:main_link>
|
||||
<!-- ee link also contain tool0-->
|
||||
<!-- link 5-->
|
||||
<xacro:ee_link d="0.2" hardware="${hardware}" p="20" parent="main1_link" tf_prefix="${tf_prefix}"></xacro:ee_link>
|
||||
<!-- END robot description -->
|
||||
<xacro:rgbd parent="${tf_prefix}ee_link" tf_prefix="${tf_prefix}">
|
||||
<origin rpy="0.0 -1.57 0.0" xyz="0.0 0.0 0.0754"></origin>
|
||||
</xacro:rgbd>
|
||||
|
||||
<!-- Insert gripper in robot description if exist -->
|
||||
<!-- <xacro:if value="${gripper_name=='rbs_gripper'}"> -->
|
||||
<!-- <xacro:include filename="$(find rbs_gripper)/urdf/rbs_gripper_macro.xacro"></xacro:include> -->
|
||||
<!-- <xacro:rbs_gripper gripper_name="rbs_gripper" hardware="${hardware}"
|
||||
parent="${tf_prefix}tool0" tf_prefix="${tf_prefix}"> -->
|
||||
<!-- <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"></origin> -->
|
||||
<!-- </xacro:rbs_gripper> -->
|
||||
<!-- </xacro:if> -->
|
||||
|
||||
<!-- If hardware == gazebo so insert additional parameters for it -->
|
||||
<xacro:if value="${hardware=='gazebo'}">
|
||||
<!-- <xacro:fts link="tool0" name="fts_sensor" tf_prefix="${tf_prefix}"></xacro:fts> -->
|
||||
<gazebo>
|
||||
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
|
||||
<parameters>$(arg simulation_controllers)</parameters>
|
||||
<ros>
|
||||
<namespace>${namespace}</namespace>
|
||||
</ros>
|
||||
</plugin>
|
||||
<plugin filename="VacuumGripper" name="rbs_mill_assist::VacuumGripper">
|
||||
<link_name>ee_link</link_name>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</xacro:if>
|
||||
<gazebo reference="ee_link">
|
||||
<sensor name='sensor_contact' type='contact'>
|
||||
<contact>
|
||||
<collision>ee_link_collision</collision>
|
||||
</contact>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
</robot>
|
Loading…
Add table
Add a link
Reference in a new issue