void Gl1_ChainedCylinder::go(const shared_ptr<Shape>& cm, const shared_ptr<State>& state,bool wire2, const GLViewInfo&)
{
Real r=(static_cast<ChainedCylinder*>(cm.get()))->radius;
Real length=(static_cast<ChainedCylinder*>(cm.get()))->length;
Quaternionr shift;// = (static_cast<ChainedCylinder*>(cm.get()))->chainedOrientation;
shift.setFromTwoVectors(Vector3r::UnitZ(),state->ori.conjugate()*(static_cast<ChainedCylinder*>(cm.get()))->segment);
glColor3v(cm->color);
if(glutNormalize) glPushAttrib(GL_NORMALIZE);
if (wire || wire2) drawCylinder(true, r,length,shift);
else drawCylinder(false, r,length,shift);
if(glutNormalize) glPopAttrib();
return;
}
/* 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);
}
}
std::string tq2pov(const Vector3r& t, const Quaternionr& q){
Matrix3r m=q.toRotationMatrix();
return "matrix <"
+to_string(m(0,0))+","+to_string(m(0,1))+","+to_string(m(0,2))+", "
+to_string(m(1,0))+","+to_string(m(1,1))+","+to_string(m(1,2))+", "
+to_string(m(2,0))+","+to_string(m(2,1))+","+to_string(m(2,2))+", "
+to_string(t[0])+","+to_string(t[1])+","+to_string(t[2])+">";
}
void Gl1_GridConnection::go(const shared_ptr<Shape>& cm, const shared_ptr<State>& st ,bool wire2, const GLViewInfo&)
{
GridConnection *GC=static_cast<GridConnection*>(cm.get());
Real r=GC->radius;
Real length=GC->getLength();
Vector3r segt = GC->node2->state->pos - GC->node1->state->pos;
//glMaterialv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, Vector3f(cm->color[0],cm->color[1],cm->color[2]));
const shared_ptr<Interaction> intr = scene->interactions->find((int)GC->node1->getId(),(int)GC->node2->getId());
glColor3v(cm->color);
if(glutNormalize) glPushAttrib(GL_NORMALIZE);
// glPushMatrix();
Quaternionr shift;
shift.setFromTwoVectors(Vector3r::UnitZ(),segt);
if(intr){drawCylinder(wire || wire2, r,length,shift);}
if(glutNormalize) glPopAttrib();
// glPopMatrix();
return;
}
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();
}
msr::airlib::VehicleCameraBase::ImageResponse to() const
{
msr::airlib::VehicleCameraBase::ImageResponse d;
d.pixels_as_float = pixels_as_float;
if (! pixels_as_float)
d.image_data_uint8 = image_data_uint8;
else
d.image_data_float = image_data_float;
d.camera_position = camera_position.to();
d.camera_orientation = camera_orientation.to();
d.time_stamp = time_stamp;
d.message = message;
d.compress = compress;
d.width = width;
d.height = height;
d.image_type = image_type;
return d;
}
/*! @brief Recalculate body's inertia tensor in rotated coordinates.
*
* @param I inertia tensor in old coordinates
* @param rot quaternion that describes rotation from old to new coordinates
* @return inertia tensor in new coordinates
*/
Matrix3r woo::Volumetric::inertiaTensorRotate(const Matrix3r& I, const Quaternionr& rot){
Matrix3r T=rot.toRotationMatrix();
return inertiaTensorRotate(I,T);
}
// http://www.euclideanspace.com/physics/kinematics/angularvelocity/QuaternionDifferentiation2.pdf
Quaternionr Leapfrog::DotQ(const Vector3r& angVel, const Quaternionr& Q){
Quaternionr dotQ;
dotQ.w() = (-Q.x()*angVel[0]-Q.y()*angVel[1]-Q.z()*angVel[2])/2;
dotQ.x() = ( Q.w()*angVel[0]-Q.z()*angVel[1]+Q.y()*angVel[2])/2;
dotQ.y() = ( Q.z()*angVel[0]+Q.w()*angVel[1]-Q.x()*angVel[2])/2;
dotQ.z() = (-Q.y()*angVel[0]+Q.x()*angVel[1]+Q.w()*angVel[2])/2;
return dotQ;
}
/*
Solve the initial value problems in order to set up continuity constraints,
which effectively store the system dynamics for this SQP iteration.
At the same time, compute the Jacobian function by applying perturbations to
each of the variables in turn, for use in the continuity constraints.
*/
void OptimalControlProblem::solve_ivps(uint32_t i) {
uint32_t j;
/* Solve the initial value problem at this horizon step. */
integrated_state_horizon[i] = integrator.integrate(
State(state_reference[i]),
control_reference[i],
dynamics,
OCP_STEP_LENGTH);
for(j = 0; j < NMPC_GRADIENT_DIM; j++) {
ReferenceVector perturbed_state;
perturbed_state.segment<NMPC_STATE_DIM>(0) = state_reference[i];
perturbed_state.segment<NMPC_CONTROL_DIM>(NMPC_STATE_DIM) =
control_reference[i];
StateVector new_state;
real_t perturbation = NMPC_EPS_4RT;
/* Need to calculate quaternion perturbations using MRPs. */
if(j < 6) {
perturbed_state[j] += perturbation;
} else if(j >= 6 && j <= 8) {
Vector3r d_p;
d_p << 0.0, 0.0, 0.0;
d_p[j-6] = perturbation;
real_t x_2 = d_p.squaredNorm();
real_t delta_w = (-NMPC_MRP_A * x_2 + NMPC_MRP_F * std::sqrt(
NMPC_MRP_F_2 + ((real_t)1.0 - NMPC_MRP_A_2) * x_2)) /
(NMPC_MRP_F_2 + x_2);
Vector3r delta_xyz = (((real_t)1.0 / NMPC_MRP_F) *
(NMPC_MRP_A + delta_w)) * d_p;
Quaternionr delta_q;
delta_q.vec() = delta_xyz;
delta_q.w() = delta_w;
Quaternionr temp = delta_q *
Quaternionr(perturbed_state.segment<4>(6));
perturbed_state.segment<4>(6) << temp.vec(), temp.w();
} else if(j < NMPC_DELTA_DIM) {
perturbed_state[j+1] += perturbation;
} else {
/*
Perturbations for the control inputs should be proportional
to the control range to make sure we don't lose too much
precision.
*/
perturbation *=
(upper_control_bound[j-NMPC_DELTA_DIM] -
lower_control_bound[j-NMPC_DELTA_DIM]);
perturbed_state[j+1] += perturbation;
}
new_state.segment<NMPC_STATE_DIM>(0) = integrator.integrate(
State(perturbed_state.segment<NMPC_STATE_DIM>(0)),
perturbed_state.segment<NMPC_CONTROL_DIM>(NMPC_STATE_DIM),
dynamics,
OCP_STEP_LENGTH);
/*
Calculate delta between perturbed state and original state, to
yield a full column of the Jacobian matrix.
*/
jacobians[i].col(j) =
state_to_delta(integrated_state_horizon[i], new_state) /
perturbation;
}
/*
Calculate integration residuals; these are needed for the continuity
constraints.
*/
integration_residuals[i] = state_to_delta(
state_reference[i+1],
integrated_state_horizon[i]);
}
