Matrix<double,7,1> Sim3
::log(const Sim3& sim3)
{
Vector7d res;
double scale = sim3.scale();
Vector3d t = sim3.translation_;
double theta;
Vector4d omega_sigma = ScSO3::logAndTheta(sim3.scso3_, &theta);
Vector3d omega = omega_sigma.head<3>();
double sigma = omega_sigma[3];
Matrix3d Omega = SO3::hat(omega);
Matrix3d W = calcW(theta, sigma, scale, Omega);
//Vector3d upsilon = W.jacobiSvd(ComputeFullU | ComputeFullV).solve(t);
Vector3d upsilon = W.partialPivLu().solve(t);
res.segment(0,3) = upsilon;
res.segment(3,3) = omega;
res[6] = sigma;
return res;
}
bool EdgeSE3Offset::read(std::istream& is) {
int pidFrom, pidTo;
is >> pidFrom >> pidTo ;
if (! setParameterId(0,pidFrom))
return false;
if (! setParameterId(1,pidTo))
return false;
Vector7d meas;
for (int i=0; i<7; i++)
is >> meas[i];
// normalize the quaternion to recover numerical precision lost by storing as human readable text
Vector4d::MapType(meas.data()+3).normalize();
setMeasurement(internal::fromVectorQT(meas));
if (is.bad()) {
return false;
}
for ( int i=0; i<information().rows() && is.good(); i++)
for (int j=i; j<information().cols() && is.good(); j++){
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
if (is.bad()) {
// we overwrite the information matrix with the Identity
information().setIdentity();
}
return true;
}
static void getDOFLimits(RaveRobotObject::Ptr robot, const vector<int> &indices,
Vector7d &out_lower, Vector7d &out_upper) {
vector<double> lb(7), ub(7);
robot->robot->GetDOFLimits(lb, ub, indices);
out_lower = toEigVec(lb);
out_upper = toEigVec(ub);
cout << "dof limits: " << out_lower.transpose() << " | " << out_upper.transpose() << endl;
}
bool ParameterSE3Offset::read(std::istream& is) {
Vector7d off;
for (int i=0; i<7; i++) {
is >> off[i];
}
// normalize the quaternion to recover numerical precision lost by storing as human readable text
Vector4D::MapType(off.data()+3).normalize();
setOffset(internal::fromVectorQT(off));
return is.good();
}
bool ParameterCamera::read(std::istream& is) {
Vector7d off;
for (int i=0; i<7; i++)
is >> off[i];
// normalize the quaternion to recover numerical precision lost by storing as human readable text
Vector4D::MapType(off.data()+3).normalize();
setOffset(internal::fromVectorQT(off));
double fx,fy,cx,cy;
is >> fx >> fy >> cx >> cy;
setKcam(fx,fy,cx,cy);
return is.good();
}
/// Read file for gains
void readGains () {
// Get the gains
Vector7d* kgains [] = {&K_stand, &K_bal};
ifstream file ("/home/cerdogan/Documents/Software/project/krang/iser/data/gains-09.txt");
assert(file.is_open());
char line [1024];
for(size_t k_idx = 0; k_idx < 2; k_idx++) {
*kgains[k_idx] = Vector7d::Zero();
file.getline(line, 1024);
std::stringstream stream(line, std::stringstream::in);
size_t i = 0;
double newDouble;
while ((i < 6) && (stream >> newDouble)) (*kgains[k_idx])(i++) = newDouble;
}
cout << "K_stand: " << K_stand.transpose() << endl;
cout << "K_bal: " << K_bal.transpose() << endl;
file.close();
}
/// Can avoid collisions and joint limits
bool singleArmIK (const VectorXd& base_conf, const Matrix4d& Twee, bool rightArm, Vector7d& theta,
bool minimizeColl) {
static const bool debug = 0;
// Get the relative goal
Transform <double, 3, Affine> relGoal;
Matrix4d Twb;
bracketTransform(base_conf, Twb);
// cout << "Twb: \n" << Twb<< "\n";
getWristInShoulder(Twb, Twee, rightArm, relGoal.matrix());
// Compute the inverse kinematics if you don't want to minimize collisions
if(!minimizeColl) {
// double phi = 0.0;
double phi = theta(0);
theta = VectorXd (7);
bool result = ik(relGoal, phi, theta);
return result;
}
// Otherwise check multiple phi values
Vector7d bestTheta;
double maxMinDistSq = -16.0;
bool result = false;
for(double phi = 0.0; phi <= M_PI; phi += M_PI/16.0) {
// Check if there is an IK
Vector7d someTheta = VectorXd(7);
bool someResult = ik(relGoal, phi, someTheta);
if(debug) cout << "phi: " << phi << ", result: " << someResult << ", theta: " << someTheta.transpose() << endl;
// If there is a successful result, see if it is the best one so far
if(someResult) {
// Set the config to the left arm and get the coms of bracket, 7 links and end-effector
krang->setConfig(left_dofs, someTheta);
const char* names [7] = {"Bracket", "L2", "L3", "L4", "L5", "L6", "lFingerA"};
Eigen::Vector3d locs [7];
for(size_t i = 0; i < 7; i++) {
locs[i] = krang->getNode(names[i])->getWorldCOM();
if(debug) cout << "\t\t locs[" << i << "] (" << names[i] << "): " << locs[i].transpose() <<
", localCOM: " << krang->getNode(names[i])->getLocalCOM().transpose() << endl;
}
// Find the distance to both joint limits for each dof and determine the one that is
// closest to the limit
double minDistSq = 16.0;
for(size_t i = 0; i < 7; i++) {
for(size_t j = i+1; j < 7; j++) {
double distSq = (locs[i] - locs[j]).squaredNorm();
if(distSq < minDistSq) minDistSq = distSq;
}
}
if(debug) cout << "\tminDistSq: " << minDistSq << ", maxMinDistSq : " << maxMinDistSq << endl;
// Check if it is more than the current maximum minAngle
if(minDistSq > maxMinDistSq) {
maxMinDistSq = minDistSq;
bestTheta = someTheta;
}
result = true;
}
}
// Set the result of sampling the space
theta = bestTheta;
return result;
}
bool G2oSlamInterface::addEdge(const std::string& tag, int id, int dimension, int v1Id, int v2Id, const std::vector<double>& measurement, const std::vector<double>& information)
{
(void) tag;
(void) id;
size_t oldEdgesSize = _optimizer->edges().size();
if (dimension == 3) {
SE2 transf(measurement[0], measurement[1], measurement[2]);
Eigen::Matrix3d infMat;
int idx = 0;
for (int r = 0; r < 3; ++r)
for (int c = r; c < 3; ++c, ++idx) {
assert(idx < (int)information.size());
infMat(r,c) = infMat(c,r) = information[idx];
}
//cerr << PVAR(infMat) << endl;
int doInit = 0;
SparseOptimizer::Vertex* v1 = _optimizer->vertex(v1Id);
SparseOptimizer::Vertex* v2 = _optimizer->vertex(v2Id);
if (! v1) {
OptimizableGraph::Vertex* v = v1 = addVertex(dimension, v1Id);
_verticesAdded.insert(v);
doInit = 1;
++_nodesAdded;
}
if (! v2) {
OptimizableGraph::Vertex* v = v2 = addVertex(dimension, v2Id);
_verticesAdded.insert(v);
doInit = 2;
++_nodesAdded;
}
if (_optimizer->edges().size() == 0) {
cerr << "FIRST EDGE ";
if (v1->id() < v2->id()) {
cerr << "fixing " << v1->id() << endl;
v1->setFixed(true);
}
else {
cerr << "fixing " << v2->id() << endl;
v2->setFixed(true);
}
}
OnlineEdgeSE2* e = new OnlineEdgeSE2;
e->vertices()[0] = v1;
e->vertices()[1] = v2;
e->setMeasurement(transf);
e->setInformation(infMat);
_optimizer->addEdge(e);
_edgesAdded.insert(e);
if (doInit) {
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
switch (doInit){
case 1: // initialize v1 from v2
{
HyperGraph::VertexSet toSet;
toSet.insert(to);
if (e->initialEstimatePossible(toSet, from) > 0.) {
e->initialEstimate(toSet, from);
}
break;
}
case 2:
{
HyperGraph::VertexSet fromSet;
fromSet.insert(from);
if (e->initialEstimatePossible(fromSet, to) > 0.) {
e->initialEstimate(fromSet, to);
}
break;
}
default: cerr << "doInit wrong value\n";
}
}
}
else if (dimension == 6) {
Eigen::Isometry3d transf;
Matrix<double, 6, 6> infMat;
if (measurement.size() == 7) { // measurement is a Quaternion
Vector7d meas;
for (int i=0; i<7; ++i)
meas(i) = measurement[i];
// normalize the quaternion to recover numerical precision lost by storing as human readable text
Vector4d::MapType(meas.data()+3).normalize();
transf = internal::fromVectorQT(meas);
for (int i = 0, idx = 0; i < infMat.rows(); ++i)
for (int j = i; j < infMat.cols(); ++j){
infMat(i,j) = information[idx++];
if (i != j)
infMat(j,i)=infMat(i,j);
}
} else { // measurement consists of Euler angles
Vector6d aux;
aux << measurement[0], measurement[1], measurement[2],measurement[3], measurement[4], measurement[5];
transf = internal::fromVectorET(aux);
Matrix<double, 6, 6> infMatEuler;
int idx = 0;
for (int r = 0; r < 6; ++r)
for (int c = r; c < 6; ++c, ++idx) {
assert(idx < (int)information.size());
infMatEuler(r,c) = infMatEuler(c,r) = information[idx];
}
// convert information matrix to our internal representation
Matrix<double, 6, 6> J;
SE3Quat transfAsSe3(transf.matrix().topLeftCorner<3,3>(), transf.translation());
jac_quat3_euler3(J, transfAsSe3);
infMat.noalias() = J.transpose() * infMatEuler * J;
//cerr << PVAR(transf.matrix()) << endl;
//cerr << PVAR(infMat) << endl;
}
int doInit = 0;
SparseOptimizer::Vertex* v1 = _optimizer->vertex(v1Id);
SparseOptimizer::Vertex* v2 = _optimizer->vertex(v2Id);
if (! v1) {
OptimizableGraph::Vertex* v = v1 = addVertex(dimension, v1Id);
_verticesAdded.insert(v);
doInit = 1;
++_nodesAdded;
}
if (! v2) {
OptimizableGraph::Vertex* v = v2 = addVertex(dimension, v2Id);
_verticesAdded.insert(v);
doInit = 2;
++_nodesAdded;
}
if (_optimizer->edges().size() == 0) {
cerr << "FIRST EDGE ";
if (v1->id() < v2->id()) {
cerr << "fixing " << v1->id() << endl;
v1->setFixed(true);
}
else {
cerr << "fixing " << v2->id() << endl;
v2->setFixed(true);
}
}
OnlineEdgeSE3* e = new OnlineEdgeSE3;
e->vertices()[0] = v1;
e->vertices()[1] = v2;
e->setMeasurement(transf);
e->setInformation(infMat);
_optimizer->addEdge(e);
_edgesAdded.insert(e);
if (doInit) {
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
switch (doInit){
case 1: // initialize v1 from v2
{
HyperGraph::VertexSet toSet;
toSet.insert(to);
if (e->initialEstimatePossible(toSet, from) > 0.) {
e->initialEstimate(toSet, from);
}
break;
}
case 2:
{
HyperGraph::VertexSet fromSet;
fromSet.insert(from);
if (e->initialEstimatePossible(fromSet, to) > 0.) {
e->initialEstimate(fromSet, to);
}
break;
}
default: cerr << "doInit wrong value\n";
}
}
}
else {
cerr << __PRETTY_FUNCTION__ << " not implemented for this dimension" << endl;
return false;
}
if (oldEdgesSize == 0) {
_optimizer->jacobianWorkspace().allocate();
}
return true;
}
/// The main loop
void run() {
// Get the initial state and set the backup values
krang->updateSensors(0.0);
Eigen::Vector2d curr (krang->amc->pos[0], krang->amc->pos[1]);
state0 << 0.0, 0.0, (curr(0) + curr(1)) / 2.0 + krang->imu, 0.0,
(curr(1) - curr(0)) / 2.0, 0.0;
Eigen::Vector2d desiredBackVals = curr - Eigen::Vector2d(0.6, 0.6);
// start some timers
Vector6d state;
size_t c_ = 0;
int balancedCounter = 0;
struct timespec t_now, t_prev = aa_tm_now();
vector <Eigen::VectorXd> data;
Eigen::VectorXd visionData;
step = STEP_VISION;
while(!somatic_sig_received) {
// Prepare for this loop
pthread_mutex_lock(&mutex);
dbg = (c_++ % 20 == 0);
if(dbg) cout << "\nstep: " << stepName(step) << endl;
ctrlMode = stepCtrlModes[step];
if(dbg) cout << "\nctrlMode: " << ctrlModeName(ctrlMode) << endl;
// Update times
t_now = aa_tm_now();
double dt = (double)aa_tm_timespec2sec(aa_tm_sub(t_now, t_prev));
t_prev = t_now;
// Get the locomotion state
getState(state, dt);
if(dbg) DISPLAY_VECTOR(state);
// Compute the torques based on the state and the mode
double ul, ur;
computeTorques(state, ul, ur);
// Apply the torque
double input [2] = {ul, ur};
if(dbg) cout << "start: " << start << "\nu: {" << ul << ", " << ur << "}" << endl;
if(!start) input[0] = input[1] = 0.0;
// if((step != STEP_BACKUP) && (step != STEP_REST))
// somatic_motor_cmd(&daemon_cx, krang->amc, SOMATIC__MOTOR_PARAM__MOTOR_CURRENT, input, 2,NULL);
// Decide on what to do specifically for this step
switch(step) {
/*
case STEP_BACKUP: {
// Check if reached desired position
Eigen::Vector2d curr (krang->amc->pos[0], krang->amc->pos[1]);
if(dbg) DISPLAY_VECTOR(curr);
if(dbg) DISPLAY_VECTOR(desiredBackVals);
if((curr(0) < desiredBackVals(0)) && (curr(1) < desiredBackVals(1))) {
double u [] = {0.0, 0.0};
somatic_motor_cmd(&daemon_cx, krang->amc, SOMATIC__MOTOR_PARAM__MOTOR_CURRENT, u, 2,NULL);
state0(2) = (curr(0) + curr(1)) / 2.0 + krang->imu;
state0(4) = (curr(1) - curr(0)) / 2.0;
if(changePermission) {
step = STEP_FIXARMS;
changePermission = false;
}
else if(dbg) cout << "\033[1;31mMoving the left arm to front pose. Ready?" << "?\033[0m\n" <<endl;
}
else {
// Go backward slowly
double u [] = {-10.5, -10.5};
if(start) somatic_motor_cmd(&daemon_cx, krang->amc, SOMATIC__MOTOR_PARAM__MOTOR_CURRENT, u, 2,NULL);
}
} break;
case STEP_FIXARMS: {
Vector7d goal;
// goal << 1.400, -1.000, 0.000, -0.800, 0.000, -0.800, 0.000;
// goal << 1.083, -1.000, 0.000, -1.382, 0.007, 0.852, -0.575;
goal << 1.083, -0.451, 0.074, -1.999, 0.007, 0.852, -0.575;
somatic_motor_setpos(&daemon_cx, krang->arms[Krang::LEFT], goal.data(), 7);
bool reached = true;
for(size_t i = 0; i < 7; i++) {
if(fabs(goal(i) - krang->arms[LEFT]->pos[i]) > 0.005) {
reached = false;
break;
}
}
if(reached) {
if(changePermission) {
step = STEP_STANDUP;
changePermission = false;
}
else if(dbg) cout << "\033[1;31mReady to standup? " << "?\033[0m\n" << endl;
}
} break;
case STEP_STANDUP: {
if(fabs(state(0)) < 0.064) balancedCounter++;
if((balancedCounter > 300)) {
if(changePermission) {
balancedCounter = 0;
step = STEP_WAIST;
changePermission = false;
}
else if(dbg) cout << "\033[1;31mReady to move the waist? " << "?\033[0m\n" << endl;
}
} break;
case STEP_WAIST: {
// Send a current value
double current = -1.8;
somatic_vector_set_data(waistDaemonCmd->data, ¤t, 1);
int r = SOMATIC_PACK_SEND(krang->waistCmdChan, somatic__waist_cmd, waistDaemonCmd);
if(ACH_OK != r) fprintf(stderr, "Couldn't send message: %s\n",
ach_result_to_string(static_cast<ach_status_t>(r)));
// Check for stop condition
if(krang->waist->pos[0] < 2.26) {
somatic_waist_cmd_set(waistDaemonCmd, SOMATIC__WAIST_MODE__STOP);
int r = SOMATIC_PACK_SEND(krang->waistCmdChan, somatic__waist_cmd, waistDaemonCmd);
if(ACH_OK != r) fprintf(stderr, "Couldn't send message: %s\n",
ach_result_to_string(static_cast<ach_status_t>(r)));
step = STEP_STABILIZE;
}
} break;
case STEP_STABILIZE: {
if(fabs(state(0)) < 0.064) balancedCounter++;
if(balancedCounter > 200) {
if(changePermission) {
balancedCounter = 0;
step = STEP_VISION;
changePermission = false;
}
else if(dbg) cout << "\033[1;31mReady to start vision?" << "?\033[0m\n" << endl;
}
} break;
*/
case STEP_VISION: {
// Collect the data - make sure the cinder block is detected
Eigen::VectorXd dataPoint (6);
if(getVecData(chan, dataPoint)) {
if(dataPoint(0) < 9.99)
data.push_back(dataPoint);
}
if(dbg) DISPLAY_SCALAR(data.size());
if(data.size() > 200) {
visionData = analyzeKinectData(data, false);
if(changePermission) {
data.clear();
step = STEP_REACHOUT;
changePermission = false;
}
else if(dbg) {
DISPLAY_VECTOR(visionData);
cout << "\033[1;31mReady to reach out? " << "?\033[0m\n" << endl;
}
}
} break;
case STEP_REACHOUT: {
static bool reached = false;
// Set the workspace direction
Eigen::Vector3d handPos =
robot->getNode("lGripper")->getWorldTransform().topRightCorner<3,1>();
if(dbg) DISPLAY_VECTOR(handPos);
Vector6d xdot = Vector6d::Zero();
Eigen::Vector3d posErr = visionData.block<3,1>(0,0) - handPos;
if(dbg) DISPLAY_VECTOR(visionData);
if(dbg) DISPLAY_VECTOR(krang->fts[Krang::LEFT]->lastExternal);
double magn = krang->fts[Krang::LEFT]->lastExternal.topLeftCorner<3,1>().norm();
if(reached || posErr.norm() < 0.05 || (magn > 30)) {
reached = true;
cout << "Reached here" << endl;
Vector7d zero = Vector7d::Zero();
somatic_motor_setvel(&daemon_cx, krang->arms[Krang::LEFT], zero.data(), 7);
if(changePermission) {
step = STEP_GRASP;
changePermission = false;
pthread_mutex_unlock(&mutex);
continue;
}
else if(dbg) cout << "\033[1;31mReady to grasp? " << "?\033[0m\n" << endl;
pthread_mutex_unlock(&mutex);
continue;
}
else xdot.block<3,1>(0,0) = posErr.normalized();
// Nullspace: construct a qdot that the jacobian will bias toward using the nullspace
Eigen::VectorXd q = robot->getConfig(*workspace->arm_ids);
Krang::Vector7d nullspace_qdot_ref = (nullspace_q_ref - q).cwiseProduct(nullspace_q_mask);
// Jacobian: compute the desired jointspace velocity from the inputs and sensors
Eigen::VectorXd qdot_jacobian;
workspace->refJSVelocity(xdot, nullspace_qdot_ref, qdot_jacobian);
if(dbg) DISPLAY_VECTOR(qdot_jacobian);
// Avoid joint limits
Eigen::VectorXd qdot_avoid(7);
Krang::computeQdotAvoidLimits(robot, *workspace->arm_ids, q, qdot_avoid);
// Add qdots together to get the overall movement
Eigen::VectorXd qdot_apply = qdot_avoid + qdot_jacobian;
qdot_apply = qdot_apply.normalized() * 0.10;
if(dbg) DISPLAY_VECTOR(qdot_apply);
// And apply that to the arm
if(!start) qdot_apply = Vector7d::Zero();
somatic_motor_setvel(&daemon_cx, krang->arms[Krang::LEFT], qdot_apply.data(), 7);
} break;
case STEP_GRASP: {
cout <<"hello world" << endl;
system("echo 0.0 | sudo somatic_motor_cmd lgripper pos");
step = STEP_PUSH;
cout <<"hello world" << endl;
} break;
case STEP_PUSH: {
Vector7d goal;
goal = eig7(krang->arms[Krang::LEFT]->pos);
goal(0) -= 2.5;
somatic_motor_setpos(&daemon_cx, krang->arms[Krang::LEFT], goal.data(), 7);
/*
// Send a current value
double current = 0.0;
somatic_vector_set_data(waistDaemonCmd->data, ¤t, 1);
int r = SOMATIC_PACK_SEND(krang->waistCmdChan, somatic__waist_cmd, waistDaemonCmd);
if(ACH_OK != r) fprintf(stderr, "Couldn't send message: %s\n",
ach_result_to_string(static_cast<ach_status_t>(r)));
// Check for stop condition
if(krang->waist->pos[0] > 2.85) {
somatic_waist_cmd_set(waistDaemonCmd, SOMATIC__WAIST_MODE__STOP);
int r = SOMATIC_PACK_SEND(krang->waistCmdChan, somatic__waist_cmd, waistDaemonCmd);
if(ACH_OK != r) fprintf(stderr, "Couldn't send message: %s\n",
ach_result_to_string(static_cast<ach_status_t>(r)));
step = STEP_REST;
}
*/
} break;
case STEP_REST: {
double u [2] = {15, 15};
if(start)
somatic_motor_cmd(&daemon_cx, krang->amc, SOMATIC__MOTOR_PARAM__MOTOR_CURRENT, u, 2,NULL);
if(krang->imu < -1.82) {
u[0] = u[1] = 0.0;
somatic_motor_cmd(&daemon_cx, krang->amc, SOMATIC__MOTOR_PARAM__MOTOR_CURRENT, u, 2,NULL);
return;
}
} break;
}
// Update the locomotion mode
pthread_mutex_unlock(&mutex);
}
}
