DeltaVector OptimalControlProblem::state_to_delta(
const StateVector &s1, const StateVector &s2) {
DeltaVector delta;
delta.segment<6>(0) = s2.segment<6>(0) - s1.segment<6>(0);
/*
In order to increase the linearity of the problem and avoid quaternion
normalisation issues, we calculate the difference between attitudes as a
3-vector of Modified Rodrigues Parameters (MRP).
*/
Quaternionr err_q = (Quaternionr(s2.segment<4>(6)) *
Quaternionr(s1.segment<4>(6)).conjugate());
if(err_q.w() < 0) {
err_q = Quaternionr(-err_q.w(), -err_q.x(), -err_q.y(), -err_q.z());
}
delta.segment<3>(6) = NMPC_MRP_F *
(err_q.vec() / (NMPC_MRP_A + err_q.w()));
delta.segment<3>(9) = s2.segment<3>(10) - s1.segment<3>(10);
return delta;
}
/*
Runs the kinematics model on the state vector and returns a vector with the
derivative of each components (except for the accelerations, which must be
calculated directly using a dynamics model).
Contents are as follows:
- Rate of change in position (3-vector, m/s, NED frame)
- Rate of change in linear velocity (3-vector, m/s^2, NED frame)
- Rate of change in attitude (quaternion (x, y, z, w), 1/s, body frame)
- Rate of change in angular velocity (3-vector, rad/s^2, body frame)
*/
const StateVectorDerivative State::model(ControlVector c, DynamicsModel *d) {
StateVectorDerivative output;
AccelerationVector a = d->evaluate(*this, c);
/* Calculate change in position. */
output.segment<3>(0) << velocity();
/* Calculate change in velocity. */
Quaternionr attitude_q = Quaternionr(attitude());
output.segment<3>(3) = attitude_q.conjugate() * a.segment<3>(0);
/* Calculate change in attitude. */
Eigen::Matrix<real_t, 4, 1> omega_q;
omega_q << angular_velocity(), 0;
attitude_q = Quaternionr(omega_q).conjugate() * attitude_q;
output.segment<4>(6) << attitude_q.vec(), attitude_q.w();
output.segment<4>(6) *= 0.5;
/* Calculate change in angular velocity (just angular acceleration). */
output.segment<3>(10) = a.segment<3>(3);
return output;
}
void ukf_set_params(struct ioboard_params *in) {
model = IOBoardModel(
Quaternionr(
in->accel_orientation[0],
in->accel_orientation[1],
in->accel_orientation[2],
in->accel_orientation[3]),
Vector3r(
in->accel_offset[0],
in->accel_offset[1],
in->accel_offset[2]),
Quaternionr(
in->gyro_orientation[0],
in->gyro_orientation[1],
in->gyro_orientation[2],
in->gyro_orientation[3]),
Quaternionr(
in->mag_orientation[0],
in->mag_orientation[1],
in->mag_orientation[2],
in->mag_orientation[3]),
Vector3r(
in->mag_field[0],
in->mag_field[1],
in->mag_field[2]));
}
TEST(IOBoardModelTest, Instantiation) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0));
EXPECT_EQ(test.collate(), MeasurementVector());
}
/* Follows section 3.1, however we're using the scaled unscented transform. */
void UnscentedKalmanFilter::create_sigma_points() {
Eigen::Matrix<real_t, UKF_STATE_DIM, UKF_STATE_DIM> S;
AssertNormalized(Quaternionr(state.attitude()));
/* Add the process noise before calculating the square root. */
state_covariance.diagonal() += process_noise_covariance;
state_covariance *= UKF_DIM_PLUS_LAMBDA;
AssertPositiveDefinite(state_covariance);
/* Compute the 'square root' of the covariance matrix. */
S = state_covariance.llt().matrixL();
/* Create centre sigma point. */
sigma_points.col(0) = state;
/* For each column in S, create the two complementary sigma points. */
for(uint8_t i = 0; i < UKF_STATE_DIM; i++) {
/*
Construct error quaternions using the MRP method, equation 34 from the
Markley paper.
*/
Vector3r d_p = S.col(i).segment<3>(9);
real_t x_2 = d_p.squaredNorm();
real_t err_w = (-UKF_MRP_A * x_2 +
UKF_MRP_F * std::sqrt(UKF_MRP_F_2 + ((real_t)1.0 - UKF_MRP_A_2) * x_2)) /
(UKF_MRP_F_2 + x_2);
Vector3r err_xyz = (((real_t)1.0 / UKF_MRP_F) * (UKF_MRP_A + err_w)) * d_p;
Quaternionr noise;
noise.vec() = err_xyz;
noise.w() = err_w;
Quaternionr temp;
/* Create positive sigma point. */
sigma_points.col(i+1).segment<9>(0) =
state.segment<9>(0) + S.col(i).segment<9>(0);
temp = noise * Quaternionr(state.attitude());
AssertNormalized(temp);
sigma_points.col(i+1).segment<4>(9) << temp.vec(), temp.w();
sigma_points.col(i+1).segment<12>(13) =
state.segment<12>(13) + S.col(i).segment<12>(12);
/* Create negative sigma point. */
sigma_points.col(i+1+UKF_STATE_DIM).segment<9>(0) =
state.segment<9>(0) - S.col(i).segment<9>(0);
temp = noise.conjugate() * Quaternionr(state.attitude());
AssertNormalized(temp);
sigma_points.col(i+1+UKF_STATE_DIM).segment<4>(9) <<
temp.vec(), temp.w();
sigma_points.col(i+1+UKF_STATE_DIM).segment<12>(13) =
state.segment<12>(13) - S.col(i).segment<12>(12);
}
}
TEST(IOBoardModelTest, SetPitotTAS) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0));
MeasurementVector target(1);
test.set_pitot_tas(1);
target << 1;
EXPECT_EQ(test.collate(), target);
}
TEST(IOBoardModelTest, SetBarometerAMSL) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0));
MeasurementVector target(1);
test.set_barometer_amsl(1);
target << 1;
EXPECT_EQ(test.collate(), target);
}
TEST(IOBoardModelTest, SetMagnetometer) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0));
MeasurementVector target(3);
test.set_magnetometer(Vector3r(1, 2, 3));
target << 1, 2, 3;
EXPECT_EQ(test.collate(), target);
}
TEST(IOBoardModelTest, SetGPSVelocity) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0));
MeasurementVector target(3);
test.set_gps_velocity(Vector3r(1, 2, 3));
target << 1, 2, 3;
EXPECT_EQ(test.collate(), target);
}
TEST(IOBoardModelTest, SetThenClear) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0));
test.set_accelerometer(Vector3r(1, 2, 3));
test.set_gyroscope(Vector3r(4, 5, 6));
test.set_barometer_amsl(7);
test.clear();
EXPECT_EQ(test.collate(), MeasurementVector());
}
/* this function is called from field renderers for all field nodes */
void Renderer::setNodeGlData(const shared_ptr<Node>& n){
bool scaleRotations=(rotScale!=1.0 && scaleOn);
bool scaleDisplacements=(dispScale!=Vector3r::Ones() && scaleOn);
const bool isPeriodic=scene->isPeriodic;
if(!n->hasData<GlData>()) n->setData<GlData>(make_shared<GlData>());
GlData& gld=n->getData<GlData>();
// inside the cell if periodic, same as pos otherwise
Vector3r cellPos=(!isPeriodic ? n->pos : scene->cell->canonicalizePt(n->pos,gld.dCellDist));
bool rendered=!pointClipped(cellPos);
// this encodes that the node is clipped
if(!rendered){ gld.dGlPos=Vector3r(NaN,NaN,NaN); return; }
if(setRefNow || isnan(gld.refPos[0])) gld.refPos=n->pos;
if(setRefNow || isnan(gld.refOri.x())) gld.refOri=n->ori;
const Vector3r& pos=n->pos; const Vector3r& refPos=gld.refPos;
const Quaternionr& ori=n->ori; const Quaternionr& refOri=gld.refOri;
// if no scaling and no periodic, return quickly
if(!(scaleDisplacements||scaleRotations||isPeriodic)){ gld.dGlPos=Vector3r::Zero(); gld.dGlOri=Quaternionr::Identity(); return; }
// apply scaling
gld.dGlPos=cellPos-n->pos;
// add scaled translation to the point of reference
if(scaleDisplacements) gld.dGlPos+=((dispScale-Vector3r::Ones()).array()*Vector3r(pos-refPos).array()).matrix();
if(!scaleRotations) gld.dGlOri=Quaternionr::Identity();
else{
Quaternionr relRot=refOri.conjugate()*ori;
AngleAxisr aa(relRot);
aa.angle()*=rotScale;
gld.dGlOri=Quaternionr(aa);
}
}
TEST(IOBoardModelTest, SetMultipleInOrder) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0));
MeasurementVector target(7);
test.set_accelerometer(Vector3r(1, 2, 3));
test.set_gyroscope(Vector3r(4, 5, 6));
test.set_barometer_amsl(7);
target << 1, 2, 3, 4, 5, 6, 7;
EXPECT_EQ(test.collate(), target);
}
void OpenGLRenderer::setBodiesDispInfo(){
if(scene->bodies->size()!=bodyDisp.size()) {
bodyDisp.resize(scene->bodies->size());
for (unsigned k=0; k<scene->bodies->size(); k++) bodyDisp[k].hidden=0;}
bool scaleRotations=(rotScale!=1.0);
bool scaleDisplacements=(dispScale!=Vector3r::Ones());
FOREACH(const shared_ptr<Body>& b, *scene->bodies){
if(!b || !b->state) continue;
size_t id=b->getId();
const Vector3r& pos=b->state->pos; const Vector3r& refPos=b->state->refPos;
const Quaternionr& ori=b->state->ori; const Quaternionr& refOri=b->state->refOri;
Vector3r cellPos=(!scene->isPeriodic ? pos : scene->cell->wrapShearedPt(pos)); // inside the cell if periodic, same as pos otherwise
bodyDisp[id].isDisplayed=!pointClipped(cellPos);
// if no scaling and no periodic, return quickly
if(!(scaleDisplacements||scaleRotations||scene->isPeriodic)){ bodyDisp[id].pos=pos; bodyDisp[id].ori=ori; continue; }
// apply scaling
bodyDisp[id].pos=cellPos; // point of reference (inside the cell for periodic)
if(scaleDisplacements) bodyDisp[id].pos+=dispScale.cwiseProduct(Vector3r(pos-refPos)); // add scaled translation to the point of reference
if(!scaleRotations) bodyDisp[id].ori=ori;
else{
Quaternionr relRot=refOri.conjugate()*ori;
AngleAxisr aa(relRot);
aa.angle()*=rotScale;
bodyDisp[id].ori=refOri*Quaternionr(aa);
}
}
}
TEST(IOBoardModelTest, SetMultipleOutOfOrder) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0));
MeasurementVector target(10);
test.set_magnetometer(Vector3r(4, 5, 6));
test.set_accelerometer(Vector3r(1, 2, 3));
test.set_barometer_amsl(10);
test.set_gps_velocity(Vector3r(7, 8, 9));
target << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
EXPECT_EQ(test.collate(), target);
}
void ScGeom6D::initRotations(const State& state1, const State& state2)
{
initialOrientation1 = state1.ori;
initialOrientation2 = state2.ori;
twist=0;
bending=Vector3r::Zero();
twistCreep=Quaternionr(1.0,0.0,0.0,0.0);
}
void TensorGlRep::render(const shared_ptr<Node>& node, const GLViewInfo* viewInfo){
const int circDiv=20;
static gleDouble circ[circDiv+2][3]={};
if(circ[0][0]==0){
gleSetNumSides(10);
Real step=2*M_PI/circDiv;
for(int i=-1; i<circDiv+1; i++){
circ[i+1][0]=cos(i*step);
circ[i+1][1]=sin(i*step);
circ[i+1][2]=0;
}
}
if(range && !skewRange) skewRange=range;
Vector3r pos=node->pos+(node->hasData<GlData>()?node->getData<GlData>().dGlPos:Vector3r::Zero());
for(int i:{0,1,2}){
Vector3r color=(range?range->color(eigVal[i]):CompUtils::scalarOnColorScale(eigVal[i],-1,1));
if(isnan(color.maxCoeff())) continue;
Real mxNorm=(range?range->maxAbs(eigVal[i]):1);
Real len=relSz*viewInfo->sceneRadius;
len*=isnan(scaleExp)?abs(eigVal[i]/mxNorm):pow(abs(eigVal[i])/mxNorm,scaleExp);
glColor3v(color);
Vector3r dx(len*eigVec.col(i));
// arrows which go towards each other for negative eigenvalues, and away from each other for positive ones
GLUtils::GLDrawArrow(pos+(eigVal[i]>0?Vector3r::Zero():dx),pos+(eigVal[i]>0?dx:Vector3r::Zero()),color);
GLUtils::GLDrawArrow(pos-(eigVal[i]>0?Vector3r::Zero():dx),pos-(eigVal[i]>0?dx:Vector3r::Zero()),color);
// draw circular arrow to show skew components, in the half-height
// compute it in the xy plane, transform coords instead
Real maxSkew=(skewRange?skewRange->maxAbs(skew[i]):1);
// if(abs(skew[i])<.05*maxSkew) continue;
int nPts=int((abs(skew[i])/maxSkew)*circDiv*.5/* show max skew as .5*2π rad */-.5/*round evenly, but exclude one segment for arrow head*/);
if(nPts>circDiv-2) nPts=circDiv-2;
if(nPts<=0) continue;
Real torRad1=(skewRelSz>0?skewRelSz:relSz)*viewInfo->sceneRadius*(abs(skew[i])/maxSkew);
Real torDist=0*.3*torRad1; // draw both arcs in-plane
Real torRad2=torRad1*.1;
glColor3v(skewRange?skewRange->color(skew[i]):CompUtils::scalarOnColorScale(skew[i],-1,1));
for(int j:{0,1,2}){
glPushMatrix();
Eigen::Affine3d T=Eigen::Affine3d::Identity();
T.translate(pos+(j==0?1:-1)*eigVec.col(i)*torDist).rotate(Quaternionr().setFromTwoVectors(Vector3r::UnitZ(),eigVec.col(i)*(skew[i]>0?-1:1))).scale(torRad1);
if(j==1) T.rotate(AngleAxisr(M_PI,Vector3r::UnitZ()));
//GLUtils::setLocalCoords(pos+(j==0?1:-1)*eigVec.col(i)*torDist,
glMultMatrixd(T.data());
// since we scaled coords to transform unit circle coords to our radius, we will need to scale dimensions now by 1/torRad1
glePolyCylinder(nPts+2,circ,/* use current color*/NULL,torRad2*(1/torRad1));
gleDouble headRad[]={2*torRad2*(1/torRad1),2*torRad2*(1/torRad1),0,0,0};
glePolyCone(4,(gleDouble(*)[3])&(circ[nPts-1]),/*use current color*/NULL,headRad);
glPopMatrix();
}
}
}
/*! Draws a 3D arrow between the 3D point \p from and the 3D point \p to, both defined in the
current ModelView coordinates system.
See drawArrow(float length, float radius, int nbSubdivisions) for details. */
void GLUtils::QGLViewer::drawArrow(const Vector3r& from, const Vector3r& to, float radius, int nbSubdivisions)
{
glPushMatrix();
glTranslatef(from[0],from[1],from[2]);
Quaternionr q(Quaternionr().setFromTwoVectors(Vector3r(0,0,1),to-from));
//glMultMatrixd(q.toRotationMatrix().data());
glMultMatrix(q.toRotationMatrix().data());
drawArrow((to-from).norm(), radius, nbSubdivisions);
glPopMatrix();
}
/*
The final update step is calculated using section 3.6, specifically with
equations 74 and 75.
*/
void UnscentedKalmanFilter::aposteriori_estimate() {
ProcessCovariance update_temp;
/*
The update_temp vector will contain the attitude update as a vector, which
we need to apply the to quaternion section of the state vector.
*/
update_temp = kalman_gain * innovation;
/*
Update the attitude quaternion from the MRP vector, equation 45 from the
Markley paper.
*/
Vector3r d_p = update_temp.segment<3>(9);
real_t x_2 = d_p.squaredNorm();
real_t d_q_w = (-UKF_MRP_A * x_2 +
UKF_MRP_F * std::sqrt(UKF_MRP_F_2 + ((real_t)1.0 - UKF_MRP_A_2) * x_2)) /
(UKF_MRP_F_2 + x_2);
Vector3r d_q_xyz = (((real_t)1.0 / UKF_MRP_F) * (UKF_MRP_A + d_q_w)) * d_p;
Quaternionr d_q;
d_q.vec() = d_q_xyz;
d_q.w() = d_q_w;
Quaternionr update_q = d_q *
Quaternionr(sigma_points.col(0).segment<4>(9));
state.attitude() << update_q.vec(), update_q.w();
AssertNormalized(Quaternionr(state.attitude()));
/* The rest of the update step is very straightforward. */
state.segment<9>(0) = apriori_mean.segment<9>(0) +
update_temp.segment<9>(0);
state.segment<12>(13) = apriori_mean.segment<12>(13) +
update_temp.segment<12>(12);
/*
And the state covariance update from equation 75, no quaternion
manipulation necessary.
*/
state_covariance = apriori_covariance -
(kalman_gain * innovation_covariance * kalman_gain.transpose());
}
TEST(IOBoardModelTest, PredictGyroscope) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0));
State test_state;
test_state << 0, 0, 0,
0, 0, 0,
1, 2, 3,
0.7071, 0, 0, 0.7071,
4, 5, 6,
0, 0, 0,
0, 0, 0,
0, 0, 0;
test.set_gyroscope(Vector3r(0, 0, 0));
EXPECT_EQ(test.predict(test_state), Vector3r(4, 5, 6));
}
TEST(IOBoardModelTest, PredictBarometerAMSL) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(7, 8, 9));
State test_state;
test_state << -37, 145, 150,
7, 8, 9,
1, 2, 3,
0.7071, 0, 0, 0.7071,
4, 5, 6,
0, 0, 0,
1, 2, 3,
0, 0, 0;
test.set_barometer_amsl(0);
EXPECT_TRUE(abs(test.predict(test_state)(0) - 150) < 0.1);
}
TEST(IOBoardModelTest, PredictPitotTAS) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(7, 8, 9));
State test_state;
test_state << 0, 0, 0,
7, 8, 9,
1, 2, 3,
0.7071, 0, 0, 0.7071,
4, 5, 6,
0, 0, 0,
1, 2, 3,
0, 0, 0;
test.set_pitot_tas(0);
EXPECT_EQ(test.predict(test_state)(0), 6);
}
TEST(IOBoardModelTest, PredictMagnetometer) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(7, 8, 9));
State test_state;
test_state << 0, 0, 0,
0, 0, 0,
1, 2, 3,
0.7071, 0, 0, 0.7071,
4, 5, 6,
0, 0, 0,
0, 0, 0,
0, 0, 0;
test.set_magnetometer(Vector3r(0, 0, 0));
EXPECT_TRUE(test.predict(test_state).isApprox(
Vector3r(7, -9, 8), 0.001));
}
TEST(IOBoardModelTest, PredictGPSVelocity) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(7, 8, 9));
State test_state;
test_state << 0, 0, 0,
7, 8, 9,
1, 2, 3,
0.7071, 0, 0, 0.7071,
4, 5, 6,
0, 0, 0,
0, 0, 0,
0, 0, 0;
test.set_gps_velocity(Vector3r(0, 0, 0));
EXPECT_TRUE(test.predict(test_state).isApprox(
Vector3r(7, 8, 9), 0.001));
}
TEST(IOBoardModelTest, PredictGPSPosition) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(7, 8, 9));
State test_state;
test_state << -37, 145, 0,
0, 0, 0,
1, 2, 3,
0.7071, 0, 0, 0.7071,
4, 5, 6,
0, 0, 0,
0, 0, 0,
0, 0, 0;
test.set_gps_position(Vector3r(0, 0, 0));
EXPECT_TRUE(test.predict(test_state).isApprox(
Vector3r(-37, 145, 0), 0.001));
}
void Membrane::setRefConf(){
if(!node) node=make_shared<Node>();
// set initial node position and orientation
node->pos=this->getCentroid();
// for orientation, there is a few choices which one to pick
enum {INI_CS_SIMPLE=0,INI_CS_NODE0_AT_X};
const int ini_cs=INI_CS_NODE0_AT_X;
//const int ini_cs=INI_CS_SIMPLE;
switch(ini_cs){
case INI_CS_SIMPLE:
node->ori.setFromTwoVectors(Vector3r::UnitZ(),this->getNormal());
break;
case INI_CS_NODE0_AT_X:{
Matrix3r T;
T.col(0)=(nodes[0]->pos-node->pos).normalized(); // QQQ: row->col
T.col(2)=this->getNormal();
T.col(1)=T.col(2).cross(T.col(0));
assert(T.col(0).dot(T.col(2))<1e-12);
node->ori=Quaternionr(T);
break;
}
};
// reference nodal positions
for(int i:{0,1,2}){
// Vector3r nl=node->ori.conjugate()*(nodes[i]->pos-node->pos); // QQQ
Vector3r nl=node->glob2loc(nodes[i]->pos);
assert(nl[2]<1e-6*(max(abs(nl[0]),abs(nl[1])))); // z-coord should be zero
refPos.segment<2>(2*i)=nl.head<2>();
}
// reference nodal rotations
refRot.resize(3);
for(int i:{0,1,2}){
// facet node orientation minus vertex node orientation, in local frame (read backwards)
/// QQQ: refRot[i]=node->ori*nodes[i]->ori.conjugate();
refRot[i]=nodes[i]->ori.conjugate()*node->ori;
//LOG_WARN("refRot["<<i<<"]="<<AngleAxisr(refRot[i]).angle()<<"/"<<AngleAxisr(refRot[i]).axis().transpose());
};
// set displacements to zero
uXy=phiXy=Vector6r::Zero();
#ifdef MEMBRANE_DEBUG_ROT
drill=Vector3r::Zero();
currRot.resize(3);
#endif
// delete stiffness matrices to force their re-creating
KKcst.resize(0,0);
KKdkt.resize(0,0);
};
void woo::Volumetric::computePrincipalAxes(const Real& M, const Vector3r& Sg, const Matrix3r& Ig, Vector3r& pos, Quaternionr& ori, Vector3r& inertia){
assert(M>0); // LOG_TRACE("M=\n"<<M<<"\nIg=\n"<<Ig<<"\nSg=\n"<<Sg);
// clump's centroid
pos=Sg/M;
// this will calculate translation only, since rotation is zero
Matrix3r Ic_orientG=inertiaTensorTranslate(Ig, -M /* negative mass means towards centroid */, pos); // inertia at clump's centroid but with world orientation
//LOG_TRACE("Ic_orientG=\n"<<Ic_orientG);
Ic_orientG(1,0)=Ic_orientG(0,1); Ic_orientG(2,0)=Ic_orientG(0,2); Ic_orientG(2,1)=Ic_orientG(1,2); // symmetrize
Eigen::SelfAdjointEigenSolver<Matrix3r> decomposed(Ic_orientG);
const Matrix3r& R_g2c(decomposed.eigenvectors());
//cerr<<"R_g2c:"<<endl<<R_g2c<<endl;
// has NaNs for identity matrix??
//LOG_TRACE("R_g2c=\n"<<R_g2c);
// set quaternion from rotation matrix
ori=Quaternionr(R_g2c); ori.normalize();
inertia=decomposed.eigenvalues();
}
ImageResponse(const msr::airlib::VehicleCameraBase::ImageResponse& s)
{
pixels_as_float = s.pixels_as_float;
image_data_uint8 = s.image_data_uint8;
image_data_float = s.image_data_float;
//TODO: remove bug workaround for https://github.com/rpclib/rpclib/issues/152
if (image_data_uint8.size() == 0)
image_data_uint8.push_back(0);
if (image_data_float.size() == 0)
image_data_float.push_back(0);
camera_position = Vector3r(s.camera_position);
camera_orientation = Quaternionr(s.camera_orientation);
time_stamp = s.time_stamp;
message = s.message;
compress = s.compress;
width = s.width;
height = s.height;
image_type = s.image_type;
}
void Gl1_PolyhedraPhys::go(const shared_ptr<IPhys>& ip, const shared_ptr<Interaction>& i, const shared_ptr<Body>& b1, const shared_ptr<Body>& b2, bool wireFrame){
if(!gluQuadric){ gluQuadric=gluNewQuadric(); if(!gluQuadric) throw runtime_error("Gl1_PolyhedraPhys::go unable to allocate new GLUquadric object (out of memory?)."); }
PolyhedraPhys* np=static_cast<PolyhedraPhys*>(ip.get());
shared_ptr<IGeom> ig(i->geom); if(!ig) return; // changed meanwhile?
PolyhedraGeom* geom=YADE_CAST<PolyhedraGeom*>(ig.get());
Real fnNorm=np->normalForce.dot(geom->normal);
if((signFilter>0 && fnNorm<0) || (signFilter<0 && fnNorm>0)) return;
int fnSign=fnNorm>0?1:-1;
fnNorm=std::abs(fnNorm);
Real radiusScale=1.;
maxFn=max(fnNorm,maxFn);
Real realMaxRadius;
if(maxRadius<0){
refRadius=min(0.03,refRadius);
realMaxRadius=refRadius;
}
else realMaxRadius=maxRadius;
Real radius=radiusScale*realMaxRadius*(fnNorm/maxFn);
if (radius<=0.) radius = 1E-8;
Vector3r color=Shop::scalarOnColorScale(fnNorm*fnSign,-maxFn,maxFn);
Vector3r p1=b1->state->pos, p2=b2->state->pos;
Vector3r relPos;
relPos=p2-p1;
Real dist=relPos.norm();
glDisable(GL_CULL_FACE);
glPushMatrix();
glTranslatef(p1[0],p1[1],p1[2]);
Quaternionr q(Quaternionr().setFromTwoVectors(Vector3r(0,0,1),relPos/dist /* normalized */));
// using Transform with OpenGL: http://eigen.tuxfamily.org/dox/TutorialGeometry.html
//glMultMatrixd(Eigen::Affine3d(q).data());
glMultMatrix(Eigen::Transform<Real,3,Eigen::Affine>(q).data());
glColor3v(color);
gluCylinder(gluQuadric,radius,radius,dist,slices,stacks);
glPopMatrix();
}
TEST(IOBoardModelTest, PredictAccelerometer) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0));
State test_state;
test_state << 0, 0, 0,
0, 0, 0,
1, 2, 3,
0.7071, 0, 0, 0.7071,
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0;
Eigen::Matrix<real_t, 6, 1> target;
test.set_accelerometer(Vector3r(0, 0, 0));
target << 1, 2, 3, 0, G_ACCEL, 0;
EXPECT_TRUE(test.predict(test_state).isApprox(target, 0.001));
test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(1, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0));
test_state << 0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0, 1,
0, 0, 1,
0, 0, 1,
0, 0, 0,
0, 0, 0;
test.set_accelerometer(Vector3r(0, 0, 0));
target << -1, 1, 0, 0, 0, -G_ACCEL;
EXPECT_TRUE(test.predict(test_state).isApprox(target, 0.001));
}
void Gl1_Membrane::go(const shared_ptr<Shape>& sh, const Vector3r& shift, bool wire2, const GLViewInfo& viewInfo){
Gl1_Facet::go(sh,shift,/*don't force wire rendering*/ wire2,viewInfo);
if(Renderer::fastDraw) return;
Membrane& ff=sh->cast<Membrane>();
if(!ff.hasRefConf()) return;
if(node){
Renderer::setNodeGlData(ff.node,false/*set refPos only for the first time*/);
Renderer::renderRawNode(ff.node);
if(ff.node->rep) ff.node->rep->render(ff.node,&viewInfo);
#ifdef MEMBRANE_DEBUG_ROT
// show what Membrane thinks the orientation of nodes is - render those midway
if(ff.currRot.size()==3){
for(int i:{0,1,2}){
shared_ptr<Node> n=make_shared<Node>();
n->pos=ff.node->pos+.5*(ff.nodes[i]->pos-ff.node->pos);
n->ori=(ff.currRot[i]*ff.node->ori.conjugate()).conjugate();
Renderer::renderRawNode(n);
}
}
#endif
}
// draw everything in in local coords now
glPushMatrix();
if(!Renderer::scaleOn){
// without displacement scaling, local orientation is easy
GLUtils::setLocalCoords(ff.node->pos,ff.node->ori);
} else {
/* otherwise compute scaled orientation such that
* +x points to the same point on the triangle (in terms of angle)
* +z is normal to the facet in the GL space
*/
Real phi0=atan2(ff.refPos[1],ff.refPos[0]);
Vector3r C=ff.getGlCentroid();
Matrix3r rot;
rot.row(2)=ff.getGlNormal();
rot.row(0)=(ff.getGlVertex(0)-C).normalized();
rot.row(1)=rot.row(2).cross(rot.row(0));
Quaternionr ori=AngleAxisr(phi0,Vector3r::UnitZ())*Quaternionr(rot);
GLUtils::setLocalCoords(C,ori.conjugate());
}
if(refConf){
glColor3v(refColor);
glLineWidth(refWd);
glBegin(GL_LINE_LOOP);
for(int i:{0,1,2}) glVertex3v(Vector3r(ff.refPos[2*i],ff.refPos[2*i+1],0));
glEnd();
}
if(uScale!=0){
glLineWidth(uWd);
for(int i:{0,1,2}) drawLocalDisplacement(ff.refPos.segment<2>(2*i),uScale*ff.uXy.segment<2>(2*i),uRange,uSplit,arrows?1:0,uWd);
}
if(relPhi!=0 && ff.hasBending()){
glLineWidth(phiWd);
for(int i:{0,1,2}) drawLocalDisplacement(ff.refPos.segment<2>(2*i),relPhi*viewInfo.sceneRadius*ff.phiXy.segment<2>(2*i),phiRange,phiSplit,arrows?2:0,phiWd
#ifdef MEMBRANE_DEBUG_ROT
, relPhi*viewInfo.sceneRadius*ff.drill[i] /* show the out-of-plane component */
#endif
);
}
glPopMatrix();
}
