bool Primitive::store(FILE* f) const {
const Pose& pose = getPose();
const Pos& vel = getVel();
const Pos& avel = getAngularVel();
if ( fwrite ( pose.ptr() , sizeof ( Pose::value_type), 16, f ) == 16 )
if( fwrite ( vel.ptr() , sizeof ( Pos::value_type), 3, f ) == 3 )
if( fwrite ( avel.ptr() , sizeof ( Pos::value_type), 3, f ) == 3 )
return true;
return false;
}
void Primitive::decellerate(double factorLin, double factorAng){
if(!body) return;
Pos vel;
if(factorLin!=0){
vel = getVel();
applyForce(vel*(-factorLin));
}
if(factorAng!=0){
vel = getAngularVel();
applyTorque(vel*(-factorAng));
}
}
void AMU::receiveDataCallback(const can_msgs::CANFrame::ConstPtr& msg)
{
// Restore angle
double angle[3] = {0.0, 0.0, 0.0}; // [deg]
if(msg->id == 0x41)
{
for(int i = 0; i < 3; i++)
{
angle[i] = getAngle(msg->data[i*2], msg->data[i*2+1]);
}
ROS_INFO("Roll: %3.1f, Pitch: %3.1f, Yaw: %3.1f [deg]", angle[0], angle[1], angle[2]);
}
// Restore angular velocity
double angular_vel[3] = {0.0, 0.0, 0.0}; // [deg/sec]
if(msg->id == 0x42)
{
for(int i = 0; i < 3; i++)
{
angular_vel[i] = getAngularVel(msg->data[i*2], msg->data[i*2+1]);
}
ROS_INFO("Roll vel: %3.1f, Pitch vel: %3.1f, Yaw vel: %3.1f [deg/sec]", angular_vel[0], angular_vel[1], angular_vel[2]);
}
// Restore translational acceleration
double acc[3] = {0.0, 0.0, 0.0}; // [G]
if(msg->id == 0x43)
{
for(int i = 0; i < 3; i++)
{
acc[i] = getAcc(msg->data[i*2], msg->data[i*2+1]);
}
ROS_INFO("Acc_x: %3.1f, Acc_y: %3.1f, Acc_z: %3.1f [m/sec^2]", acc[0], acc[1], acc[2]);
}
// Publish Topic
sensor_msgs::Imu data;
// Angle [rad] -> Quaternion
// Remark: Coordinate between KAMUI and AMU is different. (y- and z-axis are reverse)
// ** KAMUI **
// x+: forward, y+: left, z+: above
// ** AMU **
// x+: forward, y+: right, z+: below
// You must reverse the sign of pitch and yaw, and also corresponding angular velocities.
double roll = angle[0] * DEG_TO_RAD;
double pitch = -angle[1] * DEG_TO_RAD;
double yaw = -angle[2] * DEG_TO_RAD;
data.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
// Angular velocity [rad/s]
data.angular_velocity.x = angular_vel[0] * DEG_TO_RAD;
data.angular_velocity.y = -angular_vel[1] * DEG_TO_RAD;
data.angular_velocity.z = -angular_vel[2] * DEG_TO_RAD;
// Linear acceleration [m/sec^2]
data.linear_acceleration.x = acc[0] * G_TO_MS2;
data.linear_acceleration.y = -acc[1] * G_TO_MS2;
data.linear_acceleration.z = -acc[2] * G_TO_MS2;
// Covariance
// TODO: Check the definition of covariance / 20150807, Katsumoto
data.orientation_covariance[0] = -1;
data.angular_velocity_covariance[0] = -1;
data.linear_acceleration_covariance[0] = -1;
// Publish
imu_pub_.publish(data);
}
void PhysicsBody::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & WorldFieldMask)
{
if(_BodyID != 0)
{
dBodyDestroy(_BodyID);
}
if(getWorld() != NULL)
{
_BodyID = dBodyCreate(getWorld()->getWorldID());
}
}
if( ((whichField & PositionFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetPosition(_BodyID, getPosition().x(),getPosition().y(),getPosition().z());
}
if( ((whichField & RotationFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMatrix3 rotation;
Vec4f v1 = getRotation()[0];
Vec4f v2 = getRotation()[1];
Vec4f v3 = getRotation()[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dBodySetRotation(_BodyID, rotation);
}
if( ((whichField & QuaternionFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dQuaternion q;
q[0]=getQuaternion().w();
q[1]=getQuaternion().x();
q[2]=getQuaternion().y();
q[3]=getQuaternion().z();
dBodySetQuaternion(_BodyID,q);
}
if( ((whichField & LinearVelFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearVel(_BodyID, getLinearVel().x(),getLinearVel().y(),getLinearVel().z());
}
if( ((whichField & AngularVelFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularVel(_BodyID, getAngularVel().x(),getAngularVel().y(),getAngularVel().z());
}
if( ((whichField & ForceFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetForce(_BodyID, getForce().x(),getForce().y(),getForce().z());
}
if( ((whichField & TorqueFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetTorque(_BodyID, getTorque().x(),getTorque().y(),getTorque().z());
}
if( ((whichField & AutoDisableFlagFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableFlag(_BodyID, getAutoDisableFlag());
}
if( ((whichField & AutoDisableLinearThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableLinearThreshold(_BodyID, getAutoDisableLinearThreshold());
}
if( ((whichField & AutoDisableAngularThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableAngularThreshold(_BodyID, getAutoDisableAngularThreshold());
}
if( ((whichField & AutoDisableStepsFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableSteps(_BodyID, getAutoDisableSteps());
}
if( ((whichField & AutoDisableTimeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableTime(_BodyID, getAutoDisableTime());
}
if( ((whichField & FiniteRotationModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode());
}
if( ((whichField & FiniteRotationModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode());
}
if( ((whichField & FiniteRotationAxisFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationAxis(_BodyID, getFiniteRotationAxis().x(),getFiniteRotationAxis().y(),getFiniteRotationAxis().z());
}
if( ((whichField & GravityModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetGravityMode(_BodyID, getGravityMode());
}
if( ((whichField & LinearDampingFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearDamping(_BodyID, getLinearDamping());
}
if( ((whichField & AngularDampingFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularDamping(_BodyID, getAngularDamping());
}
if( ((whichField & LinearDampingThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearDampingThreshold(_BodyID, getLinearDampingThreshold());
}
if( ((whichField & AngularDampingThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularDampingThreshold(_BodyID, getAngularDampingThreshold());
}
if( ((whichField & MaxAngularSpeedFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
if(getMaxAngularSpeed() >= 0.0)
{
dBodySetMaxAngularSpeed(_BodyID, getMaxAngularSpeed());
}
else
{
dBodySetMaxAngularSpeed(_BodyID, dInfinity);
}
}
if( ((whichField & MassFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMass TheMass;
dBodyGetMass(_BodyID, &TheMass);
TheMass.mass = getMass();
dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & MassCenterOfGravityFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
//dMass TheMass;
//dBodyGetMass(_BodyID, &TheMass);
////TheMass.c[0] = getMassCenterOfGravity().x();
////TheMass.c[1] = getMassCenterOfGravity().y();
////TheMass.c[2] = getMassCenterOfGravity().z();
//Vec4f v1 = getMassInertiaTensor()[0];
//Vec4f v2 = getMassInertiaTensor()[1];
//Vec4f v3 = getMassInertiaTensor()[2];
//TheMass.I[0] = v1.x();
//TheMass.I[1] = v1.y();
//TheMass.I[2] = v1.z();
//TheMass.I[3] = getMassCenterOfGravity().x();
//TheMass.I[4] = v2.x();
//TheMass.I[5] = v2.y();
//TheMass.I[6] = v2.z();
//TheMass.I[7] = getMassCenterOfGravity().y();
//TheMass.I[8] = v3.x();
//TheMass.I[9] = v3.y();
//TheMass.I[10] = v3.z();
//TheMass.I[11] = getMassCenterOfGravity().z();
//dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & MassInertiaTensorFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMass TheMass;
dBodyGetMass(_BodyID, &TheMass);
Vec4f v1 = getMassInertiaTensor()[0];
Vec4f v2 = getMassInertiaTensor()[1];
Vec4f v3 = getMassInertiaTensor()[2];
TheMass.I[0] = v1.x();
TheMass.I[1] = v1.y();
TheMass.I[2] = v1.z();
TheMass.I[3] = 0;
TheMass.I[4] = v2.x();
TheMass.I[5] = v2.y();
TheMass.I[6] = v2.z();
TheMass.I[7] = 0;
TheMass.I[8] = v3.x();
TheMass.I[9] = v3.y();
TheMass.I[10] = v3.z();
TheMass.I[11] = 0;
dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & KinematicFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
if(getKinematic())
{
dBodySetKinematic(_BodyID);
}
else
{
dBodySetDynamic(_BodyID);
}
}
}
