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