void SetTwistHandler::handleSimulation(){ // called when the main script calls: simHandleModule if(!_initialized){ _initialize(); } Eigen::Quaternion < simFloat > orientation; //(x,y,z,w) Eigen::Matrix< simFloat, 3, 1> linVelocity((simFloat)_twistCommands.twist.linear.x, (simFloat)_twistCommands.twist.linear.y, (simFloat)_twistCommands.twist.linear.z); Eigen::Matrix< simFloat, 3, 1> angVelocity((simFloat)_twistCommands.twist.angular.x, (simFloat)_twistCommands.twist.angular.y, (simFloat)_twistCommands.twist.angular.z); // Input velocity is expected to be in body frame but V-Rep expects it to be in world frame. if(simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data())!=-1){ linVelocity = orientation*linVelocity; angVelocity = orientation*angVelocity; } else { std::stringstream ss; ss << "- [" << _associatedObjectName << "] Error getting orientation. " << std::endl; ConsoleHandler::printInConsole(ss); } simResetDynamicObject(_associatedObjectID); //Set object velocity if (_isStatic){ Eigen::Matrix<simFloat, 3, 1> position; simGetObjectPosition(_associatedObjectID, -1, position.data()); const simFloat timeStep = simGetSimulationTimeStep(); position = position + timeStep*linVelocity; simSetObjectPosition(_associatedObjectID, -1, position.data()); const simFloat angle = timeStep*angVelocity.norm(); if(angle > 1e-6){ orientation = Eigen::Quaternion< simFloat >(Eigen::AngleAxis< simFloat >(timeStep*angVelocity.norm(), angVelocity/angVelocity.norm()))*orientation; } simSetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data()); } else { // Apply the linear velocity to the object if(simSetObjectFloatParameter(_associatedObjectID, 3000, linVelocity[0]) && simSetObjectFloatParameter(_associatedObjectID, 3001, linVelocity[1]) && simSetObjectFloatParameter(_associatedObjectID, 3002, linVelocity[2])==-1) { std::stringstream ss; ss << "- [" << _associatedObjectName << "] Error setting linear velocity. " << std::endl;; ConsoleHandler::printInConsole(ss); } // Apply the angular velocity to the object if(simSetObjectFloatParameter(_associatedObjectID, 3020, angVelocity[0]) && simSetObjectFloatParameter(_associatedObjectID, 3021, angVelocity[1]) && simSetObjectFloatParameter(_associatedObjectID, 3022, angVelocity[2])==-1) { std::stringstream ss; ss << "- [" << _associatedObjectName << "] Error setting angular velocity. " << std::endl;; ConsoleHandler::printInConsole(ss); } } }
void CSubscriberData::setObjectPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& pose) { float p[3]={(float)pose->pose.position.x,(float)pose->pose.position.y,(float)pose->pose.position.z}; float q[4]={(float)pose->pose.orientation.x,(float)pose->pose.orientation.y,(float)pose->pose.orientation.z,(float)pose->pose.orientation.w}; if (simSetObjectPosition(auxInt1,auxInt2,p)==-1) shutDownGeneralSubscriber(); else simSetObjectQuaternion(auxInt1,auxInt2,q); }
void CSubscriberData::setObjectQuaternionCallback(const geometry_msgs::Quaternion::ConstPtr& quaternion) { if (_handleGeneralCallback_before()) { float v[4]={(float)quaternion->x,(float)quaternion->y,(float)quaternion->z,(float)quaternion->w}; if (simSetObjectQuaternion(auxInt1,auxInt2,v)==-1) shutDownGeneralSubscriber(); _handleGeneralCallback_after(); } }