Eigen::Vector3d unit_normal(double theta, double phi) {
Eigen::Vector3d rhat(
std::sin(theta)*std::cos(phi),
std::sin(theta)*std::sin(phi),
std::cos(theta)
);
return rhat;
}
/**
* The problem with this is that it doesn't really know if it's already performed turnaround,
* and has no hysteresis, so it ends up wobbling its ass a lot, decelerating much more slowly
* than it should be able to, and overshooting the target.
*
* Also for reasons I can't figure out, it only accelerates for about the first third of the trajectory,
* instead of until halfway as it should.
*/
void SimpleShipMovementController::step(RocketShip& ship, point2 target) {
// Angular tolerance within which directions are considered equivalent.
const plane_angle direction_slop = ship.side_thruster_acceleration() * pow<2>(0.25*seconds);
ship.stop_turning();
ship.thrust_off();
vec2<length> r = target - ship.position();
unit_vec2 rhat(r);
vec2<velocity> dr = ship.velocity();
acceleration dv = ship.main_thruster_acceleration();
// Not sure WTF these expressions mean physically, but it's the result of hours of screwing around and seems to produce nice results.
unit_vec2 thrust_direction_towards(rhat*fudge_direction_ratio_ - dr.rejection(r)*((1-fudge_direction_ratio_)*second/meter));
unit_vec2 thrust_direction_away(-rhat*fudge_direction_ratio_ - dr.rejection(r)*((1-fudge_direction_ratio_)*second/meter));
time_interval time_to_target = r.magnitude() / dr.scalar_projection(r);
time_interval time_to_stop = dr.scalar_projection(r) / dv;
//plane_angle turnaround_delta_a = angle_difference(thrust_direction_away.angle(), ship.heading() + M_PI*radians);
//time_interval maximum_turnaround_time = .75*seconds; // FIXME: pull this number from somewhere other than my ass
//time_to_stop -= maximum_turnaround_time*(fabs(turnaround_delta_a) / (M_PI*radians)); // is this really linear wrt delta_a?
/*
if(fabs(turnaround_delta_a) > direction_slop) {
time_to_stop -= maximum_turnaround_time*(fabs(turnaround_delta_a) / (M_PI*radians));
}
*/
unit_vec2 thrust_direction(0,0);
if(time_to_target < 0*seconds || time_to_stop < time_to_target) {
// Before turnaround - thrust towards the target.
thrust_direction = thrust_direction_towards;
} else {
// After turnaround - thrust away from the target.
thrust_direction = thrust_direction_away;
}
point_at(ship, thrust_direction);
// Thrust if heading within tolerance
if(fabs(angle_difference(thrust_direction.angle(), ship.heading())) < direction_slop) {
ship.thrust_on();
}
// Finally,
ship.update_forces();
}
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();
}
Matrix<double> sparse_bicgstab(SpMatrix* A, Matrix<double> b, Matrix<double> xold, double tol, int maxiter)
// Solves general linear system Ax=b using stabilized biconjugate gradient method of van der Vorst
{
Matrix<double> rold(b.rows(), 1);
Matrix<double> r(b.rows(), 1);
Matrix<double> rhat(b.rows(), 1);
Matrix<double> t(b.rows(), 1);
Matrix<double> vold(b.rows(), 1);
Matrix<double> v(b.rows(), 1);
Matrix<double> pold(b.rows(), 1);
Matrix<double> p(b.rows(), 1);
Matrix<double> s(b.rows(), 1);
Matrix<double> errdiff(b.rows(), 1);
Matrix<double> x(b.rows(), 1);
double rhoold, rho, wold, w, alpha, beta;
double error, initres;
A->multiply(xold, &t); // t = A*xold, initially (t is only a temp variable here)
rold = b - t;
initres = error = rold.l2norm();
int steps = 0;
int i;
vold.zeros(); pold.zeros();
rhat = rold;
rhoold = alpha = wold = 1.0;
while(error > tol && steps <= maxiter)
{
if(steps % 10 == 0 || steps == 1)
cout << "sparse_bicgstab(): Iteration " << steps << ": relative error = " << error << endl;
rho = rhat.dot_product(rold);
beta = rho*alpha/(rhoold*wold);
for(i = 0; i < b.rows(); i++)
p(i) = rold.get(i) + beta * (pold.get(i) - wold*vold.get(i));
A->multiply(p, &v); // v = A*p
alpha = rho / rhat.dot_product(v);
for(i = 0; i < b.rows(); i++)
s(i) = rold.get(i) - alpha*v.get(i);
if(s.dabsmax() < tol*tol)
{
x = xold + p*alpha;
break;
}
A->multiply(s, &t); // t = A*s
w = t.dot_product(s)/t.dot_product(t);
for(i = 0; i < b.rows(); i++)
{
x(i) = xold.get(i) + alpha*p.get(i) + w*s.get(i);
error += (x.get(i) - xold.get(i)) * (x.get(i)-xold.get(i));
}
error = sqrt(error);
for(i = 0; i < b.rows(); i++)
r(i) = s.get(i) - w*t.get(i);
for(i = 0; i < b.rows(); i++)
{
xold(i) = x.get(i);
rold(i) = r.get(i);
vold(i) = v.get(i);
pold(i) = p.get(i);
}
rhoold = rho;
wold = w;
steps++;
}
cout << "sparse_bicgstab(): Done. Iterations: " << steps << ", final relative error: " << error << endl;
return x;
}
//-----------------------------------------------------------------------------
void PrBiCGStab::solve(const PrMatrix& A, PrVec& x, const PrVec& b)
//-----------------------------------------------------------------------------
{
double time0 = Go::getCurrentTime();
double tol = tolerance_ * tolerance_;
int n = x.size();
int j;
PrVec r(n);
//r = b - Ax
A.prod(x,r);
for(j=0; j<n; j++) r(j) = b(j) - r(j);
if(r.inner(r) < tol)
{
it_count_ = 0;
cpu_time_ = 0.0;
converged_ = true;
return;
}
PrVec rhat(n);
for(j=0; j<n; j++) rhat(j) = r(j);
double rho0 = 1.0, alpha = 1.0, omega0 = 1.0;
double rho1,omega1,beta;
PrVec s(n);
PrVec t(n);
// All these must be set to zero. Changed PrVec so it actually happens!
PrVec v0(n);
PrVec v1(n);
PrVec p0(n);
PrVec p1(n);
double snorm;
for(int i=1; i<= max_iterations_; i++)
{
//s_o << "i = " << i << endl;
rho1 = rhat.inner(r);
beta = (rho1 / rho0) * (alpha / omega0);
//p1 = r + beta * (p0 - omega0 * v0)
for(j=0; j<n; j++) p1(j) = r(j) + beta * (p0(j) - omega0 * v0(j));
//v1 = A * p1
A.prod(p1,v1);
alpha = rho1 / rhat.inner(v1);
//s = r - alpha * v1
for(j=0; j<n; j++) s(j) = r(j) - alpha * v1(j);
snorm = s.inner(s);
//s_o << "snorm = " << snorm << endl;
if(snorm < tol)
{
//x = x + alpha * p1
for(j=0; j<n; j++) x(j) += alpha * p1(j);
it_count_ = i;
cpu_time_ = Go::getCurrentTime() - time0;
converged_ = true;
return;
}
//t = A * s
A.prod(s,t);
omega1 = t.inner(s) / t.inner(t);
//x = x + alpha * p1 + omega1 * s
for(j=0; j<n; j++) x(j) += alpha * p1(j) + omega1 * s(j);
//r = s - omega1 * t
for(j=0; j<n; j++) r(j) = s(j) - omega1 * t(j);
rho0 = rho1;
omega0 = omega1;
//v0 = v1
for(j=0; j<n; j++) v0(j) = v1(j);
//p0 = p1
for(j=0; j<n; j++) p0(j) = p1(j);
}
it_count_ = max_iterations_;
cpu_time_ = Go::getCurrentTime() - time0;
converged_ = false;
}
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();
}
