Commit 431eeff5 authored by Jennifer Buehler's avatar Jennifer Buehler
Browse files

added sources

parent e3dfd35a
# gazebo-pkgs
A collection of tools and plugins for Gazebo
cmake_minimum_required(VERSION 2.8.3)
project(gazebo_grasp_plugin)
## 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
gazebo_ros
geometry_msgs
roscpp
std_msgs
)
find_package(gazebo REQUIRED)
## 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 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 you 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 gazebo_grasp_fix
CATKIN_DEPENDS gazebo_ros geometry_msgs roscpp std_msgs
DEPENDS gazebo
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
include
${catkin_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS}
)
link_directories(
${GAZEBO_LIBRARY_DIRS}
${catkin_LIBRARY_DIRS}
)
## Declare a cpp library
add_library(gazebo_grasp_fix SHARED src/GazeboGraspFix.cpp)
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
# add_dependencies(gazebo_grasp_plugin_node gazebo_grasp_plugin_generate_messages_cpp)
add_dependencies(gazebo_grasp_fix ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(gazebo_grasp_fix
${GAZEBO_LIBRARIES}
${Boost_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 and/or libraries for installation
# install(TARGETS gazebo_grasp_plugin gazebo_grasp_plugin_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
# # 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_gazebo_grasp_plugin.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)
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <stdio.h>
namespace gazebo {
/**
* Inspired by gazebo::physics::Gripper, this is our own implementation which does not use deprecated functions any more.
*
* This is a *model* pluing, so you have to load the model plugin from the robot URDF:
*
* ```xml
* <gazebo>
* <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
* <palm_link> hand_link_name </palm_link>
* <gripper_link> finger_index_link_1 </gripper_link>
* <gripper_link> finger_index_link_2 </gripper_link>
* <gripper_link> ... </gripper_link>
* </plugin>
* </gazebo>
* ```
*
* \author Jennifer Buehler
*/
class GazeboGraspFix : public ModelPlugin {
public:
GazeboGraspFix();
GazeboGraspFix(physics::ModelPtr _model);
virtual ~GazeboGraspFix();
virtual void Init();
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
void OnUpdate();
private:
void InitValues();
void OnContact(const ConstContactsPtr& ptr);
bool HandleAttach(const std::string& objName);
void HandleDetach(const std::string& objName);
/**
* Checks whether any two vectors in the set have an angle greater than minAngleDiff (in rad), and one is at least
* lengthRatio (0..1) of the other in it's length.
*/
bool checkGrip(const std::vector<math::Vector3>& forces, float minAngleDiff, float lengthRatio);
physics::ModelPtr model;
physics::PhysicsEnginePtr physics;
physics::WorldPtr world;
physics::JointPtr fixedJoint;
physics::LinkPtr palmLink;
event::ConnectionPtr update_connection;
transport::NodePtr node;
transport::SubscriberPtr contactSub; //subscriber to contact updates
std::map<std::string, physics::CollisionPtr> collisions;
/**
* Helper class to encapsulate a collision information.
* Forward declaration here.
*/
class CollidingPoint;
//Contact forces sorted by object name the gripper collides with, and the link colliding.
//This is a vector summed up over time, sum count is kept in contactSumCnt.
std::map<std::string, std::map<std::string, CollidingPoint> > contacts;
boost::mutex mutexContacts; //mutex protects contacts
//when an object was attached, it had these colliding points.
std::map<std::string, std::map<std::string, CollidingPoint> > attachGripContacts;
//records how many update loops (at updateRate) the grip on that object has been recorded
//as "holding". Every loop, if a grip is not recorded, this number decreases.
//When it reaches grip_count_threshold, it will be attached.
//The number won't increase above max_grip_count once it has reached that number.
std::map<std::string, int> gripCounts;
//maximum record in gripCounts
int max_grip_count;
//number of recorded "grips" in the past (in gripCount) which, when it is exceeded, counts
//as the object grasped, and when it is lower, as released.
int grip_count_threshold;
//once an object is gripped, the relative position of the collision link surface to the
//object is remembered. As soon as this distance changes more than release_tolerance,
//the object is released.
float release_tolerance;
bool attached;
std::string attachedObjName;
//nano seconds between two updates
common::Time updateRate;
//last time OnUpdate() was called
common::Time prevUpdateTime;
};
}
<?xml version="1.0"?>
<package>
<name>gazebo_grasp_plugin</name>
<version>0.0.0</version>
<description>
Gazebo Model plugin(s) which handle/help grasping in Gazebo.
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<maintainer email="jennifer.e.buehler@gmail.com">Jennifer Buehler</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>GPLv3</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/gazebo_grasp_plugin</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, 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 build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>gazebo_ros</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<run_depend>gazebo_ros</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
</export>
</package>
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/physics/ContactManager.hh>
#include <gazebo/physics/Contact.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>
#include <gazebo_grasp_plugin/GazeboGraspFix.h>
using gazebo::GazeboGraspFix;
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(GazeboGraspFix)
/**
* Helper class to encapsulate a collision information
*/
class GazeboGraspFix::CollidingPoint{
public:
CollidingPoint(): sum(0) {}
CollidingPoint(const CollidingPoint& o):
collLink(o.collLink),
collObj(o.collObj),
force(o.force),
pos(o.pos),
objPos(o.objPos),
sum(o.sum){}
//the collision
physics::CollisionPtr collLink, collObj;
//average force vector of the colliding point
gazebo::math::Vector3 force;
//position (relative to reference frame of gripper collision surface) where the contact happens on collision surface
gazebo::math::Vector3 pos;
//position (relative to reference frame of gripper collision surface) where the object center is located during collision.
gazebo::math::Vector3 objPos;
//sum of force and pose (they are actually summed up from several contact points), divide force and pose by this to obtain average
int sum;
};
GazeboGraspFix::GazeboGraspFix(){
InitValues();
}
GazeboGraspFix::GazeboGraspFix(physics::ModelPtr _model){
this->model = _model;
InitValues();
}
GazeboGraspFix::~GazeboGraspFix() {
/**
//XXX this is for gazebo 4.0
if (this->world && this->model && this->world->GetRunning()) {
physics::ContactManager *mgr = this->world->GetPhysicsEngine()->GetContactManager();
mgr->RemoveFilter(this->model->GetScopedName());
}
*/
this->model.reset();
this->physics.reset();
this->update_connection.reset();
if (this->node) this->node->Fini();
this->node.reset();
}
void GazeboGraspFix::Init(){
this->prevUpdateTime = common::Time::GetWallTime();
}
void GazeboGraspFix::InitValues(){
float timeDiff=0.25;
this->release_tolerance=0.005;
this->attached = false;
this->updateRate = common::Time(0, common::Time::SecToNano(timeDiff));
this->prevUpdateTime = common::Time::GetWallTime();
float graspedSecs=2;
this->max_grip_count=floor(graspedSecs/timeDiff);
this->grip_count_threshold=floor(max_grip_count/2);
this->attached = false;
this->node = transport::NodePtr(new transport::Node());
}
void GazeboGraspFix::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
this->model = _parent;
this->world = this->model->GetWorld();
this->physics = this->model->GetWorld()->GetPhysicsEngine();
this->fixedJoint = this->physics->CreateJoint("revolute");
this->node->Init(this->model->GetWorld()->GetName());
physics::ContactManager * contactManager = this->physics->GetContactManager();
contactManager->PublishContacts(); //XXX not sure I need this?
sdf::ElementPtr handLinkElem = _sdf->GetElement("palm_link");
sdf::ElementPtr fingerLinkElem = _sdf->GetElement("gripper_link");
if (!handLinkElem.get() || !fingerLinkElem.get()) {
gzerr<<"Cannot use GazeboGraspFix model plugin because no <palm_link> and/or <gripper_link> elements specified in URDF/SDF."<<std::endl;
return;
}
this->palmLink = this->model->GetLink(handLinkElem->Get<std::string>());
std::vector<std::string> collisionScopedNames;
while (fingerLinkElem) {
physics::LinkPtr link = this->model->GetLink(fingerLinkElem->Get<std::string>());
//std::cout<<"Got link "<<fingerLinkElem->Get<std::string>()<<std::endl;
if (!link.get()){
gzerr<<"Link "<<fingerLinkElem->Get<std::string>()<<" specified in Model URDF can't be found in gazebo for GazeboGraspFix model plugin. Skipping."<<std::endl;
fingerLinkElem = fingerLinkElem->GetNextElement("gripper_link");
continue;
}
for (unsigned int j = 0; j < link->GetChildCount(); ++j) {
physics::CollisionPtr collision = link->GetCollision(j);
std::string scopedName = collision->GetScopedName();
std::map<std::string, physics::CollisionPtr>::iterator collIter = this->collisions.find(scopedName);
if (collIter != this->collisions.end()) { //this collision was already added before
continue;
}
/*collision->SetContactsEnabled(true);
this->connections.push_back(collision->ConnectContact(boost::bind(&GazeboGraspFix::OnContact, this, _1, _2)));*/
std::cout<<"GazeboGraspFix: Adding collision scoped name "<<scopedName<<std::endl;
this->collisions[scopedName] = collision;
collisionScopedNames.push_back(scopedName);
}
fingerLinkElem = fingerLinkElem->GetNextElement("gripper_link");
}
std::string topic = contactManager->CreateFilter(this->model->GetScopedName(), collisionScopedNames);
if (!this->contactSub.get()) {
std::cout<<"Subscribing contact manager to topic "<<topic<<std::endl;
bool latching=false;
this->contactSub = this->node->Subscribe(topic,&GazeboGraspFix::OnContact, this, latching);
}
update_connection=event::Events::ConnectWorldUpdateEnd(boost::bind(&GazeboGraspFix::OnUpdate, this));
}
void GazeboGraspFix::OnUpdate() {
//std::cout<<"UPDATE!"<<std::endl;
if (common::Time::GetWallTime() - this->prevUpdateTime < this->updateRate)
return;
//first, copy all contact data into local struct. Don't do the complex grip check (checkGrip)
//within the mutex, because that slows down OnContact().
this->mutexContacts.lock();
std::map<std::string, std::map<std::string, CollidingPoint> > contPoints(contacts);
this->contacts.clear();
this->mutexContacts.unlock();
//Gather all summed forces for all the objects (so we have all forces on one object).
std::map<std::string, std::vector<gazebo::math::Vector3> > appliedForces; //sorted by object name, all forces effecting on the object
std::map<std::string, std::map<std::string, CollidingPoint> >::iterator objIt;
for (objIt=contPoints.begin(); objIt!=contPoints.end(); ++objIt){
std::string objName=objIt->first;
//std::cout<<"Examining object collisions with "<<objName<<std::endl;
std::map<std::string, CollidingPoint>::iterator lIt;
std::vector<gazebo::math::Vector3> forces;
for (lIt=objIt->second.begin(); lIt!=objIt->second.end(); ++lIt){
std::string linkName=lIt->first;
CollidingPoint& collP=lIt->second;
gazebo::math::Vector3 avgForce=collP.force/collP.sum;
//std::cout<<"Found collision with "<<linkName<<": "<<avgForce.x<<", "<<avgForce.y<<", "<<avgForce.z<<" (avg over "<<collP.sum<<")"<<std::endl;
forces.push_back(avgForce);
}
appliedForces[objName]=forces;
}
std::set<std::string> grippedObjects;
std::map<std::string, std::vector<gazebo::math::Vector3> >::iterator it;
for (it=appliedForces.begin(); it!=appliedForces.end(); ++it) {
std::string objName=it->first;
float minAngleDiff=120 * M_PI/180;
//std::cout<<"Number applied forces on "<<objName<<": "<<it->second.size()<<std::endl;
if (checkGrip(it->second,minAngleDiff,0.3)) {
int& counts=gripCounts[objName];
if (counts < max_grip_count) ++counts;
//std::cout<<"GRIPPING "<<objName<<", grip count "<<counts<<" (threshold "<<grip_count_threshold<<") !!!!!!! "<<std::endl;
if (attached && (attachedObjName != objName)) {
//if (counts > grip_count_threshold)
gzwarn<<"GazeboGraspFix has found that object "<<
attachedObjName<<" is already attached, can't grasp "<<objName<<" as well !"<<std::endl;
continue;
}
if (!attached && (counts > grip_count_threshold)) {
std::cout<<"GazeboGraspFix: Attaching "<<objName<<" !!!!!!!"<<std::endl;
//for this grip, backup the array of contact poses which played part in the grip, sorted by colliding link
attachGripContacts[objName]=contPoints[objName];
if (this->HandleAttach(objName)){
attached=true;
attachedObjName=objName;
}
}
grippedObjects.insert(objName);
}
}
//now, for all objects that are not gripped, decrease grip counter, and possibly release object.
std::map<std::string, int>::iterator gripCntIt;
for (gripCntIt=gripCounts.begin(); gripCntIt!=gripCounts.end(); ++gripCntIt){
std::string objName=gripCntIt->first;
if (grippedObjects.find(objName)!=grippedObjects.end()) continue;
//std::cout<<"NOT-GRIPPING "<<objName<<", grip count "<<gripCntIt->second<<" (threshold "<<grip_count_threshold<<") !!!!!!! "<<std::endl;
if (gripCntIt->second > 0) --(gripCntIt->second);
if (!attached || (gripCntIt->second > grip_count_threshold)) continue;
//object should potentially be detached now. However, this happens too easily when just considering the count, as the fingers
//in gazebo start wobbling as the arm moves around, and albeit they are still close to the object, the grip is not
//detected any more. So to be sure, we will check that the collision point has not moved too far from where it originally was.
std::map<std::string, std::map<std::string, CollidingPoint> >::iterator collIt=attachGripContacts.find(objName);
if (collIt==attachGripContacts.end()) {
gzerr<<"Consistency: Could not find attachGripContacts for "<<objName<<std::endl;
continue;
}
std::map<std::string, CollidingPoint>& colls=collIt->second;
int cntRelease=0;
std::map<std::string, CollidingPoint>::iterator pointIt;
for (pointIt=colls.begin(); pointIt!=colls.end(); ++pointIt) {
CollidingPoint& cpInfo=pointIt->second;
gazebo::math::Vector3 relContactPos=cpInfo.pos/cpInfo.sum;
gazebo::math::Vector3 relObjPos=cpInfo.objPos/cpInfo.sum;
gazebo::math::Pose currObjWorldPose=cpInfo.collObj->GetLink()->GetWorldPose();
//now, get the contact location on the collision surface in world coordinates now.
gazebo::math::Pose currCollWorldPose=cpInfo.collLink->GetLink()->GetWorldPose();
gazebo::math::Matrix4 worldToCollision=currCollWorldPose.rot.GetAsMatrix4();
worldToCollision.SetTranslate(currCollWorldPose.pos);
//transform from collision link to contact point
gazebo::math::Matrix4 collToContact=gazebo::math::Matrix4::IDENTITY;
collToContact.SetTranslate(relContactPos);
//now, current world position of the contact point is:
gazebo::math::Matrix4 _currContactWorldPose=worldToCollision*collToContact;
gazebo::math::Vector3 currContactWorldPose=_currContactWorldPose.GetTranslation();
gazebo::math::Vector3 oldObjDist=relContactPos-relObjPos;
gazebo::math::Vector3 newObjDist=currContactWorldPose-currObjWorldPose.pos;
//std::cout<<"Obj Trans "<<cpInfo.collLink->GetName()<<": "<<relObjPos.x<<", "<<relObjPos.y<<", "<<relObjPos.z<<std::endl;
//std::cout<<"Cont Trans "<<cpInfo.collLink->GetName()<<": "<<relContactPos.x<<", "<<relContactPos.y<<", "<<relContactPos.z<<std::endl;
float diff=fabs(oldObjDist.GetLength() - newObjDist.GetLength());
//std::cout<<"Diff for link "<<cpInfo.collLink->GetName()<<": "<<diff<<std::endl;
if (diff > release_tolerance) {
++cntRelease;
}
}
if (cntRelease > 0){ //sufficient links did not meet the criteria to be close enough to the object.
std::cout<<"GazeboGraspFix: Detaching "<<objName<<" !!!!!!!"<<std::endl;
this->HandleDetach(objName);
attached=false;
attachedObjName="";
gripCntIt->second=0;
attachGripContacts.erase(collIt);
}
}
this->prevUpdateTime = common::Time::GetWallTime();
}
double angularDistance(const gazebo::math::Vector3& _v1, const gazebo::math::Vector3& _v2)