Skip to content

Commit

Permalink
Merge pull request #2 from cambel/update-robot-descriptions
Browse files Browse the repository at this point in the history
Update robot descriptions
  • Loading branch information
felixvd authored Oct 27, 2021
2 parents 75e1812 + 2d41d58 commit f684f21
Show file tree
Hide file tree
Showing 15 changed files with 410 additions and 87 deletions.
8 changes: 4 additions & 4 deletions .gitmodules
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
[submodule "gitlab-wiki"]
path = gitlab-wiki
url = https://github.com/o2ac/o2ac-ur/wiki
[submodule "github-wiki"]
path = github-wiki
url = https://github.com/o2ac/o2ac-ur.wiki.git
[submodule "underlay_ws/src/moveit"]
path = underlay_ws/src/moveit
url = http://gitlab+deploy-token-440693:vMpkB_8thDeUCDa4oKNr@gitlab.com/o2ac/moveit.git
Expand Down Expand Up @@ -30,4 +30,4 @@
url = http://gitlab+deploy-token-440688:eYTsSmM4yjjtN5eXCx2C@gitlab.com/o2ac/third-party/ur_python_utilities.git
[submodule "underlay_ws/src/third_party/ur_drivers"]
path = underlay_ws/src/third_party/ur_drivers
url = https://github.com/felixvd/Universal_Robots_ROS_Driver.git
url = https://github.com/felixvd/Universal_Robots_ROS_Driver.git
2 changes: 1 addition & 1 deletion catkin_ws/src/o2ac_gazebo/launch/controller_utils.launch
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
args="pub /calibrated std_msgs/Bool true" />

<!-- joint_state_controller -->
<rosparam file="$(find ur_gazebo)/controller/joint_state_controller.yaml" command="load"/>
<rosparam file="$(find o2ac_gazebo)/controller/joint_state_controller.yaml" command="load"/>
<node name="joint_state_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn joint_state_controller" respawn="false" output="screen"/>

</launch>
4 changes: 3 additions & 1 deletion catkin_ws/src/o2ac_gazebo/launch/o2ac_gazebo.launch
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,9 @@

<!-- send robot urdf to param server -->
<!-- In this case the "robot" is the ensemble of the 2 arms -->
<include file="$(find o2ac_scene_description)/launch/base_scene_upload.launch" />
<include file="$(find o2ac_scene_description)/launch/base_scene_upload.launch">
<arg name="gazebo" value="true"/>
</include>

<!-- push robot_description to factory and spawn robot in gazebo -->
<!-- Initial pose modified on 9 July 2018 -->
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,80 @@
# Joints limits
#
# Sources:
#
# - UR5 User Manual, Universal Robots, UR5/CB3, Version 3.13
# https://s3-eu-west-1.amazonaws.com/ur-support-site/69371/99202_UR5_User_Manual_en_Global.pdf
# - Support > Articles > UR articles > Max. joint torques
# https://www.universal-robots.com/articles/ur-articles/max-joint-torques
# retrieved: 2020-06-16, last modified: 2020-06-09
joint_limits:
shoulder_pan:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 150.0
max_position: !degrees 180.0
max_velocity: !degrees 180.0
min_position: !degrees -180.0
shoulder_lift:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 150.0
max_position: !degrees 180.0
max_velocity: !degrees 180.0
min_position: !degrees -180.0
elbow_joint:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 150.0
# we artificially limit this joint to half its actual joint position limit
# to avoid (MoveIt/OMPL) planning problems, as due to the physical
# construction of the robot, it's impossible to rotate the 'elbow_joint'
# over more than approx +- 1 pi (the shoulder lift joint gets in the way).
#
# This leads to planning problems as the search space will be divided into
# two sections, with no connections from one to the other.
#
# Refer to https://github.com/ros-industrial/universal_robot/issues/265 for
# more information.
max_position: !degrees 180.0
max_velocity: !degrees 180.0
min_position: !degrees 0.0
wrist_1:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 28.0
max_position: !degrees 180.0
max_velocity: !degrees 180.0
min_position: !degrees -270.0
wrist_2:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 28.0
max_position: !degrees 270.0
max_velocity: !degrees 180.0
min_position: !degrees -270.0
wrist_3:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 28.0
max_position: !degrees 360.0
max_velocity: !degrees 180.0
min_position: !degrees -360.0
Original file line number Diff line number Diff line change
@@ -0,0 +1,80 @@
# Joints limits
#
# Sources:
#
# - UR5 User Manual, Universal Robots, UR5/CB3, Version 3.13
# https://s3-eu-west-1.amazonaws.com/ur-support-site/69371/99202_UR5_User_Manual_en_Global.pdf
# - Support > Articles > UR articles > Max. joint torques
# https://www.universal-robots.com/articles/ur-articles/max-joint-torques
# retrieved: 2020-06-16, last modified: 2020-06-09
joint_limits:
shoulder_pan:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 150.0
max_position: !degrees 180.0
max_velocity: !degrees 180.0
min_position: !degrees -180.0
shoulder_lift:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 150.0
max_position: !degrees 180.0
max_velocity: !degrees 180.0
min_position: !degrees -180.0
elbow_joint:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 150.0
# we artificially limit this joint to half its actual joint position limit
# to avoid (MoveIt/OMPL) planning problems, as due to the physical
# construction of the robot, it's impossible to rotate the 'elbow_joint'
# over more than approx +- 1 pi (the shoulder lift joint gets in the way).
#
# This leads to planning problems as the search space will be divided into
# two sections, with no connections from one to the other.
#
# Refer to https://github.com/ros-industrial/universal_robot/issues/265 for
# more information.
max_position: !degrees 180.0
max_velocity: !degrees 180.0
min_position: !degrees 0.0
wrist_1:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 28.0
max_position: !degrees 180.0
max_velocity: !degrees 180.0
min_position: !degrees -270.0
wrist_2:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 28.0
max_position: !degrees 270.0
max_velocity: !degrees 180.0
min_position: !degrees -270.0
wrist_3:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 28.0
max_position: !degrees 360.0
max_velocity: !degrees 180.0
min_position: !degrees -360.0
Original file line number Diff line number Diff line change
@@ -1,8 +1,13 @@
<?xml version="1.0"?>
<launch>
<arg name="taskboard" default="false"/>
<arg name="gazebo" default="false"/>
<!-- This file is used by MoveIt. -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find o2ac_scene_description)/urdf/base_scene.urdf.xacro' taskboard:=$(arg taskboard)" />
<param name="robot_description" command="$(find xacro)/xacro --inorder
'$(find o2ac_scene_description)/urdf/base_scene.urdf.xacro'
taskboard:=$(arg taskboard)
gazebo:=$(arg gazebo)
" />

<!-- These args are declared so the UR upload script does not complain. -->
<arg name="limited" default=""/>
Expand Down
79 changes: 57 additions & 22 deletions catkin_ws/src/o2ac_scene_description/urdf/base_scene.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
name="o2ac_base_scene" >
<!-- This is the main file defining our robot arrangement and physical environment. -->
<xacro:arg name="taskboard" default="false"/>
<xacro:arg name="gazebo" default="false"/>

<!-- Calibration parameters -->
<xacro:property name="a_bot_x" value="-0.005"/> <!-- Positive moves back -->
<xacro:property name="a_bot_y" value="-0.527"/> <!-- Positive moves left -->
Expand All @@ -19,11 +21,19 @@
<xacro:property name="bots_z" value="0.75"/>

<!-- common stuff -->
<xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />
<!-- <xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />
<xacro:include filename="$(find ur_e_description)/urdf/ur5e.urdf.xacro" /> -->

<xacro:if value="$(arg gazebo)">
<xacro:include filename="$(find o2ac_scene_description)/urdf/ur_gazebo.xacro"/>
</xacro:if>
<xacro:unless value="$(arg gazebo)">
<xacro:include filename="$(find ur_description)/urdf/inc/ur_macro.xacro"/>
</xacro:unless>

<!-- <xacro:include filename="$(find o2ac_gazebo)/urdf/o2ac_base_scene.gazebo" /> -->

<!-- robots -->
<xacro:include filename="$(find ur_e_description)/urdf/ur5e.urdf.xacro" />

<!-- Scene items (trays, columns, tools, jigs, feeders...) -->
<xacro:include filename="$(find o2ac_scene_description)/urdf/o2ac_environment.xacro" />
Expand Down Expand Up @@ -62,31 +72,56 @@
<origin xyz="0.0 0.0 ${bots_z-robot_mount_plate+workplate_h}" rpy="0.0 0.0 0.0" />
</joint>

<xacro:ur5e_robot prefix="a_bot_" joint_limited="true"
shoulder_pan_lower_limit="${-pi}" shoulder_pan_upper_limit="${pi}"
shoulder_lift_lower_limit="${-pi}" shoulder_lift_upper_limit="${pi}"
elbow_joint_lower_limit="0.0" elbow_joint_upper_limit="${pi}"
wrist_1_lower_limit="${-pi*1.5}" wrist_1_upper_limit="${pi}"
wrist_2_lower_limit="${-pi*1.5}" wrist_2_upper_limit="${pi*1.5}"
wrist_3_lower_limit="${-2*pi}" wrist_3_upper_limit="${2*pi}"
transmission_hw_interface="hardware_interface/PositionJointInterface"
safety_limits="false" safety_pos_margin="0.15"
<xacro:ur_robot
prefix="a_bot_"
joint_limits_parameters_file='$(find o2ac_scene_description)/config/a_bot_joint_limits.yaml'
kinematics_parameters_file='$(find o2ac_scene_description)/config/a_bot_calibration.yaml'
physical_parameters_file='$(find ur_description)/config/ur5e/physical_parameters.yaml'
visual_parameters_file='$(find ur_description)/config/ur5e/visual_parameters.yaml'
transmission_hw_interface='hardware_interface/PositionJointInterface'
safety_limits="false"
safety_pos_margin="0.15"
safety_k_position="20"
kinematics_file="${load_yaml('$(find o2ac_scene_description)/config/a_bot_calibration.yaml')}"
/>
<link name="a_bot_ee_link">
<collision>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.01 0 0"/>
</collision>
</link>
<joint name="a_bot_ee_fixed_joint" type="fixed">
<parent link="a_bot_wrist_3_link"/>
<child link="a_bot_ee_link"/>
<origin rpy="0.0 -1.57079632679 1.57079632679" xyz="0.0 0.0 0.0"/>
</joint>

<xacro:ur5e_robot prefix="b_bot_" joint_limited="true"
shoulder_pan_lower_limit="${-pi}" shoulder_pan_upper_limit="${pi}"
shoulder_lift_lower_limit="${-pi}" shoulder_lift_upper_limit="${pi}"
elbow_joint_lower_limit="0.0" elbow_joint_upper_limit="${pi}"
wrist_1_lower_limit="${-pi*1.5}" wrist_1_upper_limit="${pi}"
wrist_2_lower_limit="${-pi*1.5}" wrist_2_upper_limit="${pi*1.5}"
wrist_3_lower_limit="${-2*pi}" wrist_3_upper_limit="${2*pi}"
transmission_hw_interface="hardware_interface/PositionJointInterface"
safety_limits="false" safety_pos_margin="0.15"
<xacro:ur_robot
prefix="b_bot_"
joint_limits_parameters_file='$(find o2ac_scene_description)/config/b_bot_joint_limits.yaml'
kinematics_parameters_file='$(find o2ac_scene_description)/config/b_bot_calibration.yaml'
physical_parameters_file='$(find ur_description)/config/ur5e/physical_parameters.yaml'
visual_parameters_file='$(find ur_description)/config/ur5e/visual_parameters.yaml'
transmission_hw_interface='hardware_interface/PositionJointInterface'
safety_limits="false"
safety_pos_margin="0.15"
safety_k_position="20"
kinematics_file="${load_yaml('$(find o2ac_scene_description)/config/b_bot_calibration.yaml')}"
/>

<link name="b_bot_ee_link">
<collision>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.01 0 0"/>
</collision>
</link>
<joint name="b_bot_ee_fixed_joint" type="fixed">
<parent link="b_bot_wrist_3_link"/>
<child link="b_bot_ee_link"/>
<origin rpy="0.0 -1.57079632679 1.57079632679" xyz="0.0 0.0 0.0"/>
</joint>

<link name="o2ac_ground" />

Expand Down
33 changes: 33 additions & 0 deletions catkin_ws/src/o2ac_scene_description/urdf/ur_gazebo.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="ur_robot_gazebo">

<!-- Convenient wrapper for ur-gazebo macro -->

<xacro:macro name="ur_robot" params="
prefix
joint_limits_parameters_file
kinematics_parameters_file
physical_parameters_file
visual_parameters_file
transmission_hw_interface:=hardware_interface/PositionJointInterface
safety_limits:=false safety_pos_margin:=0.15 safety_k_position:=20">

<xacro:include filename="$(find o2ac_scene_description)/urdf/ur_gazebo_macro.xacro"/>

<!-- Instantiate the Gazebo robot and pass it all the required arguments. -->
<xacro:ur_robot_gazebo
prefix="${prefix}"
joint_limits_parameters_file="${joint_limits_parameters_file}"
kinematics_parameters_file="${kinematics_parameters_file}"
physical_parameters_file="${physical_parameters_file}"
visual_parameters_file="${visual_parameters_file}"
transmission_hw_interface="${transmission_hw_interface}"
safety_limits="${safety_limits}"
safety_pos_margin="${safety_pos_margin}"
safety_k_position="${safety_k_position}"
/>

</xacro:macro>


</robot>
Loading

0 comments on commit f684f21

Please sign in to comment.