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();
}
}
