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

Added plugin to dynamically insert models

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-18 00:17:18 -04:00
parent 02d03710b9
commit 71acdaf9d7
5 changed files with 660 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
rrrobot_ws/src/gazebo_models/square_trash_can/model.config Normal file
View File

@@ -0,0 +1,11 @@
<?xml version="1.0" ?>
<model>
<name>square_trash_can</name>
<version>1.0</version>
<sdf version="1.6">model.sdf</sdf>
<author>
<name></name>
<email></email>
</author>
<description></description>
</model>

582
rrrobot_ws/src/gazebo_models/square_trash_can/model.sdf Normal file
View File

@@ -0,0 +1,582 @@
<?xml version='1.0'?>
<sdf version='1.6'>
<model name='square_trash_can'>
<link name='link_1'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
<pose frame=''>0 0 0 0 -0 0</pose>
</inertial>
<pose frame=''>0 0.25 0.082 0 -0 0</pose>
<gravity>1</gravity>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<enable_wind>0</enable_wind>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.5 0.02 0.8</size>
</box>
</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>
<box>
<size>0.5 0.02 0.8</size>
</box>
</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_clone'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
<pose frame=''>0 0 0 0 -0 0</pose>
</inertial>
<pose frame=''>0 -0.25 0.082 0 -0 0</pose>
<gravity>1</gravity>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<enable_wind>0</enable_wind>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.5 0.02 0.8</size>
</box>
</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>
<box>
<size>0.5 0.02 0.8</size>
</box>
</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_clone_0'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
<pose frame=''>0 0 0 0 -0 0</pose>
</inertial>
<pose frame=''>0.25 0 0.082 0 -0 0</pose>
<gravity>1</gravity>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<enable_wind>0</enable_wind>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.02 0.5 0.8</size>
</box>
</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>
<box>
<size>0.02 0.5 0.8</size>
</box>
</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_clone_0_clone'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
<pose frame=''>0 0 0 0 -0 0</pose>
</inertial>
<pose frame=''>-0.25 0 0.082 0 -0 0</pose>
<gravity>1</gravity>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<enable_wind>0</enable_wind>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.02 0.5 0.8</size>
</box>
</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>
<box>
<size>0.02 0.5 0.8</size>
</box>
</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_clone_0_clone_0'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
<pose frame=''>0 0 0 0 -0 0</pose>
</inertial>
<pose frame=''>0 0 -0.328 0 -0 0</pose>
<gravity>1</gravity>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<enable_wind>0</enable_wind>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.5 0.5 0.02</size>
</box>
</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>
<box>
<size>0.5 0.5 0.02</size>
</box>
</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='link_1_JOINT_0' type='fixed'>
<parent>link_1</parent>
<child>link_1_clone_0</child>
<pose frame=''>0 0 0 0 -0 0</pose>
<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='link_1_JOINT_4' type='fixed'>
<parent>link_1</parent>
<child>link_1_clone_0_clone_0</child>
<pose frame=''>0 0 0 0 -0 0</pose>
<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='link_1_clone_0_JOINT_1' type='fixed'>
<parent>link_1_clone_0</parent>
<child>link_1_clone</child>
<pose frame=''>0 0 0 0 -0 0</pose>
<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='link_1_clone_0_clone_JOINT_3' type='fixed'>
<parent>link_1_clone_0_clone</parent>
<child>link_1</child>
<pose frame=''>0 0 0 0 -0 0</pose>
<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='link_1_clone_JOINT_2' type='fixed'>
<parent>link_1_clone</parent>
<child>link_1_clone_0_clone</child>
<pose frame=''>0 0 0 0 -0 0</pose>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<static>0</static>
<allow_auto_disable>1</allow_auto_disable>
</model>
</sdf>

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

@@ -134,11 +134,16 @@ add_library(arm_motors SHARED
src/arm_motors.cpp
)
add_library(insert_model SHARED
src/model_insertion_plugin.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(arm_angles ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(arm_motors ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(insert_model ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
@@ -166,6 +171,11 @@ target_link_libraries(arm_motors
${GAZEBO_LIBRARIES}
)
target_link_libraries(insert_model
${catkin_LIBRARIES}
${GAZEBO_LIBRARIES}
)
target_link_libraries(test_move_arm
${catkin_LIBRARIES}
)
@@ -192,9 +202,9 @@ target_link_libraries(test_move_arm
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS arm_angles arm_motors
install(TARGETS arm_angles arm_motors insert_model
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION /home/rrrobot/rrrobot_src/lib/
LIBRARY DESTINATION /home/rrrobot/rrrobot_ws/lib/
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)

52
rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp Normal file
View File

@@ -0,0 +1,52 @@
#include <ignition/math/Pose3.hh>
#include "gazebo/physics/physics.hh"
#include "gazebo/common/common.hh"
#include "gazebo/gazebo.hh"
#include "ros/ros.h"
#include <std_msgs/String.h>
#include <string>
#include <memory>
#include <iostream>
namespace gazebo
{
class ModelInsertion : public WorldPlugin
{
public:
void Load(physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/)
{
std::cout << "Loading model insertion plugin" << std::endl;
// Option 1: Insert model from file via function call.
// The filename must be in the GAZEBO_MODEL_PATH environment variable.
parent = _parent; //->InsertModelFile("model://box");
if (!ros::isInitialized())
{
int argc = 0;
char **argv = NULL;
ros::init(argc, argv, "arm");
}
nh.reset(new ros::NodeHandle());
subscriber = nh->subscribe("object_to_load", 1000, &ModelInsertion::insertModel, this);
}
void insertModel(const std_msgs::String &msg)
{
std::cout << "Received message to load model: " << msg.data << std::endl;
parent->InsertModelFile(std::string("model://") + msg.data);
}
private:
physics::WorldPtr parent;
std::unique_ptr<ros::NodeHandle> nh;
ros::Subscriber subscriber;
};
// Register this plugin with the simulator
GZ_REGISTER_WORLD_PLUGIN(ModelInsertion)
} // namespace gazebo

4
rrrobot_ws/world/rrrobot.world
View File

@@ -12,7 +12,9 @@
</include>
<include>
<uri>model://fanuc_robotic_arm</uri>
<uri>model://fanuc_robotic_arm_with_gripper</uri>
</include>
<plugin name="insert_models" filename="libinsert_model.so"/>
</world>
</sdf>