sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-28 18:32:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

GEAR Environment, ROS Package, File Cleanup

Browse Source 
- Remove gazebo_models folder
- Remove simulation_env folder
- Remove world folder
- Create rrrobot ROS package w/ Node
- Move bash scripts to scripts folder in rrrobot package
- Create rrrobot_node.cpp from GEAR example in documentation
- Add yaml files for sensor and environment configuration
This commit is contained in:
Sravan Balaji
2020-04-18 14:02:14 -04:00
parent aa19a69917
commit 1f3e445903
26 changed files with 590 additions and 1425 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/arm_joint_and_link_definitions.jpg
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.9 MiB

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL
View File

Binary file not shown.

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL
View File

Binary file not shown.

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL
View File

Binary file not shown.

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL
View File

Binary file not shown.

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL
View File

Binary file not shown.

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL
View File

Binary file not shown.

BIN
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL
View File

Binary file not shown.

11
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.config
View File

@@ -1,11 +0,0 @@
<?xml version="1.0" ?>
<model>
<name>Fanuc_robot_arm</name>
<version>1.0</version>
<sdf version="1.6">model.sdf</sdf>
<author>
<name></name>
<email></email>
</author>
<description>This was created using parts from a CAD model on GrabCAD</description>
</model>

905
rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf
View File

@@ -1,905 +0,0 @@
<?xml version='1.0'?>
<sdf version='1.6'>
<model name='Fanuc_robotic_arm'>
<link name='base'>
<pose frame=''>0 0 0 3.14159 -0 0</pose>
<inertial>
<mass>86.082</mass>
<inertia>
<ixx>4.3404</ixx>
<ixy>0</ixy>
<ixz>0.0020207</ixz>
<iyy>2.4866</iyy>
<iyz>0</iyz>
<izz>6.1574</izz>
</inertia>
<pose frame=''>-0.003895 -0.062844 -0.11068 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<link name='link_1'>
<pose frame=''>0.001715 0.003779 0.661683 1.5708 -0 0</pose>
<inertial>
<mass>266.27</mass>
<inertia>
<ixx>279.51</ixx>
<ixy>0</ixy>
<ixz>4.6477</ixz>
<iyy>43.386</iyy>
<iyz>34.265</iyz>
<izz>255.26</izz>
</inertia>
<pose frame=''>0.093634 -0.51398 -0.1614 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<link name='link_3'>
<pose frame=''>-0.00025 -0.94659 5.45119 1.5708 -0 0</pose>
<inertial>
<mass>63.612</mass>
<inertia>
<ixx>6.6124</ixx>
<ixy>0.099584</ixy>
<ixz>0</ixz>
<iyy>5.9295</iyy>
<iyz>0</iyz>
<izz>1.9517</izz>
</inertia>
<pose frame=''>0.00756 -3.4862 -0.40714 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<link name='link_2'>
<pose frame=''>-0.068455 0.275759 0.315754 -1.571 -0 0</pose>
<inertial>
<mass>60.795</mass>
<inertia>
<ixx>8.1939</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.76659</iyy>
<iyz>0</iyz>
<izz>8.0226</izz>
</inertia>
<pose frame=''>0.25344 -1.009 -0.57114 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<link name='link_4'>
<pose frame=''>-0.000257 -3.97477 2.07064 -1.5708 7e-06 3.14159</pose>
<inertial>
<mass>6.6264</mass>
<inertia>
<ixx>0.089718</ixx>
<ixy>0</ixy>
<ixz>0.017666</ixz>
<iyy>0.092881</iyy>
<iyz>3e-08</iyz>
<izz>0.028564</izz>
</inertia>
<pose frame=''>-0.028814 0 -2.454 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<link name='link_5'>
<pose frame=''>0.000743 -4.767 2.06498 -1.5708 7e-06 3.14159</pose>
<inertial>
<mass>5.1229</mass>
<inertia>
<ixx>0.038195</ixx>
<ixy>4e-07</ixy>
<ixz>0</ixz>
<iyy>0.066675</iyy>
<iyz>6e-07</iyz>
<izz>0.04821</izz>
</inertia>
<pose frame=''>-0.010371 1e-06 -3.0669 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<link name='link_6'>
<pose frame=''>-2.06426 -5.37015 4.13075 1.88426 1.57078 0.313465</pose>
<inertial>
<mass>0.72893</mass>
<inertia>
<ixx>0.00131</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0012978</iyy>
<iyz>0</iyz>
<izz>0.0024341</izz>
</inertia>
<pose frame=''>2.0657 2.0642 -3.5441 0 -0 0</pose>
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<joint name='wrist_pivot' type='revolute'>
<parent>link_3</parent>
<child>link_4</child>
<pose frame=''>0 0 -2.7 0 -0 0</pose>
<axis>
<xyz>0 0 1</xyz>
<use_parent_model_frame>0</use_parent_model_frame>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-2000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<joint name='shoulder_joint' type='revolute'>
<parent>link_1</parent>
<child>link_2</child>
<pose frame=''>0 -0.41 -0.61 0 -0 0</pose>
<axis>
<xyz>1 0 0</xyz>
<use_parent_model_frame>0</use_parent_model_frame>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-2000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<joint name='elbow_joint' type='revolute'>
<parent>link_2</parent>
<child>link_3</child>
<pose frame=''>0 -3.65 -0.6 0 -0 0</pose>
<axis>
<xyz>1 0 0</xyz>
<use_parent_model_frame>0</use_parent_model_frame>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-2000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<joint name='wrist_joint' type='revolute'>
<parent>link_4</parent>
<child>link_5</child>
<pose frame=''>0 0 -3.15 0 -0 0</pose>
<axis>
<xyz>1 0 0</xyz>
<use_parent_model_frame>0</use_parent_model_frame>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-2000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<joint name='shoulder_pivot' type='revolute'>
<parent>base</parent>
<child>link_1</child>
<pose frame=''>0 0 0 0 -0 0</pose>
<axis>
<xyz>0 1 0</xyz>
<use_parent_model_frame>0</use_parent_model_frame>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-2000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<joint name='end_effector_pivot' type='revolute'>
<parent>link_5</parent>
<child>link_6</child>
<pose frame=''>2.065 2.065 -3.5 0 -0 0</pose>
<axis>
<xyz>0 0 1</xyz>
<use_parent_model_frame>0</use_parent_model_frame>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-2000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<joint name='world_fix' type='fixed'>
<parent>world</parent>
<child>base</child>
</joint>
<static>0</static>
<allow_auto_disable>0</allow_auto_disable>
<plugin name="joint_angles" filename="libarm_angles.so"/>
<plugin name="arm_control" filename="libarm_motors.so"/>
</model>
</sdf>

75
rrrobot_ws/src/rrrobot/CMakeLists.txt Normal file
View File

@@ -0,0 +1,75 @@
cmake_minimum_required(VERSION 2.8.3)
project(rrrobot)
find_package(catkin REQUIRED COMPONENTS
osrf_gear
roscpp
sensor_msgs
std_srvs
trajectory_msgs
)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
catkin_package()
###########
## Build ##
###########
include_directories(include ${catkin_INCLUDE_DIRS})
## Declare a C++ executable
add_executable(rrrobot_node src/rrrobot_node.cpp)
add_dependencies(rrrobot_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(rrrobot_node ${catkin_LIBRARIES})
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
install(PROGRAMS
script/rrrobot_node.py
script/tf2_example.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark executables and/or libraries for installation
install(TARGETS rrrobot_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES
config/sample_gear_conf.yaml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ariac_example.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

163
rrrobot_ws/src/rrrobot/config/rrrobot.yaml Normal file
View File

@@ -0,0 +1,163 @@
# Competition configuration options
options:
insert_models_over_bins: true # Whether or not to insert the models that are specified in models_over_bins
spawn_extra_models: true # Whether or not to spawn the models that are specified in models_to_spawn
gazebo_state_logging: true # Whether or not to generate a gazebo state log
belt_population_cycles: 5 # How many cycles to spawn parts on the conveyor
model_type_aliases: # Aliases for model types which can be used in the configuration file
order_part1: piston_rod_part # Wherever 'order_part1' is used in the configuration file, use 'piston_rod_part'
order_part2: gear_part
order_part3: pulley_part
visualize_drop_regions: false # Whether or not to visualize drop regions (world frame only)
time_limit: 500 # Maximum time allowed for the trial once started, in seconds
random_seed: 1 # Seed for the pseudo random number generator (used to randomize model names)
# Orders to announce during the trial
# orders:
# order_0:
# announcement_condition: time # Announce the order base on elapsed time
# announcement_condition_value: 0 # Time in seconds to wait before announcing the order
# shipment_count: 1 # How many of the same shipment are in the order
# destinations: [agv1] # Which agv the shipments must be delivered to
# # If specified it must be a list the same length as the shipment count
# # allowed values: agv1, agv2, any
# products: # List of products required to be in the kit
# part_0:
# type: order_part1 # Type of model required
# pose:
# xyz: [0.1, -0.15, 0] # Position required in the tray frame
# rpy: [0, 0, 'pi/2'] # Orientation required in the tray frame
# part_1:
# type: order_part1
# pose:
# xyz: [-0.1, -0.15, 0]
# rpy: [0, 0, 'pi/2']
# part_2:
# type: order_part2
# pose:
# xyz: [0.1, 0.15, 0]
# rpy: [0, 0, 0]
# part_3:
# type: order_part2
# pose:
# xyz: [-0.1, 0.15, 0]
# rpy: [0, 0, 0]
# order_1:
# announcement_condition: wanted_products # Announce the order when the boxes contain products from this order
# announcement_condition_value: 2
# shipment_count: 1
# destinations: [any]
# products:
# part_0:
# type: order_part3
# pose:
# xyz: [0.12, -0.2, 0]
# rpy: ['pi', 0, 0]
# part_1:
# type: order_part3
# pose:
# xyz: [-0.12, -0.2, 0]
# rpy: [0, 'pi', 0]
# part_2:
# type: order_part1
# pose:
# xyz: [0.15, 0.15, 0]
# rpy: [0, 0, 0]
# part_3:
# type: order_part2
# pose:
# xyz: [-0.15, 0.15, 0]
# rpy: [0, 0, 0]
# part_4:
# type: order_part2
# pose:
# rpy: [0, 'pi', 0]
# # Individual products that will be reported as faulty
# faulty_products:
# - piston_rod_part_57 # The piston rod part in the bins with randomized ID of 57
# - piston_rod_part_45
# Models to be inserted in the bins
models_over_bins:
bin1: # Name of the bin (bin1-bin6, as named in the environment simulation)
models: # List of models to insert
# gear_part: # Type of model to insert
# xyz_start: [0.1, 0.1, 0] # Origin of the first model to insert
# xyz_end: [0.5, 0.5, 0] # Origin of the last model to insert
# rpy: [0, 0, 'pi/4'] # Orientation of all models to insert
# num_models_x: 3 # How many models to insert along the x dimension
# num_models_y: 5 # How many models to insert along the y dimension
# bin5:
# models:
# gasket_part:
# xyz_start: [0.1, 0.1, 0.0]
# xyz_end: [0.5, 0.5, 0.0]
# rpy: [0, 0, 'pi/4']
# num_models_x: 2
# num_models_y: 3
# bin6:
# models:
# piston_rod_part:
# xyz_start: [0.1, 0.1, 0.0]
# xyz_end: [0.5, 0.5, 0.0]
# rpy: [0, 0, 'pi/4']
# num_models_x: 3
# num_models_y: 3
# # Models to be spawned in particular reference frames
# models_to_spawn:
# bin2::link: # Name of the reference frame
# models: # List of models to spawn
# piston_rod_part_1: # An arbitrary unique name of the model (will be randomized)
# type: piston_rod_part # Type of model (must be installed locally)
# pose:
# xyz: [0.2, 0.2, 0.75] # Co-ordinates of the model origin in the specified reference frame
# rpy: [0, 0, '-pi/2'] # Roll, pitch, yaw of the model in the specified reference frame
# # Models to be spawned on the conveyor belt
# belt_models:
# pulley_part: # name of part model to spawn
# 1.0: # Time in seconds after trial starts to spawn it
# pose:
# xyz: [0.0, 0.0, 0.1] # Coordinates relative to the start of the container to spawn the part
# rpy: [0, 0, 'pi/2'] # Roll, pitch, yaw of the model
# disk_part:
# 5.0:
# pose:
# xyz: [0.0, 0.0, 0.1]
# rpy: [0, 0, 'pi/2']
# # Drops from the vacuum gripper to be triggered at particular locations
# drops:
# drop_regions:
# above_agv_1:
# frame: agv1::kit_tray_1 # Frame the drop region/destination is specified in
# min: # Min corner of the bounding box that triggers a drop
# xyz: [-0.3, -0.3, 0.0]
# max: # Max corner of the bounding box that triggers a drop
# xyz: [0.3, 03, 0.5]
# destination: # Where to drop the part to
# xyz: [-0.35, 0.1, 0.15]
# rpy: [0, 0, 0.2]
# product_type_to_drop: gear_part
# above_agv_2:
# frame: agv2::kit_tray_2
# min:
# xyz: [-0.3, -0.3, 0.0]
# max:
# xyz: [0.3, 0.3, 0.5]
# destination:
# xyz: [0.15, 0.15, 0.15]
# rpy: [0, 0, -0.2]
# product_type_to_drop: pulley_part
# bin5_reachable:
# min:
# xyz: [0.0, 0.7, 0.7]
# max:
# xyz: [0.2, 1.60, 1.3]
# destination:
# xyz: [0.65, 1.15, 0.76]
# rpy: [0, 0, 0.5]
# product_type_to_drop: gasket_part

36
rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml Normal file
View File

@@ -0,0 +1,36 @@
sensors:
break_beam_1:
type: break_beam
pose:
xyz: [1.6, 2.25, 0.91]
rpy: [0, 0, 'pi']
proximity_sensor_1:
type: proximity_sensor
pose:
xyz: [0.95, 2.6, 0.92]
rpy: [0, 0, 0]
logical_camera_1:
type: logical_camera
pose:
xyz: [-0.3, 0.383, 1.27]
rpy: [0.2, 1.5707, 0]
logical_camera_2:
type: logical_camera
pose:
xyz: [0.3, 3.15, 1.5]
rpy: [0, 'pi/2', 0]
logical_camera_3:
type: logical_camera
pose:
xyz: [0.3, -3.15, 1.5]
rpy: [0, 'pi/2', 0]
depth_camera_1:
type: depth_camera
pose:
xyz: [-0.3, -0.383, 1.2]
rpy: [0, 1.5707, 0]
laser_profiler_1:
type: laser_profiler
pose:
xyz: [1.21, 4, 1.6]
rpy: ['-pi', 'pi/2', 'pi/2']

28
rrrobot_ws/src/rrrobot/launch/rrrobot.launch Normal file
View File

@@ -0,0 +1,28 @@
<launch>
<arg name="verbose" default="false" />
<arg unless="$(arg verbose)" name="verbose_args" value="" />
<arg if="$(arg verbose)" name="verbose_args" value="--verbose" />
<arg name="state_logging" default="false" />
<arg unless="$(arg state_logging)" name="state_logging_args" value="" />
<arg if="$(arg state_logging)" name="state_logging_args" value="--state-logging=true" />
<arg name="no_gui" default="false" />
<arg unless="$(arg no_gui)" name="gui_args" value="" />
<arg if="$(arg no_gui)" name="gui_args" value="--no-gui" />
<arg name="fill_demo_shipment" default="false" />
<arg unless="$(arg fill_demo_shipment)" name="fill_demo_shipment_args" value="" />
<arg if="$(arg fill_demo_shipment)" name="fill_demo_shipment_args" value="--fill-demo-shipment" />
<node name="ariac_sim" pkg="osrf_gear" type="gear.py"
args="--development-mode
$(arg verbose_args)
$(arg state_logging_args)
$(arg gui_args)
$(arg fill_demo_shipment_args)
--visualize-sensor-views
-f /app/rrrobot_ws/src/rrrobot/config/rrrobot.yaml
/app/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml
" required="true" output="screen" />
</launch>

22
rrrobot_ws/src/rrrobot/package.xml Normal file
View File

@@ -0,0 +1,22 @@
<?xml version="1.0"?>
<package format="2">
<name>rrrobot</name>
<version>0.1.0</version>
<description>RRRobot package</description>
<maintainer email="balajsra@umich.edu">Sravan Balaji</maintainer>
<maintainer email="chenxgu@umich.edu">Chenxi Gu</maintainer>
<maintainer email="thejakej@umich.edu">Jake Johnson</maintainer>
<maintainer email="dwitcpa@umich.edu">Derek Witcpalek</maintainer>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>osrf_gear</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>std_srvs</depend>
<depend>tf2_geometry_msgs</depend>
<depend>trajectory_msgs</depend>
</package>

5
rrrobot_ws/src/rrrobot/scripts/rrrobot_run.sh Normal file
View File

@@ -0,0 +1,5 @@
#!/bin/bash
sudo cp ../launch/rrrobot.launch /opt/ros/melodic/share/osrf_gear/launch
roslaunch osrf_gear rrrobot.launch

0
rrrobot_ws/src/gear/sample_run.sh → rrrobot_ws/src/rrrobot/scripts/sample_run.sh
View File

261
rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp Normal file
View File

@@ -0,0 +1,261 @@
// Copyright 2016 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// %Tag(FULLTEXT)%
// %Tag(INCLUDE_STATEMENTS)%
#include <algorithm>
#include <vector>
#include <ros/ros.h>
#include <osrf_gear/LogicalCameraImage.h>
#include <osrf_gear/Order.h>
#include <osrf_gear/Proximity.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <std_srvs/Trigger.h>
#include <trajectory_msgs/JointTrajectory.h>
// %EndTag(INCLUDE_STATEMENTS)%
// %Tag(START_COMP)%
/// Start the competition by waiting for and then calling the start ROS Service.
void start_competition(ros::NodeHandle & node) {
// Create a Service client for the correct service, i.e. '/ariac/start_competition'.
ros::ServiceClient start_client =
node.serviceClient<std_srvs::Trigger>("/ariac/start_competition");
// If it's not already ready, wait for it to be ready.
// Calling the Service using the client before the server is ready would fail.
if (!start_client.exists()) {
ROS_INFO("Waiting for the competition to be ready...");
start_client.waitForExistence();
ROS_INFO("Competition is now ready.");
}
ROS_INFO("Requesting competition start...");
std_srvs::Trigger srv; // Combination of the "request" and the "response".
start_client.call(srv); // Call the start Service.
if (!srv.response.success) { // If not successful, print out why.
ROS_ERROR_STREAM("Failed to start the competition: " << srv.response.message);
} else {
ROS_INFO("Competition started!");
}
}
// %EndTag(START_COMP)%
/// Example class that can hold state and provide methods that handle incoming data.
class MyCompetitionClass
{
public:
explicit MyCompetitionClass(ros::NodeHandle & node)
: current_score_(0), arm_1_has_been_zeroed_(false), arm_2_has_been_zeroed_(false)
{
// %Tag(ADV_CMD)%
arm_1_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(
"/ariac/arm1/arm/command", 10);
arm_2_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(
"/ariac/arm2/arm/command", 10);
// %EndTag(ADV_CMD)%
}
/// Called when a new message is received.
void current_score_callback(const std_msgs::Float32::ConstPtr & msg) {
if (msg->data != current_score_)
{
ROS_INFO_STREAM("Score: " << msg->data);
}
current_score_ = msg->data;
}
/// Called when a new message is received.
void competition_state_callback(const std_msgs::String::ConstPtr & msg) {
if (msg->data == "done" && competition_state_ != "done")
{
ROS_INFO("Competition ended.");
}
competition_state_ = msg->data;
}
/// Called when a new Order message is received.
void order_callback(const osrf_gear::Order::ConstPtr & order_msg) {
ROS_INFO_STREAM("Received order:\n" << *order_msg);
received_orders_.push_back(*order_msg);
}
// %Tag(CB_CLASS)%
/// Called when a new JointState message is received.
void arm_1_joint_state_callback(
const sensor_msgs::JointState::ConstPtr & joint_state_msg)
{
ROS_INFO_STREAM_THROTTLE(10,
"Joint States arm 1 (throttled to 0.1 Hz):\n" << *joint_state_msg);
// ROS_INFO_STREAM("Joint States:\n" << *joint_state_msg);
arm_1_current_joint_states_ = *joint_state_msg;
if (!arm_1_has_been_zeroed_) {
arm_1_has_been_zeroed_ = true;
ROS_INFO("Sending arm to zero joint positions...");
send_arm_to_zero_state(arm_1_joint_trajectory_publisher_);
}
}
void arm_2_joint_state_callback(
const sensor_msgs::JointState::ConstPtr & joint_state_msg)
{
ROS_INFO_STREAM_THROTTLE(10,
"Joint States arm 2 (throttled to 0.1 Hz):\n" << *joint_state_msg);
// ROS_INFO_STREAM("Joint States:\n" << *joint_state_msg);
arm_2_current_joint_states_ = *joint_state_msg;
if (!arm_2_has_been_zeroed_) {
arm_2_has_been_zeroed_ = true;
ROS_INFO("Sending arm 2 to zero joint positions...");
send_arm_to_zero_state(arm_2_joint_trajectory_publisher_);
}
}
// %EndTag(CB_CLASS)%
// %Tag(ARM_ZERO)%
/// Create a JointTrajectory with all positions set to zero, and command the arm.
void send_arm_to_zero_state(ros::Publisher & joint_trajectory_publisher) {
// Create a message to send.
trajectory_msgs::JointTrajectory msg;
// Fill the names of the joints to be controlled.
// Note that the vacuum_gripper_joint is not controllable.
msg.joint_names.clear();
msg.joint_names.push_back("shoulder_pan_joint");
msg.joint_names.push_back("shoulder_lift_joint");
msg.joint_names.push_back("elbow_joint");
msg.joint_names.push_back("wrist_1_joint");
msg.joint_names.push_back("wrist_2_joint");
msg.joint_names.push_back("wrist_3_joint");
msg.joint_names.push_back("linear_arm_actuator_joint");
// Create one point in the trajectory.
msg.points.resize(1);
// Resize the vector to the same length as the joint names.
// Values are initialized to 0.
msg.points[0].positions.resize(msg.joint_names.size(), 0.0);
// How long to take getting to the point (floating point seconds).
msg.points[0].time_from_start = ros::Duration(0.001);
ROS_INFO_STREAM("Sending command:\n" << msg);
joint_trajectory_publisher.publish(msg);
}
// %EndTag(ARM_ZERO)%
/// Called when a new LogicalCameraImage message is received.
void logical_camera_callback(
const osrf_gear::LogicalCameraImage::ConstPtr & image_msg)
{
ROS_INFO_STREAM_THROTTLE(10,
"Logical camera: '" << image_msg->models.size() << "' objects.");
}
/// Called when a new Proximity message is received.
void break_beam_callback(const osrf_gear::Proximity::ConstPtr & msg) {
if (msg->object_detected) { // If there is an object in proximity.
ROS_INFO("Break beam triggered.");
}
}
private:
std::string competition_state_;
double current_score_;
ros::Publisher arm_1_joint_trajectory_publisher_;
ros::Publisher arm_2_joint_trajectory_publisher_;
std::vector<osrf_gear::Order> received_orders_;
sensor_msgs::JointState arm_1_current_joint_states_;
sensor_msgs::JointState arm_2_current_joint_states_;
bool arm_1_has_been_zeroed_;
bool arm_2_has_been_zeroed_;
};
void proximity_sensor_callback(const sensor_msgs::Range::ConstPtr & msg) {
if ((msg->max_range - msg->range) > 0.01) { // If there is an object in proximity.
ROS_INFO_THROTTLE(1, "Proximity sensor sees something.");
}
}
void laser_profiler_callback(const sensor_msgs::LaserScan::ConstPtr & msg) {
size_t number_of_valid_ranges = std::count_if(
msg->ranges.begin(), msg->ranges.end(), [](const float f) {return std::isfinite(f);});
if (number_of_valid_ranges > 0) {
ROS_INFO_THROTTLE(1, "Laser profiler sees something.");
}
}
// %Tag(MAIN)%
int main(int argc, char ** argv) {
// Last argument is the default name of the node.
ros::init(argc, argv, "rrrobot_node");
ros::NodeHandle node;
// Instance of custom class from above.
MyCompetitionClass comp_class(node);
// Subscribe to the '/ariac/current_score' topic.
ros::Subscriber current_score_subscriber = node.subscribe(
"/ariac/current_score", 10,
&MyCompetitionClass::current_score_callback, &comp_class);
// Subscribe to the '/ariac/competition_state' topic.
ros::Subscriber competition_state_subscriber = node.subscribe(
"/ariac/competition_state", 10,
&MyCompetitionClass::competition_state_callback, &comp_class);
// %Tag(SUB_CLASS)%
// Subscribe to the '/ariac/orders' topic.
ros::Subscriber orders_subscriber = node.subscribe(
"/ariac/orders", 10,
&MyCompetitionClass::order_callback, &comp_class);
// %EndTag(SUB_CLASS)%
// Subscribe to the '/ariac/joint_states' topic.
ros::Subscriber arm_1_joint_state_subscriber = node.subscribe(
"/ariac/arm1/joint_states", 10,
&MyCompetitionClass::arm_1_joint_state_callback, &comp_class);
ros::Subscriber arm_2_joint_state_subscriber = node.subscribe(
"/ariac/arm2/joint_states", 10,
&MyCompetitionClass::arm_2_joint_state_callback, &comp_class);
// %Tag(SUB_FUNC)%
// Subscribe to the '/ariac/proximity_sensor_1' topic.
ros::Subscriber proximity_sensor_subscriber = node.subscribe(
"/ariac/proximity_sensor_1", 10, proximity_sensor_callback);
// %EndTag(SUB_FUNC)%
// Subscribe to the '/ariac/break_beam_1_change' topic.
ros::Subscriber break_beam_subscriber = node.subscribe(
"/ariac/break_beam_1_change", 10,
&MyCompetitionClass::break_beam_callback, &comp_class);
// Subscribe to the '/ariac/logical_camera_1' topic.
ros::Subscriber logical_camera_subscriber = node.subscribe(
"/ariac/logical_camera_1", 10,
&MyCompetitionClass::logical_camera_callback, &comp_class);
// Subscribe to the '/ariac/laser_profiler_1' topic.
ros::Subscriber laser_profiler_subscriber = node.subscribe(
"/ariac/laser_profiler_1", 10, laser_profiler_callback);
ROS_INFO("Setup complete.");
start_competition(node);
ros::spin(); // This executes callbacks on new data until ctrl-c.
return 0;
}
// %EndTag(MAIN)%
// %EndTag(FULLTEXT)%

225
rrrobot_ws/src/simulation_env/CMakeLists.txt
View File

@@ -1,225 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(simulation_env)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
gazebo_ros
message_generation
)
find_package(gazebo REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
arm_angles.msg
arm_command.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
# LIBRARIES simulation_env
CATKIN_DEPENDS roscpp std_msgs gazebo_ros
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
${roscpp_INCLUDE_DIRS}
${std_msgs_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(arm_angles SHARED
src/arm_angle_sensor_plugin.cpp
)
add_library(arm_motors SHARED
src/arm_motors.cpp
)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(test_move_arm test/move_arm.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(test_move_arm arm_angles arm_motors)
## Specify libraries to link a library or executable target against
target_link_libraries(arm_angles
${catkin_LIBRARIES}
${GAZEBO_LIBRARIES}
)
target_link_libraries(arm_motors
${catkin_LIBRARIES}
${GAZEBO_LIBRARIES}
)
target_link_libraries(test_move_arm
${catkin_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS arm_angles
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION /home/rrrobot/rrrobot_src/lib/
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_simulation_env.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

6
rrrobot_ws/src/simulation_env/msg/arm_angles.msg
View File

@@ -1,6 +0,0 @@
float32 shoulder_pivot_angle
float32 shoulder_joint_angle
float32 elbow_joint_angle
float32 wrist_pivot_angle
float32 wrist_joint_angle
float32 end_effector_pivot_angle

6
rrrobot_ws/src/simulation_env/msg/arm_command.msg
View File

@@ -1,6 +0,0 @@
float32 shoulder_pivot_force
float32 shoulder_joint_force
float32 elbow_joint_force
float32 wrist_pivot_force
float32 wrist_joint_force
float32 end_effector_pivot_force

70
rrrobot_ws/src/simulation_env/package.xml
View File

@@ -1,70 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>simulation_env</name>
<version>0.0.0</version>
<description>The simulation_env package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="root@todo.todo">root</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/simulation_env</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>gazebo_ros</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>gazebo_ros</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>gazebo_ros</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

61
rrrobot_ws/src/simulation_env/src/arm_angle_sensor_plugin.cpp
View File

@@ -1,61 +0,0 @@
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <memory>
#include <iostream>
#include "ros/ros.h"
#include "simulation_env/arm_angles.h"
using std::cout;
using std::endl;
namespace gazebo
{
class JointAngle : public ModelPlugin
{
public:
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
cout << "Loading JointAngle sensor plugin for arm" << endl;
model = _parent;
updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&JointAngle::onUpdate, this));
if (!ros::isInitialized())
{
int argc = 0;
char **argv = NULL;
ros::init(argc, argv, "arm");
}
nh.reset(new ros::NodeHandle("arm_node"));
publisher = nh->advertise<simulation_env::arm_angles>("arm_positions", 1);
}
private:
void onUpdate()
{
simulation_env::arm_angles msg;
// read in the joint angle for each joint in the arm
msg.shoulder_pivot_angle = model->GetJoint("shoulder_pivot")->Position();
msg.shoulder_joint_angle = model->GetJoint("shoulder_joint")->Position();
msg.elbow_joint_angle = model->GetJoint("elbow_joint")->Position();
msg.wrist_pivot_angle = model->GetJoint("wrist_pivot")->Position();
msg.wrist_joint_angle = model->GetJoint("wrist_joint")->Position();
msg.end_effector_pivot_angle = model->GetJoint("end_effector_pivot")->Position();
// publish the updated sensor measurements
publisher.publish(msg);
// make sure the message gets published
ros::spinOnce();
}
physics::ModelPtr model;
event::ConnectionPtr updateConnection;
std::unique_ptr<ros::NodeHandle> nh;
ros::Publisher publisher;
};
GZ_REGISTER_MODEL_PLUGIN(JointAngle)
} // namespace gazebo

53
rrrobot_ws/src/simulation_env/src/arm_motors.cpp
View File

@@ -1,53 +0,0 @@
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <memory>
#include <iostream>
#include "ros/ros.h"
#include "simulation_env/arm_command.h"
using std::cout;
using std::endl;
namespace gazebo
{
class ArmControl : public ModelPlugin
{
public:
void arm_command_callback(const simulation_env::arm_command &cmd)
{
// update the torque applied to each joint when a message is received
model->GetJoint("shoulder_pivot")->SetForce(0, cmd.shoulder_pivot_force);
model->GetJoint("shoulder_joint")->SetForce(0, cmd.shoulder_joint_force);
model->GetJoint("elbow_joint")->SetForce(0, cmd.elbow_joint_force);
model->GetJoint("wrist_pivot")->SetForce(0, cmd.wrist_pivot_force);
model->GetJoint("wrist_joint")->SetForce(0, cmd.wrist_joint_force);
model->GetJoint("end_effector_pivot")->SetForce(0, cmd.end_effector_pivot_force);
ros::spinOnce();
}
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
cout << "Loading motor control plugin for arm" << endl;
model = _parent;
if (!ros::isInitialized())
{
int argc = 0;
char **argv = NULL;
ros::init(argc, argv, "arm");
}
nh.reset(new ros::NodeHandle("arm_node"));
subscriber = nh->subscribe("arm_commands", 1000, &ArmControl::arm_command_callback, this);
}
private:
physics::ModelPtr model;
std::unique_ptr<ros::NodeHandle> nh;
ros::Subscriber subscriber;
};
GZ_REGISTER_MODEL_PLUGIN(ArmControl)
} // namespace gazebo

70
rrrobot_ws/src/simulation_env/test/move_arm.cpp
View File

@@ -1,70 +0,0 @@
#include "ros/ros.h"
#include "simulation_env/arm_command.h"
#include "simulation_env/arm_angles.h"
class PID_Control
{
public:
PID_Control(float Kp_in, float Ki_in, float Kd_in)
: Kp(Kp_in), Ki(Ki_in), Kd(Kd_in)
{}
float update(float error)
{
if(error > 0 and prev_error < 0
|| error < 0 and prev_error > 0)
{
reset();
}
sum_error += error;
float to_ret = Kp * error + Ki * sum_error + Kd * (error - prev_error);
prev_error = error;
return to_ret;
}
void reset()
{
sum_error = 0;
prev_error = 0;
}
private:
const float Kp;
const float Ki;
const float Kd;
double sum_error;
double prev_error;
};
PID_Control pid_controller(1000, 1, 0);
ros::Publisher publisher;
void angle_callback(const simulation_env::arm_angles &msg)
{
// just control the shoulder joint to go to 0 (should be roughly straight up)
float output = pid_controller.update(0 - msg.shoulder_joint_angle);
simulation_env::arm_command cmd;
cmd.shoulder_joint_force = output;
publisher.publish(cmd);
ros::spinOnce();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "arm_control_test");
ros::NodeHandle nh;
publisher = nh.advertise<simulation_env::arm_command>("/arm_node/arm_commands", 1000);
ros::Subscriber sub = nh.subscribe("/arm_node/arm_positions", 1000, angle_callback);
ros::spin();
return 0;
}

18
rrrobot_ws/world/rrrobot.world
View File

@@ -1,18 +0,0 @@
<?xml version="1.0"?>
<sdf version="1.6">
<world name="default">
<!-- Ground Plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://fanuc_robotic_arm</uri>
</include>
</world>
</sdf>