// Compute the a posteriori estimate of the system state, as well as
// the a posteriori estimate error variance. Version for
// one-dimensional systems without control input on the system.
//
// @param phiValue State transition gain.
// @param processNoiseVariance Process noise variance.
// @param measurement Measurement value.
// @param measurementsGain Measurements gain.
// @param measurementsNoiseVariance Measurements noise variance.
//
// @return
// 0 if OK
// -1 if problems arose
//
int SimpleKalmanFilter::Compute( const double& phiValue,
const double& processNoiseVariance,
const double& measurement,
const double& measurementsGain,
const double& measurementsNoiseVariance )
throw(InvalidSolver)
{
try
{
Predict( phiValue,
xhat(0),
processNoiseVariance );
Correct( measurement,
measurementsGain,
measurementsNoiseVariance );
}
catch(InvalidSolver e)
{
GPSTK_THROW(e);
return -1;
}
return 0;
} // End of method 'SimpleKalmanFilter::Compute()'
void FifthCKF::FifthCKFfiltering(double *z, UINT m_length)
{
CMatrix Pplus(4, 4);
Pplus = glb.Pplus;
CMatrix xhat(4, 1);
xhat = glb.xhat;
StoreData *fifthckf = new StoreData(xhat(1, 1), xhat(2, 1), xhat(3, 1), xhat(4, 1));
X.Add(fifthckf);
CMatrix Shat(dim, dim);
CMatrix rjpoint1(dim, 1);
CMatrix rjpoint2(dim, cpoints);
CMatrix rjpoint3(dim, hcpoints - cpoints - 1);
CMatrix Xminus1(dim, 1);
CMatrix Xminus2(dim, cpoints);
CMatrix Repmat2(dim, cpoints);
CMatrix Xminus3(dim, hcpoints - cpoints - 1);
CMatrix Repmat3(dim, hcpoints - cpoints - 1);
CMatrix jtemp(dim, 1);
CMatrix Zhat1(1, 1);
CMatrix Zhat2(1, cpoints);
CMatrix RepmatZ2(1, cpoints);
CMatrix Zhat3(1, hcpoints - cpoints - 1);
CMatrix RepmatZ3(1, hcpoints - cpoints - 1);
CMatrix W(dim, 1);
CMatrix Pz(1, 1);
CMatrix Xtemp(dim, 1);
CMatrix Pxz(4, 1);
int num = 1;//第0个未知存放的是占位符0，并不是真正的测量值，因此应当从下标1开始取测量值
for (UINT count = TimeInterval; count <= m_length; count += TimeInterval)
{
//Time Update
//Evaluate the Cholesky factor
Shat = Pplus.Cholesky();
//Evaluate the cubature points and the propagated cubature points//Estimate the predicted state
CMatrix CX(4, 1);
for (int i = 1; i <= hcpoints; ++i)
{
for (int j = 1; j <= dim; ++j)
{
jtemp(j, 1) = glb.kesi_FifthCKF(j, i);
}
jtemp = Shat * jtemp + xhat;
Xtemp = glb.fai * jtemp;
if ( 1 == i)
{
for (int j = 1; j <= dim; ++j)
{
Xminus1(j, 1) = Xtemp(j, 1);
}
CX = CX + c5_w2 * Xtemp;
}
else if (i > 1 && i <= cpoints + 1)//此编程风格会提高代码的冗余度，但提高了程序的运行效率，下同
{
for (int j = 1; j <= dim; ++j)
{
Xminus2(j, i - 1) = Xtemp(j, 1);
}
CX = CX + c5_w1 * Xtemp;
}
else
{
for (int j = 1; j <= dim; ++j)
{
Xminus3(j, i - cpoints - 1) = Xtemp(j, 1);
}
CX = CX + c5_w4 * Xtemp;
}
}
//xhat = CX;
//Estimate the predicted error covariance
for (int i = 1; i <= cpoints; ++i)
{
#pragma omp parallel for
for (int j = 1; j <= dim; ++j)
{
Repmat2(j, i) = CX(j, 1);
}
}
for (int i = 1; i <= hcpoints - cpoints - 1; ++i)
{
#pragma omp parallel for
for(int j = 1; j <= dim; ++j)
{
Repmat3(j, i) = CX(j, 1);
}
}
Pplus = c5_w2 * ((Xminus1 - CX) * (~(Xminus1 - CX))) +
c5_w1 * (Xminus2 - Repmat2) * (~(Xminus2 - Repmat2)) +
c5_w4 * (Xminus3 - Repmat3) * (~(Xminus3 - Repmat3)) + glb.gama * glb.Im * (~glb.gama);
//Measurement Update
//Evaluate the Cholesky factor
Shat = Pplus.Cholesky();
//Evaluate the cubature points and the propagated cubature points//Estimate the predicted measurement
CMatrix Z(1, 1);
for (int i = 1; i <= hcpoints; ++i)
{
for (int j = 1; j <= dim; ++j)
{
jtemp(j, 1) = glb.kesi_FifthCKF(j, i);
}
jtemp = Shat * jtemp + CX;
if (1 == i)
{
Zhat1(1, 1) = atan(jtemp(3, 1) / jtemp(1, 1));
Z(1, 1) = Z(1, 1) + c5_w2 * Zhat1(1, 1);
#pragma omp parallel for
for (int j = 1; j<= dim; ++j)
{
rjpoint1(j, 1) = jtemp(j, 1);
}
}
else if (i > 1 && i <= cpoints + 1)
{
Zhat2(1, i - 1) = atan(jtemp(3, 1) / jtemp(1, 1));
Z(1, 1) = Z(1, 1) + c5_w1 * Zhat2(1, i - 1);
#pragma omp parallel for
for (int j = 1; j <= dim; ++j)
{
rjpoint2(j, i - 1) = jtemp(j, 1);
}
}
else
{
Zhat3(1, i - cpoints - 1) = atan(jtemp(3, 1) / jtemp(1, 1));
Z(1, 1) = Z(1, 1) + c5_w4 * Zhat3(1, i - cpoints - 1);
#pragma omp parallel for
for (int j = 1; j <= dim; ++j)
{
rjpoint3(j, i - cpoints - 1) = jtemp(j, 1);
}
}
}
//Estimate the innovation covariance matrix
Pz(1, 1) = Rn;//For saving memory
#pragma omp parallel for
for (int i = 1; i <= cpoints; ++i)
{
RepmatZ2(1, i) = Z(1, 1);
}
#pragma omp parallel for
for (int i = 1; i <= hcpoints - cpoints - 1; ++i)
{
RepmatZ3(1, i) = Z(1, 1);
}
Pz = c5_w2 * (Zhat1 - Z) * (~(Zhat1 - Z)) +
c5_w1 * (Zhat2 - RepmatZ2)* (~(Zhat2 - RepmatZ2)) +
c5_w4 * (Zhat3 - RepmatZ3) * (~(Zhat3 - RepmatZ3)) + Pz;
//Estimate the cross-covariance matrix
Pxz = c5_w2 * (rjpoint1 - CX) * (~(Zhat1 - Z)) +
c5_w1 *(rjpoint2 - Repmat2) * (~(Zhat2 - RepmatZ2)) +
c5_w4 * (rjpoint3 - Repmat3) * (~(Zhat3 - RepmatZ3));
//Estimate the Kalman gain
W = ((double)1 / Pz(1, 1)) * Pxz;
//Estimate the updated state
//Znum(1, 1) = z[num];
xhat = CX + W * (z[num] - Z(1, 1));
++num;
Pplus = Pplus - Pz(1, 1) * W * (~W);
StoreData *fifthckf = new StoreData(xhat(1, 1), xhat(2, 1), xhat(3, 1), xhat(4, 1));
X.Add(fifthckf);
}
}
void main(void)
{
// indices for nodes in the scene
static const int TARGET_GEOM_INDEX = 1;
static const int HAND_GEOM_INDEX = 4;
static const int OBJECT_GEOM_INDEX = 13;
enum StateMachine
{
Default,
AlignPerpendicularToObject,
ApproachingObject,
OpenGrip,
CloseGrip,
ClosingGrip,
MoveTowardsTarget,
};
StateMachine stateMachine = Default;
// connect to mujoco server.
//
mjInit();
mjConnect(10000);
double lTarget = 0.0;
double rTarget = 0.0;
// load hand model.
//
if (mjIsConnected())
{
mjLoad("hand.xml");
Sleep(1000); // wait till the load is complete
}
if (mjIsConnected() && mjIsModel())
{
mjSetMode(2);
mjReset();
// size containts model dimensions.
//
mjSize size = mjGetSize();
// number of arm degress of freedom (DOFs) and controls
// (does not include target object degrees of freedom)
//
int dimtheta = size.nu;
// identity matrix (useful for implementing Jacobian pseudoinverse methods).
//
Matrix I(dimtheta, dimtheta);
I.setIdentity();
// Zero Reference vector.
//
Vector ZeroVector(dimtheta);
ZeroVector.setConstant(0.0);
// target arm DOFs.
//
Vector thetahat(dimtheta); thetahat.setConstant(0.0);
for (;;) // run simulation forever
{
// simulation advance substep.
//
mjStep1();
mjState state = mjGetState();
// current arm degrees of freedom.
//
Vector theta(dimtheta);
// state.qpos contains DOFs for the whole system. Here extract
// only DOFs for the arm into theta.
//
for (int e = 0; e<dimtheta; e++)
{
theta[e] = state.qpos[e];
}
mjCartesian geoms = mjGetGeoms();
// current hand/gripper position.
//
Vector x(3, geoms.pos + 3 * HAND_GEOM_INDEX);
// Target blue object position - which has the object to pickup
//
Vector xhatObject(3, geoms.pos + 3 * OBJECT_GEOM_INDEX);
// Target Green marker position.
//
Vector xhat(3, geoms.pos + 3 * TARGET_GEOM_INDEX);
// current hand/gripper orientation
//
Quat r(geoms.quat + 4 * HAND_GEOM_INDEX);
// Blue Object orientation
//
Quat rhatObject(geoms.quat + 4 * OBJECT_GEOM_INDEX);
// Create a quartenion which represents a rotation by 90 degrees around X-axis.
//
Quat Rotation90(0.4, rhatObject[1] * 0.707, 0, 0);
// Except when the state machine is in the last stage of taking the object towards the target marker,
// mask the orientation of the BLUE object to be perpendicular to its actual orientation,
// rotate around X-axis, by 90 degrees.
//
if (stateMachine != MoveTowardsTarget)
{
rhatObject = Rotation90 * rhatObject;
}
// Target Green Marker orientation.
//
Quat rhat(geoms.quat + 4 * TARGET_GEOM_INDEX);
mjJacobian jacobians = mjJacGeom(HAND_GEOM_INDEX);
// current hand position Jacobian.
//
Matrix Jpos(3, jacobians.nv, jacobians.jacpos);
// current hand orientation Jacobian.
//
Matrix Jrot(3, jacobians.nv, jacobians.jacrot);
// extract only columns of the Jacobian that relate to arm DOFs
//
Jpos = Jpos.getBlock(0, 3, 0, dimtheta);
Jrot = Jrot.getBlock(0, 3, 0, dimtheta);
// -- your code goes here --
// set thetahat through Jacobian control methods here
// for part 4 of assignment, you may need to call setGrip(amount, thetahat) here
// thetahat should be filled by this point
// ----------------------------------------------------------------------------------------
// Homework Part 4
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// Step 1 - Align perpendicular to Capsule/Object orientation. Pseudo Inverse Method - Explicit Optimization Criterion, for orientation control
// ----------------------------------------------------------------------------------------
// Step 2 - Move towards the BLUE object, with combined position and orientation control. Pseudo Inverse Method - Explicit Optimization Criterion, for orientation control
// Orientation control remains perpendicular w.r.t BLUE object orientation.
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// Step 3 - When close enough to the BLUE object, open claws/set grip and continue combined position and orientation control. Pseudo Inverse Method - Explicit Optimization Criterion, for orientation control
// Orientation control remains perpendicular w.r.t BLUE object orientation.
// ----------------------------------------------------------------------------------------
// Step 4 - Close grip, wait till closed to enough extent, based on the target close setting of -0.4
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// Step 5 - Take object, manouvre combined position and orientation control to move towards target GREEN marker.
// Orientation control such that BLUE object is oriented w.r.t GREEN marker.
// ----------------------------------------------------------------------------------------
Vector rdelta;
rdelta = quatdiff(r, rhatObject);
switch (stateMachine)
{
case Default:
stateMachine = AlignPerpendicularToObject;
break;
}
// If aligned perpendicularly is complete (which means rdelta[0] == 0), proceed to next state, ApproachingObject.
//
if (rdelta[0] == 0.0 && stateMachine == AlignPerpendicularToObject)
{
stateMachine = ApproachingObject;
}
else if (stateMachine == AlignPerpendicularToObject)
{
// Continue with orientation control to complete step 1
//
// ----------------------------------------------------------------------------------------
// Step 1 - Align perpendicular to Capsule/Object orientation. Pseudo Inverse Method - Explicit Optimization Criterion, for orientation control
// ----------------------------------------------------------------------------------------
Matrix Jrottransponse = Jrot.transpose();
float alpha2 = 0.01;
Matrix JHash = Jrottransponse * ((Jrot * Jrottransponse).inverse());
Matrix JHashJ = JHash * Jrot;
Matrix Intermediate1 = I - JHashJ;
Vector Intermediate2 = ZeroVector - theta;
Vector RightTerm = Intermediate1 * Intermediate2;
Vector LeftTerm = (JHash * rdelta) * alpha2;
Vector deltaTheta = LeftTerm + RightTerm;
thetahat = deltaTheta + theta;
}
else if (stateMachine == ApproachingObject)
{
// ----------------------------------------------------------------------------------------
// Step 2 - Move towards the BLUE object, with combined position and orientation control. Pseudo Inverse Method - Explicit Optimization Criterion, for orientation control
// Orientation control remains perpendicular w.r.t BLUE object orientation.
// ----------------------------------------------------------------------------------------
Vector rDelta = quatdiff(r, rhatObject);
Vector xdelta = xhatObject - x;
// This threshold/constant is based on the size of the blue object
// and the size of the claws/fingers
// When close enough to the object, open the claws/gripper to grip the object.
//
if (xdelta[0] < 0.11)
{
stateMachine = OpenGrip;
}
else
{
Vector ConcatenatedXRDelta(xdelta.getSize() + rDelta.getSize());
ConcatenatedXRDelta[0] = xdelta[0];
ConcatenatedXRDelta[1] = xdelta[1];
ConcatenatedXRDelta[2] = xdelta[2];
ConcatenatedXRDelta[3] = rDelta[0];
ConcatenatedXRDelta[4] = rDelta[1];
ConcatenatedXRDelta[5] = rDelta[2];
assert(Jrot.getNumCols() == Jpos.getNumCols());
assert(Jrot.getNumRows() == Jpos.getNumRows());
Matrix ConcatenatedJacobMatrix(Jpos.getNumRows() + Jrot.getNumRows(), Jpos.getNumCols());
int i = 0, j = 0;
for (i = 0; i < Jpos.getNumRows(); i++)
{
ConcatenatedJacobMatrix.setRow(i, Jpos.getRow(i));
}
assert(i == Jpos.getNumRows());
for (j = 0; j < Jrot.getNumRows(); j++)
{
ConcatenatedJacobMatrix.setRow(i + j, Jrot.getRow(j));
}
assert(j == Jrot.getNumRows());
Matrix JConcatTranspose = ConcatenatedJacobMatrix.transpose();
float alpha5 = 0.006;
Matrix JHash = JConcatTranspose * ((ConcatenatedJacobMatrix * JConcatTranspose).inverse());
Matrix JHashJ = JHash * ConcatenatedJacobMatrix;
Matrix Intermediate1 = I - JHashJ;
Vector Intermediate2 = ZeroVector - theta;
Vector RightTerm = Intermediate1 * Intermediate2;
Vector LeftTerm = (JHash * ConcatenatedXRDelta) * alpha5;
Vector deltaTheta = LeftTerm + RightTerm;
thetahat = deltaTheta + theta;
}
}
else if (stateMachine == MoveTowardsTarget)
{
// ----------------------------------------------------------------------------------------
// Step 5 - Take object, manouvre combined position and orientation control to move towards target GREEN marker.
// Orientation control such that BLUE object is oriented w.r.t GREEN marker.
// ----------------------------------------------------------------------------------------
Vector rDelta = quatdiff(rhatObject, rhat);
Vector xdelta = xhat - xhatObject;
Vector ConcatenatedXRDelta(xdelta.getSize() + rDelta.getSize());
ConcatenatedXRDelta[0] = xdelta[0];
ConcatenatedXRDelta[1] = xdelta[1];
ConcatenatedXRDelta[2] = xdelta[2];
ConcatenatedXRDelta[3] = rDelta[0];
ConcatenatedXRDelta[4] = rDelta[1];
ConcatenatedXRDelta[5] = rDelta[2];
assert(Jrot.getNumCols() == Jpos.getNumCols());
assert(Jrot.getNumRows() == Jpos.getNumRows());
Matrix ConcatenatedJacobMatrix(Jpos.getNumRows() + Jrot.getNumRows(), Jpos.getNumCols());
int i = 0, j = 0;
for (i = 0; i < Jpos.getNumRows(); i++)
{
ConcatenatedJacobMatrix.setRow(i, Jpos.getRow(i));
}
assert(i == Jpos.getNumRows());
for (j = 0; j < Jrot.getNumRows(); j++)
{
ConcatenatedJacobMatrix.setRow(i + j, Jrot.getRow(j));
}
assert(j == Jrot.getNumRows());
Matrix JConcatTranspose = ConcatenatedJacobMatrix.transpose();
float alpha5 = 0.001;
Matrix JHash = JConcatTranspose * ((ConcatenatedJacobMatrix * JConcatTranspose).inverse());
Matrix JHashJ = JHash * ConcatenatedJacobMatrix;
Matrix Intermediate1 = I - JHashJ;
Vector Intermediate2 = ZeroVector - theta;
Vector RightTerm = Intermediate1 * Intermediate2;
Vector LeftTerm = (JHash * ConcatenatedXRDelta) * alpha5;
Vector deltaTheta = LeftTerm + RightTerm;
thetahat = deltaTheta + theta;
}
else if (stateMachine == OpenGrip)
{
// ----------------------------------------------------------------------------------------
// Step 3 - When close enough to the BLUE object, open claws/set grip and continue combined position and orientation control. Pseudo Inverse Method - Explicit Optimization Criterion, for orientation control
// Orientation control remains perpendicular w.r.t BLUE object orientation.
// ----------------------------------------------------------------------------------------
Vector rDelta = quatdiff(r, rhatObject);
Vector xdelta = xhatObject - x;
// This threshold/constant is based on the size of the blue object
// and the size of the claws/fingers
//
if (xdelta[0] < 0.036)
{
stateMachine = CloseGrip;
}
else
{
Vector ConcatenatedXRDelta(xdelta.getSize() + rDelta.getSize());
ConcatenatedXRDelta[0] = xdelta[0];
ConcatenatedXRDelta[1] = xdelta[1];
ConcatenatedXRDelta[2] = xdelta[2];
ConcatenatedXRDelta[3] = rDelta[0];
ConcatenatedXRDelta[4] = rDelta[1];
ConcatenatedXRDelta[5] = rDelta[2];
assert(Jrot.getNumCols() == Jpos.getNumCols());
assert(Jrot.getNumRows() == Jpos.getNumRows());
Matrix ConcatenatedJacobMatrix(Jpos.getNumRows() + Jrot.getNumRows(), Jpos.getNumCols());
int i = 0, j = 0;
for (i = 0; i < Jpos.getNumRows(); i++)
{
ConcatenatedJacobMatrix.setRow(i, Jpos.getRow(i));
}
assert(i == Jpos.getNumRows());
for (j = 0; j < Jrot.getNumRows(); j++)
{
ConcatenatedJacobMatrix.setRow(i + j, Jrot.getRow(j));
}
assert(j == Jrot.getNumRows());
Matrix JConcatTranspose = ConcatenatedJacobMatrix.transpose();
float alpha5 = 0.00004;
Matrix JHash = JConcatTranspose * ((ConcatenatedJacobMatrix * JConcatTranspose).inverse());
Matrix JHashJ = JHash * ConcatenatedJacobMatrix;
Matrix Intermediate1 = I - JHashJ;
Vector Intermediate2 = ZeroVector - theta;
Vector RightTerm = Intermediate1 * Intermediate2;
Vector LeftTerm = (JHash * ConcatenatedXRDelta) * alpha5;
Vector deltaTheta = LeftTerm + RightTerm;
thetahat = deltaTheta + theta;
}
}
if (stateMachine == OpenGrip)
{
// Open the claws of the gripper to be able to encapsulate the object/capsule.
//
setGrip(1, thetahat);
}
else if (stateMachine == CloseGrip)
{
// ----------------------------------------------------------------------------------------
// Step 4 - Close grip, wait till closed to enough extent, based on the target close setting of -0.4
// ----------------------------------------------------------------------------------------
// Close the grip to the extent of -0.4 as gripAmount.This number was arrived at
// as a function of the sizes of the capsule and the claw dimensions.
//
lTarget = 0.4;
rTarget = -0.4;
setGrip(-0.4, thetahat);
stateMachine = ClosingGrip;
}
else if (stateMachine == ClosingGrip)
{
// If the grip is almost close to the target values (lTarget, rTarget) at this point,
// Proceed with the next step - Moving towards the target.
// Else, proceed with ClosingGrip state.
//
if (abs(theta[7] - lTarget) < 0.01 && abs(rTarget - theta[8]) < 0.01)
{
stateMachine = MoveTowardsTarget;
}
}
// set target DOFs to thetahat and advance simulation.
//
mjSetControl(dimtheta, thetahat);
mjStep2();
}
mjDisconnect();
}
mjClear();
}
/* main routine */
int main(int argc, char *argv[]){
FILELog::ReportingLevel() = logINFO;
VARIABLES vars;
vars = commandline_input(argc, argv);
clock_t init, final;
int number_of_particles = vars.num;
double timestep = vars.dt;
bool use_T2 = vars.use_T2; // = false (no T2 decay), = true (T2 decay)
int num_of_repeat = vars.num_of_repeat; //Number of times to repeat simulation. For every repeat all data is flushed and we start the simulation again.
int number_of_timesteps; number_of_timesteps = (int)ceil(vars.gs/timestep);
double permeability = 0.0;
double radius = .00535;
double d = sqrt( 2.0*PI*radius*radius/( sqrt(3.0)*.79 ) );
//double lattice_size = 0.00601922026995422789215053328651;
double grad_duration = 4.5;
double D_extra = 2.5E-6;
double D_intra = 1.0E-6;
double T2_e = 200;
double T2_i = 200;
FILE_LOG(logINFO) << "f = " << 2.0*PI*radius*radius/(sqrt(3.0)*d*d) << std::endl;
Vector3 xhat(1.0,0.0,0.0);
Vector3 yhat(0.0,1.0,0.0);
Vector3 zhat(0.0,0.0,1.0);
Lattice<Cylinder_XY> lattice(D_extra, T2_e, permeability);
lattice.setLatticeVectors(d,2.0*d*0.86602540378443864676372317075294,d,xhat,yhat,zhat);
lattice.addBasis(Cylinder_XY(d/2.0, 0.0, radius, T2_i, D_intra, 1));
lattice.addBasis(Cylinder_XY(0.0, d*0.86602540378443864676372317075294, radius, T2_i, D_intra, 2));
lattice.addBasis(Cylinder_XY(d/2.0, 2.0*d*0.86602540378443864676372317075294, radius, T2_i, D_intra, 3));
lattice.addBasis(Cylinder_XY(d, d*0.86602540378443864676372317075294, radius, T2_i, D_intra, 4));
double gspacings [] = { 8.0 , 10.5 , 14.0 , 18.5 , 24.5 , 32.5 , 42.5 , 56.5 };
double bvals [] = {108780, 154720, 219040, 301730, 411980, 558990, 742420, 1000000 };
double G [9];
for (int kk = 0; kk < num_of_repeat; kk++) {
vector<PGSE> measurements_x;
vector<PGSE> measurements_y;
vector<PGSE> measurements_z;
for (int i = 0; i < 8; i++){
double echo_time = 2.0*grad_duration + gspacings[i];
G[0] = 0.0;
G[i+1] = sqrt(bvals[i]/(GAMMA*GAMMA*grad_duration*grad_duration*(grad_duration + gspacings[i] - (grad_duration/3.0))));
measurements_x.push_back(PGSE(grad_duration,gspacings[i], timestep, G[0], echo_time, number_of_particles, xhat));
measurements_y.push_back(PGSE(grad_duration,gspacings[i], timestep, G[0], echo_time, number_of_particles, yhat));
measurements_z.push_back(PGSE(grad_duration,gspacings[i], timestep, G[0], echo_time, number_of_particles, zhat));
measurements_x.push_back(PGSE(grad_duration,gspacings[i], timestep, G[i+1], echo_time, number_of_particles, xhat));
measurements_y.push_back(PGSE(grad_duration,gspacings[i], timestep, G[i+1], echo_time, number_of_particles, yhat));
measurements_z.push_back(PGSE(grad_duration,gspacings[i], timestep, G[i+1], echo_time, number_of_particles, zhat));
}
vector<double> lnsignal(2);
vector<double> b(2);
cout << " trial = " << kk << endl;
Particles ensemble(number_of_particles,timestep, use_T2);
lattice.initializeUniformly(ensemble.getGenerator() , ensemble.getEnsemble() );
for (int k = 0; k < measurements_x.size();k++){
measurements_x[k].updatePhase(ensemble.getEnsemble(), 0.0);
measurements_y[k].updatePhase(ensemble.getEnsemble(), 0.0);
measurements_z[k].updatePhase(ensemble.getEnsemble(), 0.0);
}
for (int i = 1; i <= number_of_timesteps; i++){
ensemble.updateposition(lattice);
for (int k = 0; k < measurements_x.size();k++){
measurements_x[k].updatePhase(ensemble.getEnsemble(), i*timestep);
measurements_y[k].updatePhase(ensemble.getEnsemble(), i*timestep);
measurements_z[k].updatePhase(ensemble.getEnsemble(), i*timestep);
}
}
for (int i = 0; i < 8*2; i+=2){
double ADCx, ADCy, ADCz, diff_time;
lnsignal[0] = log(measurements_x[i].get_signal());
b[0] = measurements_x[i].get_b();
lnsignal[1] = log(measurements_x[i+1].get_signal());
b[1] = measurements_x[i+1].get_b();
ADCx = -1.0*linear_regression(lnsignal,b);
//std::cout << b[0] << " " << lnsignal[0] << " " << b[1] << " " << lnsignal[1] << " ";
lnsignal[0] = log(measurements_y[i].get_signal());
b[0] = measurements_y[i].get_b();
lnsignal[1] = log(measurements_y[i+1].get_signal());
b[1] = measurements_y[i+1].get_b();
ADCy = -1.0*linear_regression(lnsignal,b);
//std::cout << b[0] << " " << lnsignal[0] << " " << b[1] << " " << lnsignal[1] << " ";
lnsignal[0] = log(measurements_z[i].get_signal());
b[0] = measurements_z[i].get_b();
lnsignal[1] = log(measurements_z[i+1].get_signal());
b[1] = measurements_z[i+1].get_b();
ADCz = -1.0*linear_regression(lnsignal,b);
//std::cout << b[0] << " " << lnsignal[0] << " " << b[1] << " " << lnsignal[1] << std::endl;
diff_time = measurements_x[i].get_DT();
std::cout << i << " " << diff_time << " " << ADCx << " " << ADCy << " " << ADCz << " " << b[1] << " " << G[1] << std::endl;
}
}
final= clock()-init; //final time - intial time
void main3(void)
{
// indices for nodes in the scene
static const int TARGET_GEOM_INDEX = 1;
static const int HAND_GEOM_INDEX = 4;
static const int OBJECT_GEOM_INDEX = 13;
// connect to mujoco server.
//
mjInit();
mjConnect(10000);
double lTarget = 0.0;
double rTarget = 0.0;
// load hand model.
//
if (mjIsConnected())
{
mjLoad("hand.xml");
Sleep(1000); // wait till the load is complete
}
if (mjIsConnected() && mjIsModel())
{
mjSetMode(2);
mjReset();
// size containts model dimensions.
//
mjSize size = mjGetSize();
// number of arm degress of freedom (DOFs) and controls
// (does not include target object degrees of freedom)
//
int dimtheta = size.nu;
// identity matrix (useful for implementing Jacobian pseudoinverse methods).
//
Matrix I(dimtheta, dimtheta);
I.setIdentity();
// Zero Reference vector.
//
Vector ZeroVector(dimtheta);
ZeroVector.setConstant(0.0);
// target arm DOFs.
//
Vector thetahat(dimtheta); thetahat.setConstant(0.0);
for (;;) // run simulation forever
{
// simulation advance substep.
//
mjStep1();
mjState state = mjGetState();
// current arm degrees of freedom.
//
Vector theta(dimtheta);
// state.qpos contains DOFs for the whole system. Here extract
// only DOFs for the arm into theta.
//
for (int e = 0; e<dimtheta; e++)
{
theta[e] = state.qpos[e];
}
mjCartesian geoms = mjGetGeoms();
// current hand/gripper position.
//
Vector x(3, geoms.pos + 3 * HAND_GEOM_INDEX);
// Target blue object position - which has the object to pickup
//
Vector xhatObject(3, geoms.pos + 3 * OBJECT_GEOM_INDEX);
// Target Green marker position.
//
Vector xhat(3, geoms.pos + 3 * TARGET_GEOM_INDEX);
// current hand/gripper orientation
//
Quat r(geoms.quat + 4 * HAND_GEOM_INDEX);
// Blue Object orientation
//
Quat rhatObject(geoms.quat + 4 * OBJECT_GEOM_INDEX);
// Target Green Marker orientation.
//
Quat rhat(geoms.quat + 4 * TARGET_GEOM_INDEX);
mjJacobian jacobians = mjJacGeom(HAND_GEOM_INDEX);
// current hand position Jacobian.
//
Matrix Jpos(3, jacobians.nv, jacobians.jacpos);
// current hand orientation Jacobian.
//
Matrix Jrot(3, jacobians.nv, jacobians.jacrot);
// extract only columns of the Jacobian that relate to arm DOFs
//
Jpos = Jpos.getBlock(0, 3, 0, dimtheta);
Jrot = Jrot.getBlock(0, 3, 0, dimtheta);
// -- your code goes here --
// set thetahat through Jacobian control methods here
// for part 4 of assignment, you may need to call setGrip(amount, thetahat) here
// thetahat should be filled by this point
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// Homework Part 3
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// Combined Position and Orientation Control - Common code for concatenating position and orientation variables
// to be used for all methods.
// ----------------------------------------------------------------------------------------
Vector rDelta = quatdiff(r, rhat);
Vector xdelta = xhat - x;
Vector ConcatenatedXRDelta(xdelta.getSize() + rDelta.getSize());
ConcatenatedXRDelta[0] = xdelta[0];
ConcatenatedXRDelta[1] = xdelta[1];
ConcatenatedXRDelta[2] = xdelta[2];
ConcatenatedXRDelta[3] = rDelta[0];
ConcatenatedXRDelta[4] = rDelta[1];
ConcatenatedXRDelta[5] = rDelta[2];
assert(Jrot.getNumCols() == Jpos.getNumCols());
assert(Jrot.getNumRows() == Jpos.getNumRows());
Matrix ConcatenatedJacobMatrix(Jpos.getNumRows() + Jrot.getNumRows(), Jpos.getNumCols());
int i = 0, j = 0;
for (i = 0; i < Jpos.getNumRows(); i++)
{
ConcatenatedJacobMatrix.setRow(i, Jpos.getRow(i));
}
assert(i == Jpos.getNumRows());
for (j = 0; j < Jrot.getNumRows(); j++)
{
ConcatenatedJacobMatrix.setRow(i + j, Jrot.getRow(j));
}
assert(j == Jrot.getNumRows());
// ----------------------------------------------------------------------------------------
// Combined Position and Orientation Control - Jacobian Transpose method
// ----------------------------------------------------------------------------------------
/*Matrix JConcatTranspose = ConcatenatedJacobMatrix.transpose();
float alpha3 = 0.1;
Vector deltatheta = (JConcatTranspose * ConcatenatedXRDelta) * alpha3;
thetahat = theta + deltatheta;*/
// ----------------------------------------------------------------------------------------
// Combined Position and Orientation Control - Pseudo Inverse Method - Basic Method (Lecture Slide Page 10)
// ----------------------------------------------------------------------------------------
/*Matrix JConcatTranspose = ConcatenatedJacobMatrix.transpose();
float alpha4 = 0.01;
Matrix JJConcatTransponseInverse = (ConcatenatedJacobMatrix * JConcatTranspose).inverse();
Vector deltaTheta = JConcatTranspose * (JJConcatTransponseInverse * ConcatenatedXRDelta); // Multiplied with xdelta to perform Matrix * vector rather than Matrix * Matrix
deltaTheta = deltaTheta * alpha4;
thetahat = deltaTheta + theta;*/
// ----------------------------------------------------------------------------------------
// Combined Position and Orientation Control - Pseudo Inverse Method - Explicit Optimization Criterion (Lecture Slide Page 13)
// ----------------------------------------------------------------------------------------
Matrix JConcatTranspose = ConcatenatedJacobMatrix.transpose();
float alpha5 = 0.01;
Matrix JHash = JConcatTranspose * ((ConcatenatedJacobMatrix * JConcatTranspose).inverse());
Matrix JHashJ = JHash * ConcatenatedJacobMatrix;
Matrix Intermediate1 = I - JHashJ;
Vector Intermediate2 = ZeroVector - theta;
Vector RightTerm = Intermediate1 * Intermediate2;
Vector LeftTerm = (JHash * ConcatenatedXRDelta) * alpha5;
Vector deltaTheta = LeftTerm + RightTerm;
thetahat = deltaTheta + theta;
// set target DOFs to thetahat and advance simulation.
//
mjSetControl(dimtheta, thetahat);
mjStep2();
}
mjDisconnect();
}
mjClear();
}
