SetTwistHandler.cpp
Go to the documentation of this file.
00001 
00002 #include <pluginlib/class_list_macros.h>
00003 
00004 #include <rigid_body_handler/SetTwistHandler.h>
00005 #include <v_repLib.h>
00006 #include <vrep_ros_plugin/access.h>
00007 
00008 
00009 #include <Eigen/Geometry>
00010 
00011 #include <vrep_ros_plugin/ConsoleHandler.h>
00012 
00013 
00014 SetTwistHandler::SetTwistHandler() : GenericObjectHandler(),
00015 _isStatic(false){
00016     // Set Object twist defines
00017     simRegisterCustomLuaVariable("sim_ext_ros_bridge_set_obj_twist_data_main", (boost::lexical_cast<std::string>(int(SET_OBJ_TWIST_DATA_MAIN))).c_str());
00018     simRegisterCustomLuaVariable("sim_ext_ros_bridge_set_obj_twist_data_lin_gain", (boost::lexical_cast<std::string>(int(SET_OBJ_TWIST_DATA_LIN_GAIN))).c_str());
00019     simRegisterCustomLuaVariable("sim_ext_ros_bridge_set_obj_twist_data_ang_gain", (boost::lexical_cast<std::string>(int(SET_OBJ_TWIST_DATA_ANG_GAIN))).c_str());
00020 }
00021 
00022 SetTwistHandler::~SetTwistHandler(){
00023 }
00024 
00025 unsigned int SetTwistHandler::getObjectType() const {
00026         return SET_OBJ_TWIST_DATA_MAIN;
00027 }
00028 
00029 void SetTwistHandler::synchronize(){
00030         // We update SetTwistHandler's data from its associated scene object custom data:
00031         // 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
00032         int buffSize = simGetObjectCustomDataLength(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER);
00033         char* datBuff=new char[buffSize];
00034         simGetObjectCustomData(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
00035         std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
00036         delete[] datBuff;
00037         // 2. From that retrieved data, try to extract sub-data with the SET_OBJ_TWIST_DATA_MAIN tag:
00038         std::vector<unsigned char> tempMainData;
00039 
00040         if (CAccess::extractSerializationData(developerCustomData,SET_OBJ_TWIST_DATA_MAIN,tempMainData)){ // Yes, the tag is there. For now we only have to synchronize _maxVelocity:
00041 
00042                 // Remove # chars for compatibility with ROS
00043                 _associatedObjectName = simGetObjectName(_associatedObjectID);
00044                 std::string objectName(_associatedObjectName);
00045                 std::replace( objectName.begin(), objectName.end(), '#', '_');
00046                 //_pub = _nh.advertise<geometry_msgs::TwistStamped>(objectName+"/twist", 1);
00047                 std::stringstream streamtemp;
00048 
00049 
00050                 streamtemp << "- [Set Twist Target] in '" << _associatedObjectName << "'. Subscribed on the topic "<< objectName<<"/SetTwist"<< std::endl;
00051                 ConsoleHandler::printInConsole(streamtemp);
00052 
00053         }
00054 
00055 
00056 
00057 }
00058 
00059 void SetTwistHandler::handleSimulation(){
00060         // called when the main script calls: simHandleModule
00061         if(!_initialized){
00062                 _initialize();
00063         }
00064 
00065         std::stringstream ss;
00066 
00067         Eigen::Quaternion < simFloat > orientation; //(x,y,z,w)
00068         Eigen::Matrix< simFloat, 3, 1> desLinVel((simFloat)_twistCommands.twist.linear.x,
00069                         (simFloat)_twistCommands.twist.linear.y,
00070                         (simFloat)_twistCommands.twist.linear.z);
00071         Eigen::Matrix< simFloat, 3, 1> desAngVel((simFloat)_twistCommands.twist.angular.x,
00072                         (simFloat)_twistCommands.twist.angular.y,
00073                         (simFloat)_twistCommands.twist.angular.z);
00074 
00075         // Input velocity is expected to be in body frame but V-Rep expects it to be in world frame.
00076         if(simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data())!=-1){
00077         } else {
00078                 ss << "- [" << _associatedObjectName << "] Error getting orientation. " << std::endl;
00079         }
00080 
00081         //      simResetDynamicObject(_associatedObjectID);
00082 
00083         //Set object velocity
00084         if (_isStatic){
00085                 Eigen::Matrix<simFloat, 3, 1> position;
00086                 simGetObjectPosition(_associatedObjectID, -1, position.data());
00087                 const simFloat timeStep = simGetSimulationTimeStep();
00088                 const simFloat angle = timeStep*desAngVel.norm();
00089                 const Eigen::Matrix<simFloat, 3, 1> axis =
00090                                 (angle > 1e-6) ? desAngVel.normalized() : (Eigen::Matrix<simFloat, 3, 1>() << 1,0,0).finished();
00091 
00092                 position += timeStep*(orientation*desLinVel);
00093                 orientation *= Eigen::Quaternion<simFloat>(Eigen::AngleAxis<simFloat>(angle, axis));
00094 
00095                 simSetObjectPosition(_associatedObjectID, -1, position.data());
00096                 simSetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data());
00097         } else {
00098                 // Apply the linear velocity to the object
00099                 //              simResetDynamicObject(_associatedObjectID);
00100                 //              if(simSetObjectFloatParameter(_associatedObjectID, sim_shapefloatparam_init_velocity_x, desLinVel[0]) != 1 ||
00101                 //                              simSetObjectFloatParameter(_associatedObjectID, sim_shapefloatparam_init_velocity_y, desLinVel[1]) != 1 ||
00102                 //                              simSetObjectFloatParameter(_associatedObjectID, sim_shapefloatparam_init_velocity_z, desLinVel[2]) != 1 ||
00103                 //                              simSetObjectFloatParameter(_associatedObjectID, sim_shapefloatparam_init_velocity_a, desAngVel[0]) != 1 ||
00104                 //                              simSetObjectFloatParameter(_associatedObjectID, sim_shapefloatparam_init_velocity_b, desAngVel[1]) != 1 ||
00105                 //                              simSetObjectFloatParameter(_associatedObjectID, sim_shapefloatparam_init_velocity_g, desAngVel[2]) != 1) {
00106                 //
00107                 //                      ss << "- [" << _associatedObjectName << "] Error setting object velocity." << std::endl;
00108                 //              } else {
00109                 //                      ss << "- [" << _associatedObjectName << "] Object velocity correctly set to ["
00110                 //                                      << desLinVel.transpose() << " " << desAngVel.transpose() << "]." << std::endl;
00111                 //              }
00112 
00113                 simFloat mass;
00114                 Eigen::Matrix<simFloat, 3, 3> inertiaMatrix;
00115                 Eigen::Matrix<simFloat, 3, 1> centerOfMass, linVel, angVel, gravity;
00116                 _simGetGravity(gravity.data());
00117 
00118                 simFloat matrix[12];
00119                 simGetObjectMatrix(_associatedObjectID, -1, matrix);
00120                 simGetShapeMassAndInertia(_associatedObjectID, &mass, inertiaMatrix.data(), centerOfMass.data(), matrix);
00121 
00122                 simGetVelocity(_associatedObjectID, linVel.data(), angVel.data());
00123                 angVel = orientation.conjugate()*angVel;
00124 
00125                 Eigen::Matrix<simFloat, 3, 1> force = -mass*(gravity + _linGain*(linVel-orientation*desLinVel));
00126                 Eigen::Matrix<simFloat, 3, 1> torque = orientation*(angVel.cross(inertiaMatrix*angVel) - inertiaMatrix*_angGain*(angVel-desAngVel));
00127 
00128                 simAddForceAndTorque(_associatedObjectID, force.data(), torque.data());
00129         }
00130 
00131         if (ss.rdbuf()->in_avail()){
00132                 ConsoleHandler::printInConsole(ss);
00133         }
00134 
00135 }
00136 
00137 
00138 void SetTwistHandler::_initialize(){
00139         if (_initialized)
00140                 return;
00141 
00142         std::stringstream ss;
00143 
00144         std::string objectName(_associatedObjectName);
00145         std::replace( objectName.begin(), objectName.end(), '#', '_');
00146         _sub = _nh.subscribe(objectName+"/SetTwist", 1, &SetTwistHandler::TwistCommandCallback, this);
00147 
00148         //Check if the object is static
00149         simGetObjectIntParameter(_associatedObjectID, sim_shapeintparam_static, &_isStatic);
00150         if(_isStatic){
00151                 std::stringstream ss;
00152                 ss << "- [" << _associatedObjectName << "] WARNING: setting velocity of a static object might give inaccurate results." << std::endl;;
00153                 ConsoleHandler::printInConsole(ss);
00154         }
00155 
00156         std::vector<unsigned char> developerCustomData;
00157         getDeveloperCustomData(developerCustomData);
00158         std::vector<unsigned char> tempMainData;
00159 
00160         if (CAccess::extractSerializationData(developerCustomData,SET_OBJ_TWIST_DATA_LIN_GAIN,tempMainData)){
00161                 _linGain=CAccess::pop_float(tempMainData);
00162                 ss << "- [" << _associatedObjectName << "] Linear gain set to " << _linGain << "." << std::endl;
00163         } else {
00164                 _linGain = 1.0;
00165                 ss << "- [" << _associatedObjectName << "] Linear gain not specified. Using " << _linGain << " as default." << std::endl;
00166         }
00167 
00168         if (CAccess::extractSerializationData(developerCustomData,SET_OBJ_TWIST_DATA_ANG_GAIN,tempMainData)){
00169                 _angGain=CAccess::pop_float(tempMainData);
00170                 ss << "- [" << _associatedObjectName << "] Angular gain set to " << _angGain << "." << std::endl;
00171         } else {
00172                 _angGain = 1.0;
00173                 ss << "- [" << _associatedObjectName << "] Angular gain not specified. Using " << _angGain << " as default." << std::endl;
00174         }
00175 
00176         ConsoleHandler::printInConsole(ss);
00177         _initialized=true;
00178 }
00179 
00180 
00181 
00182 void SetTwistHandler::TwistCommandCallback(const geometry_msgs::TwistStamped& msg){
00183 
00184         _twistCommands = msg;
00185 
00186 }
00187 
00188 PLUGINLIB_EXPORT_CLASS(SetTwistHandler, GenericObjectHandler)
00189 
00190 


rigid_body_handler
Author(s): Riccardo Spica , Giovanni Claudio
autogenerated on Sat Jun 8 2019 20:22:55