Skip to content
Permalink
Browse files

adding the ability to use the EffortJointInterface for the ur10

  • Loading branch information...
wjwwood committed Jul 19, 2016
1 parent 1dafa8a commit 6c4318a3c0024be31e919cdd30741456f4eaf778
@@ -1,7 +1,8 @@
<?xml version="1.0"?>
<launch>
<arg name="limited" default="false"/>

<param unless="$(arg limited)" name="robot_description" command="$(find xacro)/xacro.py '$(find ur_description)/urdf/ur10_robot.urdf.xacro'" />
<param if="$(arg limited)" name="robot_description" command="$(find xacro)/xacro.py '$(find ur_description)/urdf/ur10_joint_limited_robot.urdf.xacro'" />
<arg name="effort_interface" default="false"/>

<param unless="$(arg limited)" name="robot_description" command="$(find xacro)/xacro.py '$(find ur_description)/urdf/ur10_robot.urdf.xacro' use_effort_interface:=$(arg effort_interface)" />
<param if="$(arg limited)" name="robot_description" command="$(find xacro)/xacro.py '$(find ur_description)/urdf/ur10_joint_limited_robot.urdf.xacro' use_effort_interface:=$(arg effort_interface)" />
</launch>
@@ -1,12 +1,12 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

<xacro:macro name="ur_arm_transmission" params="prefix">
<xacro:macro name="ur_arm_transmission" params="prefix joint_interface:=PositionJointInterface">

<transmission name="${prefix}shoulder_pan_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}shoulder_pan_joint">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<hardwareInterface>${joint_interface}</hardwareInterface>
</joint>
<actuator name="${prefix}shoulder_pan_motor">
<mechanicalReduction>1</mechanicalReduction>
@@ -16,7 +16,7 @@
<transmission name="${prefix}shoulder_lift_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}shoulder_lift_joint">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<hardwareInterface>${joint_interface}</hardwareInterface>
</joint>
<actuator name="${prefix}shoulder_lift_motor">
<mechanicalReduction>1</mechanicalReduction>
@@ -26,7 +26,7 @@
<transmission name="${prefix}elbow_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}elbow_joint">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<hardwareInterface>${joint_interface}</hardwareInterface>
</joint>
<actuator name="${prefix}elbow_motor">
<mechanicalReduction>1</mechanicalReduction>
@@ -36,7 +36,7 @@
<transmission name="${prefix}wrist_1_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}wrist_1_joint">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<hardwareInterface>${joint_interface}</hardwareInterface>
</joint>
<actuator name="${prefix}wrist_1_motor">
<mechanicalReduction>1</mechanicalReduction>
@@ -46,7 +46,7 @@
<transmission name="${prefix}wrist_2_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}wrist_2_joint">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<hardwareInterface>${joint_interface}</hardwareInterface>
</joint>
<actuator name="${prefix}wrist_2_motor">
<mechanicalReduction>1</mechanicalReduction>
@@ -56,7 +56,7 @@
<transmission name="${prefix}wrist_3_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}wrist_3_joint">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<hardwareInterface>${joint_interface}</hardwareInterface>
</joint>
<actuator name="${prefix}wrist_3_motor">
<mechanicalReduction>1</mechanicalReduction>
@@ -6,6 +6,8 @@
Contributers: Jimmy Da Silva, Ajit Krisshna N L, Muhammad Asif Rana
-->

<xacro:arg name="use_effort_interface" defalut="false" />

<xacro:include filename="$(find ur_description)/urdf/ur.transmission.xacro" />
<xacro:include filename="$(find ur_description)/urdf/ur.gazebo.xacro" />

@@ -297,7 +299,12 @@
</collision>
</link>

<xacro:ur_arm_transmission prefix="${prefix}" />
<xacro:if value="$(arg use_effort_interface)">
<xacro:ur_arm_transmission prefix="${prefix}" joint_interface="EffortJointInterface" />
</xacro:if>
<xacro:unless value="$(arg use_effort_interface)">
<xacro:ur_arm_transmission prefix="${prefix}" joint_interface="PositionJointInterface" />
</xacro:unless>
<xacro:ur_arm_gazebo prefix="${prefix}" />

<!-- ROS base_link to UR 'Base' Coordinates transform -->
@@ -0,0 +1,28 @@
arm_controller:
type: effort_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
gains:
shoulder_pan_joint: {p: 10000, d: 100, i: 5, i_clamp: 1}
shoulder_lift_joint: {p: 10000, d: 100, i: 5, i_clamp: 1}
elbow_joint: {p: 10000, d: 100, i: 5, i_clamp: 1}
wrist_1_joint: {p: 10000, d: 100, i: 5, i_clamp: 1}
wrist_2_joint: {p: 10000, d: 100, i: 5, i_clamp: 1}
wrist_3_joint: {p: 10000, d: 100, i: 5, i_clamp: 1}
stop_trajectory_duration: 0.5
state_publish_rate: 25
action_monitor_rate: 10
@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<launch>
<arg name="limited" default="false"/>
<arg name="paused" default="false"/>
<arg name="gui" default="true"/>

<!-- startup simulated world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" default="worlds/empty.world"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="gui" value="$(arg gui)"/>
</include>

<!-- send robot urdf to param server -->
<include file="$(find ur_description)/launch/ur10_upload.launch">
<arg name="limited" value="$(arg limited)"/>
<arg name="effort_interface" value="true"/>
</include>

<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
args="-urdf -param robot_description -model robot -z 0.1"
respawn="false" output="screen" />

<include file="$(find ur_gazebo)/launch/controller_utils.launch"/>

<rosparam file="$(find ur_gazebo)/controller/arm_controller_ur10_effort.yaml" command="load"/>
<node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager"
args="spawn arm_controller" respawn="false" output="screen"/>

</launch>

4 comments on commit 6c4318a

@gavanderhoorn

This comment has been minimized.

Copy link
Member

replied Jul 19, 2016

#227? :)

@wjwwood

This comment has been minimized.

Copy link
Author

replied Jul 19, 2016

Oh, didn't see that... Man I searched the pr's and everything... Maybe I'll use that branch instead, but that pr doesn't have any pid values that I can see, but they are required for the effort controller to work properly. While our pid values aren't reasonable yet, at least it somewhat works. Thanks for pointing it out to me @gavanderhoorn.

Looking at the history #161, I don't completely agree that it is strictly better to use the position interface with Gazebo. As you may be aware, using "set position" or "set velocity" in gazebo forces the physics simulation to try and accomplish this, which results in strange behavior. It's actually preferable, according to the Gazebo team members I'm working with, to always use "set force" in Gazebo because that allows a more normal interaction when the commanded force of an arm joint puts the arm in contact with the environment. So that's why we changed, in our branch, to use the Effort controller.

At least making it optional seems reasonable to me (which was the approach I took in my branch too).

/cc @jonbinney @caguero @dhood

@jonbinney

This comment has been minimized.

Copy link
Contributor

replied Jul 19, 2016

Right, #161 appears to add and optional effort Interface but not the configuration for a JointTrajectoryController that this branch has. I can understand wanting to have the option for a position interface - sometimes people don't care about simulating the dynamics of the arm and don't want to have to worry about tuning the PID parameters to get the tracking performance they want. But in general I think the effort interface is the right way to go.

@gavanderhoorn

This comment has been minimized.

Copy link
Member

replied Jul 20, 2016

Looking at the history #161, I don't completely agree that it is strictly better to use the position interface with Gazebo. As you may be aware, using "set position" or "set velocity" in gazebo forces the physics simulation to try and accomplish this, which results in strange behavior. It's actually preferable, according to the Gazebo team members I'm working with, to always use "set force" in Gazebo because that allows a more normal interaction when the commanded force of an arm joint puts the arm in contact with the environment. So that's why we changed, in our branch, to use the Effort controller.

Yes, I am aware, but it's partly a logistical issue why the current packages use joint position controllers: we have many many robot support packages in ROS-Industrial, and as all of them are for different variants / series, we'd be tweaking gains forever. The position controllers don't need the gains, and as @ipa-fxm wrote in #161, position control is also what almost all industrial robots support.

Anecdotal - and not 100% relevant, but I've actually had people 'complain' that they spent considerable time creating controllers / software for their simulated robot, only to find out that the real hw was incapable of supporting the same interface.

In addition, the dynamic behaviour of the simulated manipulators will never come close to the real robots, as we don't have the required information to make accurate models of the arms.

At least making it optional seems reasonable to me

Yes, that is probably a good compromise.

Please sign in to comment.
You can’t perform that action at this time.