Skip to content
Snippets Groups Projects
Commit 5956b98b authored by Shaun Edwards's avatar Shaun Edwards
Browse files

Merge pull request #7 from ipa-mdl/gazebo_fix

Gazebo fixes
parents b993a8f4 a374261e
No related merge requests found
...@@ -4,17 +4,15 @@ ...@@ -4,17 +4,15 @@
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"> xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
<gazebo> <gazebo>
<controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so"> <plugin name="gazebo_ros_controller_manager" filename="libgazebo_ros_controller_manager.so">
<alwaysOn>true</alwaysOn> <alwaysOn>true</alwaysOn>
<updateRate>1000.0</updateRate> <updateRate>1000.0</updateRate>
<interface:audio name="gazebo_ros_controller_manager_dummy_iface" /> </plugin>
</controller:gazebo_ros_controller_manager>
<controller:gazebo_ros_power_monitor name="gazebo_ros_power_monitor_controller" plugin="libgazebo_ros_power_monitor.so"> <plugin name="gazebo_ros_power_monitor_controller" filename="libgazebo_ros_power_monitor.so">
<alwaysOn>true</alwaysOn> <alwaysOn>true</alwaysOn>
<updateRate>1.0</updateRate> <updateRate>1.0</updateRate>
<timeout>5</timeout> <timeout>5</timeout>
<interface:audio name="power_monitor_dummy_interface" />
<powerStateTopic>power_state</powerStateTopic> <powerStateTopic>power_state</powerStateTopic>
<powerStateRate>10.0</powerStateRate> <powerStateRate>10.0</powerStateRate>
<fullChargeCapacity>87.78</fullChargeCapacity> <fullChargeCapacity>87.78</fullChargeCapacity>
...@@ -22,7 +20,7 @@ ...@@ -22,7 +20,7 @@
<chargeRate>525</chargeRate> <chargeRate>525</chargeRate>
<dischargeVoltage>15.52</dischargeVoltage> <dischargeVoltage>15.52</dischargeVoltage>
<chargeVoltage>16.41</chargeVoltage> <chargeVoltage>16.41</chargeVoltage>
</controller:gazebo_ros_power_monitor> </plugin>
</gazebo> </gazebo>
</robot> </robot>
...@@ -5,43 +5,42 @@ ...@@ -5,43 +5,42 @@
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:xacro="http://ros.org/wiki/xacro"> xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="ur10_arm_gazebo" params="name"> <xacro:macro name="ur10_arm_gazebo" params="prefix">
<gazebo reference="${name}_base_link"> <gazebo reference="${prefix}base_link">
<material value="Gazebo/Grey" /> <material value="Gazebo/Grey" />
<turnGravityOff>false</turnGravityOff> <turnGravityOff>false</turnGravityOff>
</gazebo> </gazebo>
<gazebo reference="${name}_shoulder_pan_link"> <gazebo reference="${prefix}shoulder_pan_link">
<material value="Gazebo/Blue " /> <material value="Gazebo/Blue " />
<turnGravityOff>false</turnGravityOff> <turnGravityOff>false</turnGravityOff>
</gazebo> </gazebo>
<gazebo reference="${name}_shoulder_lift_link"> <gazebo reference="${prefix}shoulder_lift_link">
<material value="Gazebo/Grey" /> <material value="Gazebo/Grey" />
<turnGravityOff>false</turnGravityOff> <turnGravityOff>false</turnGravityOff>
</gazebo> </gazebo>
<gazebo reference="${name}_elbow_link"> <gazebo reference="${prefix}elbow_link">
<material value="Gazebo/Blue " /> <material value="Gazebo/Blue " />
<turnGravityOff>false</turnGravityOff> <turnGravityOff>false</turnGravityOff>
</gazebo> </gazebo>
<gazebo reference="${name}_wrist_1_link"> <gazebo reference="${prefix}wrist_1_link">
<material value="Gazebo/Grey" /> <material value="Gazebo/Grey" />
<turnGravityOff>false</turnGravityOff> <turnGravityOff>false</turnGravityOff>
</gazebo> </gazebo>
<gazebo reference="${name}_wrist_2_link"> <gazebo reference="${prefix}wrist_2_link">
<material value="Gazebo/Blue " /> <material value="Gazebo/Blue " />
<turnGravityOff>false</turnGravityOff> <turnGravityOff>false</turnGravityOff>
</gazebo> </gazebo>
<gazebo reference="${name}_wrist_1_link"> <gazebo reference="${prefix}wrist_3_link">
<material value="Gazebo/Grey" /> <material value="Gazebo/Grey" />
<turnGravityOff>false</turnGravityOff> <turnGravityOff>false</turnGravityOff>
</gazebo> </gazebo>
</xacro:macro> </xacro:macro>
</robot> </robot>
...@@ -5,41 +5,41 @@ ...@@ -5,41 +5,41 @@
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:xacro="http://ros.org/wiki/xacro"> xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="ur10_arm_transmission" params="name"> <xacro:macro name="ur10_arm_transmission" params="prefix">
<transmission name="shoulder_pan_trans" type="pr2_mechanism_model/SimpleTransmission"> <transmission name="shoulder_pan_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="shoulder_pan_motor"/> <actuator name="shoulder_pan_motor"/>
<joint name="${name}_shoulder_pan_joint"/> <joint name="${prefix}shoulder_pan_joint"/>
<mechanicalReduction>1</mechanicalReduction> <mechanicalReduction>1</mechanicalReduction>
</transmission> </transmission>
<transmission name="shoulder_lift_trans" type="pr2_mechanism_model/SimpleTransmission"> <transmission name="shoulder_lift_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="shoulder_lift_motor"/> <actuator name="shoulder_lift_motor"/>
<joint name="${name}_shoulder_lift_joint"/> <joint name="${prefix}shoulder_lift_joint"/>
<mechanicalReduction>1</mechanicalReduction> <mechanicalReduction>1</mechanicalReduction>
</transmission> </transmission>
<transmission name="elbow_trans" type="pr2_mechanism_model/SimpleTransmission"> <transmission name="elbow_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="elbow_motor"/> <actuator name="elbow_motor"/>
<joint name="${name}_elbow_joint"/> <joint name="${prefix}elbow_joint"/>
<mechanicalReduction>1</mechanicalReduction> <mechanicalReduction>1</mechanicalReduction>
</transmission> </transmission>
<transmission name="wrist_1_trans" type="pr2_mechanism_model/SimpleTransmission"> <transmission name="wrist_1_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="wrist_1_motor"/> <actuator name="wrist_1_motor"/>
<joint name="${name}_wrist_1_joint"/> <joint name="${prefix}wrist_1_joint"/>
<mechanicalReduction>1</mechanicalReduction> <mechanicalReduction>1</mechanicalReduction>
</transmission> </transmission>
<transmission name="wrist_2_trans" type="pr2_mechanism_model/SimpleTransmission"> <transmission name="wrist_2_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="wrist_2_motor"/> <actuator name="wrist_2_motor"/>
<joint name="${name}_wrist_2_joint"/> <joint name="${prefix}wrist_2_joint"/>
<mechanicalReduction>1</mechanicalReduction> <mechanicalReduction>1</mechanicalReduction>
</transmission> </transmission>
<transmission name="wrist_3_trans" type="pr2_mechanism_model/SimpleTransmission"> <transmission name="wrist_3_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="wrist_3_motor"/> <actuator name="wrist_3_motor"/>
<joint name="${name}_wrist_3_joint"/> <joint name="${prefix}wrist_3_joint"/>
<mechanicalReduction>1</mechanicalReduction> <mechanicalReduction>1</mechanicalReduction>
</transmission> </transmission>
......
...@@ -264,8 +264,8 @@ ...@@ -264,8 +264,8 @@
</inertial> </inertial>
</link> </link>
<xacro:ur10_arm_transmission name="${prefix}" /> <xacro:ur10_arm_transmission prefix="${prefix}" />
<xacro:ur10_arm_gazebo name="${prefix}" /> <xacro:ur10_arm_gazebo prefix="${prefix}" />
</xacro:macro> </xacro:macro>
</robot> </robot>
...@@ -252,7 +252,7 @@ center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0, ...@@ -252,7 +252,7 @@ center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0,
<link name="${prefix}ee_link" /> <link name="${prefix}ee_link" />
<xacro:ur_arm_transmission name="${prefix}" /> <xacro:ur5_arm_transmission name="${prefix}" />
<gazebo reference="universal_robot"> <gazebo reference="universal_robot">
<material>Gazebo/Blue</material> <material>Gazebo/Blue</material>
......
arm_controller: arm_controller:
type: robot_mechanism_controllers/JointTrajectoryActionController type: robot_mechanism_controllers/JointTrajectoryActionController
joints: joints:
- arm_shoulder_pan_joint - shoulder_pan_joint
- arm_shoulder_lift_joint - shoulder_lift_joint
- arm_elbow_joint - elbow_joint
- arm_wrist_1_joint - wrist_1_joint
- arm_wrist_2_joint - wrist_2_joint
- arm_wrist_3_joint - wrist_3_joint
gains: gains:
arm_shoulder_pan_joint: shoulder_pan_joint:
p: 2000.0 p: 2000.0
d: 0.0 d: 0.0
arm_shoulder_lift_joint: shoulder_lift_joint:
p: 2000.0 p: 2000.0
d: 0.0 d: 0.0
arm_elbow_joint: elbow_joint:
p: 2000.0 p: 2000.0
d: 0.0 d: 0.0
arm_wrist_1_joint: wrist_1_joint:
p: 2020.0 p: 2020.0
d: 0.0 d: 0.0
arm_wrist_2_joint: wrist_2_joint:
p: 2030.0 p: 2030.0
d: 0.0 d: 0.0
arm_wrist_3_joint: wrist_3_joint:
p: 2050.0 p: 2050.0
d: 0.0 d: 0.0
arm_joint_trajectory_action_node: arm_joint_trajectory_action_node:
joints: joints:
- arm_shoulder_pan_joint - shoulder_pan_joint
- arm_shoulder_lift_joint - shoulder_lift_joint
- arm_elbow_joint - elbow_joint
- arm_wrist_1_joint - wrist_1_joint
- arm_wrist_2_joint - wrist_2_joint
- arm_wrist_3_joint - wrist_3_joint
constraints: constraints:
goal_time: 0.6 goal_time: 0.6
arm_shoulder_pan_joint: shoulder_pan_joint:
goal: 0.05 goal: 0.05
arm_shoulder_lift_joint: shoulder_lift_joint:
goal: 0.05 goal: 0.05
arm_elbow_joint: elbow_joint:
goal: 0.05 goal: 0.05
arm_wrist_1_joint: wrist_1_joint:
goal: 0.05 goal: 0.05
arm_wrist_2_joint: wrist_2_joint:
goal: 0.05 goal: 0.05
arm_wrist_3_joint: wrist_3_joint:
goal: 0.05 goal: 0.05
arm_controller: arm_controller:
type: robot_mechanism_controllers/JointTrajectoryActionController type: robot_mechanism_controllers/JointTrajectoryActionController
joints: joints:
- arm_shoulder_pan_joint - shoulder_pan_joint
- arm_shoulder_lift_joint - shoulder_lift_joint
- arm_elbow_joint - elbow_joint
- arm_wrist_1_joint - wrist_1_joint
- arm_wrist_2_joint - wrist_2_joint
- arm_wrist_3_joint - wrist_3_joint
gains: gains:
arm_shoulder_pan_joint: shoulder_pan_joint:
p: 2000.0 p: 2000.0
d: 0.0 d: 0.0
arm_shoulder_lift_joint: shoulder_lift_joint:
p: 2000.0 p: 2000.0
d: 0.0 d: 0.0
arm_elbow_joint: elbow_joint:
p: 2000.0 p: 2000.0
d: 0.0 d: 0.0
arm_wrist_1_joint: wrist_1_joint:
p: 2020.0 p: 2020.0
d: 0.0 d: 0.0
arm_wrist_2_joint: wrist_2_joint:
p: 2030.0 p: 2030.0
d: 0.0 d: 0.0
arm_wrist_3_joint: wrist_3_joint:
p: 2050.0 p: 2050.0
d: 0.0 d: 0.0
arm_joint_trajectory_action_node: arm_joint_trajectory_action_node:
joints: joints:
- arm_shoulder_pan_joint - shoulder_pan_joint
- arm_shoulder_lift_joint - shoulder_lift_joint
- arm_elbow_joint - elbow_joint
- arm_wrist_1_joint - wrist_1_joint
- arm_wrist_2_joint - wrist_2_joint
- arm_wrist_3_joint - wrist_3_joint
constraints: constraints:
goal_time: 0.6 goal_time: 0.6
arm_shoulder_pan_joint: shoulder_pan_joint:
goal: 0.05 goal: 0.05
arm_shoulder_lift_joint: shoulder_lift_joint:
goal: 0.05 goal: 0.05
arm_elbow_joint: elbow_joint:
goal: 0.05 goal: 0.05
arm_wrist_1_joint: wrist_1_joint:
goal: 0.05 goal: 0.05
arm_wrist_2_joint: wrist_2_joint:
goal: 0.05 goal: 0.05
arm_wrist_3_joint: wrist_3_joint:
goal: 0.05 goal: 0.05
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment