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; }
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(); }
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); } }