PitchRollState()
{
Omega.setZero();
GyroBias.setZero();
AccelBias.setZero();
Covariance.resize(ndim(), ndim());
Covariance.setIdentity();
};
void
cylinder_kinematics(double /*data_time*/, Eigen::Vector3d& U_com, Eigen::Vector3d& W_com, void* /*ctx*/)
{
U_com.setZero();
W_com.setZero();
return;
} // cylinder_kinematics
void mean(const std::vector<double>& weights,
const std::vector<struct IMUOrientationMeasurement>& Chimeas)
{
acceleration.setZero();
omega.setZero();
for(const auto &&t: zip_range(weights, Chimeas))
{
double w = t.get<0>();
struct IMUOrientationMeasurement m = t.get<1>();
acceleration += w*m.acceleration;
omega += w*m.omega;
}
};
// Fit a plane to 3D data
void PlaneFit::fitPlane(const Points3D& P, Eigen::Vector3d& normal, Eigen::Vector3d& mean)
{
// Retrieve how many data points there are
size_t N = P.size();
// Handle trivial cases
if(N <= 0)
{
normal << 0.0, 0.0, 1.0;
mean.setZero();
return;
}
else if(N == 1)
{
normal << 0.0, 0.0, 1.0;
mean = P[0];
return;
}
// Calculate the mean of the data points (the plane of best fit always passes through the mean)
mean = Eigen::Vector3d::Zero();
if(N >= 1)
mean = std::accumulate(P.begin(), P.end(), mean) / N;
// Construct the matrix of zero mean data points
Eigen::MatrixXd A(3, N);
for(size_t i = 0; i < N; i++)
A.col(i) = P[i] - mean;
// Perform an SVD decomposition to determine the direction with the minimum variance
unsigned int options = (N >= 3 ? Eigen::ComputeThinU : Eigen::ComputeFullU) | Eigen::ComputeThinV;
normal = A.jacobiSvd(options).matrixU().col(2).normalized();
}
inline void fit(const std::vector<LaserBeam> &input, Eigen::Vector3d &output, double &probability) {
PointCloud<PointXY>::Ptr cloud( new PointCloud<PointXY> );
for(std::vector<LaserBeam>::const_iterator it = input.begin() ; it != input.end() ; ++it) {
PointXY p;
p.x = it->posX();
p.y = it->posY();
cloud->push_back(p);
}
SampleConsensusModelCircle2D<PointXY>::Ptr ransac_model(new SampleConsensusModelCircle2D<PointXY>(cloud));
SampleConsensus<PointXY>::Ptr ransac(new RandomSampleConsensus<PointXY>(ransac_model));
ransac->setDistanceThreshold(0.05);
Eigen::VectorXf coefficients;
ransac->computeModel();
ransac->getModelCoefficients(coefficients);
if(coefficients.size() > 0) {
output.x() = coefficients[0];
output.y() = coefficients[1];
output.z() = coefficients[2];
probability = ransac->getProbability();
} else {
output.setZero();
probability = 0.0;
}
}
/** \brief Constructor
*/
FilterState():fsm_(nullptr), transformFeatureOutputCT_(nullptr), featureOutputCov_((int)(FeatureOutput::D_),(int)(FeatureOutput::D_)){
usePredictionMerge_ = true;
imgTime_ = 0.0;
imageCounter_ = 0;
groundtruth_qCJ_.setIdentity();
groundtruth_JrJC_.setZero();
groundtruth_qJI_.setIdentity();
groundtruth_IrIJ_.setZero();
groundtruth_qCB_.setIdentity();
groundtruth_BrBC_.setZero();
plotGroundtruth_ = true;
state_.initFeatureManagers(fsm_);
fsm_.allocateMissing();
drawPB_ = 1;
drawPS_ = mtState::patchSize_*pow(2,mtState::nLevels_-1)+2*drawPB_;
}
static void testJacobian(const ExpressionTester<RotationExpression> & expressionTester) {
auto rotExp = expressionTester.getExp();
for (int i = 0; i < 3; i++) {
Eigen::Vector3d p;
p.setZero();
p(i) = 1;
RotationExpressionNodeFunctor functor(rotExp, p);
sm::eigen::NumericalDiff<RotationExpressionNodeFunctor> numdiff(functor, expressionTester.getEps());
/// Discern the size of the jacobian container
Eigen::Matrix3d C = rotExp.toRotationMatrix();
JacobianContainer Jc(3);
rotExp.evaluateJacobians(Jc);
Eigen::Matrix3d Cp_cross = sm::kinematics::crossMx(C * p);
Jc.applyChainRule(Cp_cross);
Eigen::VectorXd dp(Jc.cols());
dp.setZero();
Eigen::MatrixXd Jest = numdiff.estimateJacobian(dp);
auto JcM = Jc.asSparseMatrix();
sm::eigen::assertNear(Jc.asSparseMatrix(), Jest, expressionTester.getTolerance(), SM_SOURCE_FILE_POS, static_cast<std::stringstream&>(std::stringstream("Testing the RotationExpression's Jacobian (column=") << i <<")").str());
if (expressionTester.getPrintResult()) {
std::cout << "Jest=\n" << Jest << std::endl;
std::cout << "Jc=\n" << JcM << std::endl;
}
}
}
void Mesh::computeFaceGradients(Eigen::MatrixXd& gradients, const Eigen::VectorXd& u) const
{
for (FaceCIter f = faces.begin(); f != faces.end(); f++) {
Eigen::Vector3d gradient;
gradient.setZero();
Eigen::Vector3d normal = f->normal();
normal.normalize();
HalfEdgeCIter he = f->he;
do {
double ui = u(he->next->next->vertex->index);
Eigen::Vector3d ei = he->next->vertex->position - he->vertex->position;
gradient += ui * normal.cross(ei);
he = he->next;
} while (he != f->he);
gradient /= (2.0 * f->area());
gradient.normalize();
gradients.row(f->index) = -gradient;
}
}
void mesh_core::Plane::leastSquaresGeneral(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d* average)
{
if (points.empty())
{
normal_ = Eigen::Vector3d(0,0,1);
d_ = 0;
if (average)
*average = Eigen::Vector3d::Zero();
return;
}
// find c, the average of the points
Eigen::Vector3d c;
c.setZero();
EigenSTL::vector_Vector3d::const_iterator p = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; p != end ; ++p)
c += *p;
c *= 1.0/double(points.size());
// Find the matrix
Eigen::Matrix3d m;
m.setZero();
p = points.begin();
for ( ; p != end ; ++p)
{
Eigen::Vector3d cp = *p - c;
m(0,0) += cp.x() * cp.x();
m(1,0) += cp.x() * cp.y();
m(2,0) += cp.x() * cp.z();
m(1,1) += cp.y() * cp.y();
m(2,1) += cp.y() * cp.z();
m(2,2) += cp.z() * cp.z();
}
m(0,1) = m(1,0);
m(0,2) = m(2,0);
m(1,2) = m(2,1);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(m);
if (eigensolver.info() == Eigen::Success)
{
normal_ = eigensolver.eigenvectors().col(0);
normal_.normalize();
}
else
{
normal_ = Eigen::Vector3d(0,0,1);
}
d_ = -c.dot(normal_);
if (average)
*average = c;
}
/**
* @brief Handle RAW_IMU MAVlink message.
* Message specification: https://mavlink.io/en/messages/common.html/#RAW_IMU
* @param msg Received Mavlink msg
* @param imu_raw RAW_IMU msg
*/
void handle_raw_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RAW_IMU &imu_raw)
{
ROS_INFO_COND_NAMED(!has_raw_imu, "imu", "IMU: Raw IMU message used.");
has_raw_imu = true;
if (has_hr_imu || has_scaled_imu)
return;
auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
auto header = m_uas->synchronized_header(frame_id, imu_raw.time_usec);
/** @note APM send SCALED_IMU data as RAW_IMU
*/
auto gyro_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC);
auto accel_frd = Eigen::Vector3d(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc);
auto accel_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(accel_frd);
if (m_uas->is_ardupilotmega()) {
accel_frd *= MILLIG_TO_MS2;
accel_flu *= MILLIG_TO_MS2;
} else if (m_uas->is_px4()) {
accel_frd *= MILLIMS2_TO_MS2;
accel_flu *= MILLIMS2_TO_MS2;
}
publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd);
if (!m_uas->is_ardupilotmega()) {
ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: linear acceleration on RAW_IMU known on APM only.");
ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: ~imu/data_raw stores unscaled raw acceleration report.");
linear_accel_vec_flu.setZero();
linear_accel_vec_frd.setZero();
}
/** Magnetic field data:
* @snippet src/plugins/imu.cpp mag_field
*/
// [mag_field]
auto mag_field = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
Eigen::Vector3d(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag) * MILLIT_TO_TESLA);
// [mag_field]
publish_mag(header, mag_field);
}
Plane::Plane(Eigen::Vector3d pos, Eigen::Vector3d rotation, Eigen::Vector2d size, double alpha, double intervall, bool minimal){
Eigen::Quaterniond q(1,0,0,0);
Eigen::AngleAxisd rot_x(rotation[0],Eigen::Vector3d::UnitX());
Eigen::AngleAxisd rot_y(rotation[1],Eigen::Vector3d::UnitY());
Eigen::AngleAxisd rot_z(rotation[2],Eigen::Vector3d::UnitZ());
q = q* rot_x * rot_y * rot_z;
inertiaAxisX = q.toRotationMatrix().col(0);
inertiaAxisY = q.toRotationMatrix().col(1);
normalVector = q.toRotationMatrix().col(2);
Eigen::Vector3d S;
Eigen::Matrix<double,9,1> C;
Eigen::Matrix<double,9,1> productMatrix;
S.setZero();
C.setZero();
productMatrix.setZero();
bias = 0;
if(!minimal){
for(double x = -size[0]/2.0; x < +size[0]/2.0; x+=intervall){
for(double y = -size[1]/2.0; y <+size[1]/2.0; y+=intervall){
Eigen::Vector3d p(x,y,0);
p = (q*p)+pos;
points.insert(Shared_Point(new Point(p[0],p[1],p[2])));
}
}
}else{
Eigen::Vector3d p0(-size[0]/2.0,-size[1]/2.0,0);
Eigen::Vector3d p1(-size[0]/2.0,size[1]/2.0,0);
Eigen::Vector3d p2(size[0]/2.0,-size[1]/2.0,0);
Eigen::Vector3d p3(size[0]/2.0,size[1]/2.0,0);
p0 = (q*p0)+pos;
p1 = (q*p1)+pos;
p2 = (q*p2)+pos;
p3 = (q*p3)+pos;
points.insert(Shared_Point(new Point(p0[0],p0[1],p0[2])));
points.insert(Shared_Point(new Point(p1[0],p1[1],p1[2])));
points.insert(Shared_Point(new Point(p2[0],p2[1],p2[2])));
points.insert(Shared_Point(new Point(p3[0],p3[1],p3[2])));
}
for(int i=0;i<3;i++){
centerOfMass[i] = pos[i];
this->S[i]=S[i];
}
for(int i=0;i<9;i++){
this->C[i]=C(i,0);
this->productMatrix[i]=productMatrix(i,0);
}
rectangleCalculated = false;
alpha_=alpha;
}
void BoundingBox::computeOrientedBox(std::vector<Vertex>& vertices)
{
type = "Oriented";
orientedPoints.clear();
// compute mean
Eigen::Vector3d center;
center.setZero();
for (VertexCIter v = vertices.begin(); v != vertices.end(); v++) {
center += v->position;
}
center /= (double)vertices.size();
// adjust for mean and compute covariance
Eigen::Matrix3d covariance;
covariance.setZero();
for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
Eigen::Vector3d pAdg = v->position - center;
covariance += pAdg * pAdg.transpose();
}
covariance /= (double)vertices.size();
// compute eigenvectors for the covariance matrix
Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();
// project min and max points on each principal axis
double min1 = INFINITY, max1 = -INFINITY;
double min2 = INFINITY, max2 = -INFINITY;
double min3 = INFINITY, max3 = -INFINITY;
double d = 0.0;
eigenVectors.transpose();
for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
d = eigenVectors.row(0).dot(v->position);
if (min1 > d) min1 = d;
if (max1 < d) max1 = d;
d = eigenVectors.row(1).dot(v->position);
if (min2 > d) min2 = d;
if (max2 < d) max2 = d;
d = eigenVectors.row(2).dot(v->position);
if (min3 > d) min3 = d;
if (max3 < d) max3 = d;
}
// add points to vector
orientedPoints.push_back(eigenVectors.row(0) * min1);
orientedPoints.push_back(eigenVectors.row(0) * max1);
orientedPoints.push_back(eigenVectors.row(1) * min2);
orientedPoints.push_back(eigenVectors.row(1) * max2);
orientedPoints.push_back(eigenVectors.row(2) * min3);
orientedPoints.push_back(eigenVectors.row(2) * max3);
}
void mesh_core::Plane::leastSquaresFast(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d* average)
{
Eigen::Matrix3d m;
Eigen::Vector3d b;
Eigen::Vector3d c;
m.setZero();
b.setZero();
c.setZero();
EigenSTL::vector_Vector3d::const_iterator p = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; p != end ; ++p)
{
m(0,0) += p->x() * p->x();
m(1,0) += p->x() * p->y();
m(2,0) += p->x();
m(1,1) += p->y() * p->y();
m(2,1) += p->y();
b(0) += p->x() * p->z();
b(1) += p->y() * p->z();
b(2) += p->z();
c += *p;
}
m(0,1) = m(1,0);
m(0,2) = m(2,0);
m(1,2) = m(2,1);
m(2,2) = double(points.size());
c *= 1.0/double(points.size());
normal_ = m.colPivHouseholderQr().solve(b);
if (normal_.squaredNorm() > std::numeric_limits<double>::epsilon())
normal_.normalize();
d_ = -c.dot(normal_);
if (average)
*average = c;
}
void BoundingBox::computeOrientedBox(std::vector<Eigen::Vector3d>& positions)
{
// compute mean
Eigen::Vector3d cm;
cm.setZero();
for (size_t i = 0; i < positions.size(); i++) {
cm += positions[i];
}
cm /= (double)positions.size();
// adjust for mean and compute covariance matrix
Eigen::Matrix3d covariance;
covariance.setZero();
for (size_t i = 0; i < positions.size(); i++) {
Eigen::Vector3d pAdg = positions[i] - cm;
covariance += pAdg * pAdg.transpose();
}
covariance /= (double)positions.size();
// compute eigenvectors for covariance matrix
Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();
// set axes
eigenVectors.transpose();
xAxis = eigenVectors.row(0);
yAxis = eigenVectors.row(1);
zAxis = eigenVectors.row(2);
// project min and max points on each principal axis
double min1 = INF, max1 = -INF;
double min2 = INF, max2 = -INF;
double min3 = INF, max3 = -INF;
double d = 0.0;
for (size_t i = 0; i < positions.size(); i++) {
d = xAxis.dot(positions[i]);
if (min1 > d) min1 = d;
if (max1 < d) max1 = d;
d = yAxis.dot(positions[i]);
if (min2 > d) min2 = d;
if (max2 < d) max2 = d;
d = zAxis.dot(positions[i]);
if (min3 > d) min3 = d;
if (max3 < d) max3 = d;
}
// set center and halflengths
center = (xAxis*(min1 + max1) + yAxis*(min2 + max2) + zAxis*(min3 + max3)) /2;
halfLx = (max1 - min1)/2; halfLy = (max2 - min2)/2; halfLz = (max3 - min3)/2;
}
int collideSphereSphere(CollisionObject* o1, CollisionObject* o2, const double& _r0, const Eigen::Isometry3d& c0,
const double& _r1, const Eigen::Isometry3d& c1,
CollisionResult& result)
{
double r0 = _r0;
double r1 = _r1;
double rsum = r0 + r1;
Eigen::Vector3d normal = c0.translation() - c1.translation();
double normal_sqr = normal.squaredNorm();
if ( normal_sqr > rsum * rsum )
{
return 0;
}
r0 /= rsum;
r1 /= rsum;
Eigen::Vector3d point = r1 * c0.translation() + r0 * c1.translation();
double penetration;
if (normal_sqr < DART_COLLISION_EPS)
{
normal.setZero();
penetration = rsum;
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = point;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
return 1;
}
normal_sqr = sqrt(normal_sqr);
normal *= (1.0/normal_sqr);
penetration = rsum - normal_sqr;
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = point;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
return 1;
}
void mean(const std::vector<double> &weights,
const std::vector<struct PitchRollState> &Chistate)
{
Omega.setZero();
GyroBias.setZero();
AccelBias.setZero();
RotationAverage ra;
for(auto && t: zip_range(weights, Chistate))
{
double w=t.get<0>();
const struct PitchRollState& pt = t.get<1>();
ra(w, pt.Orientation);
Omega += w*pt.Omega;
GyroBias += w*pt.GyroBias;
AccelBias += w*pt.AccelBias;
}
double error = ra.geodesic(&Orientation);
if(error<0)
{
Orientation = Chistate.front().Orientation;
}
Covariance = Chistate[0].Covariance;
};
struct PitchRollState
boxplus(const Eigen::VectorXd& offset) const
{
struct PitchRollState out;
Eigen::Vector3d omega;
omega.setZero();
omega = offset.segment<3>(0);
out.Omega = Omega + offset.segment<3>(3);
out.GyroBias = GyroBias + offset.segment<3>(6);
Sophus::SO3d rot=Sophus::SO3d::exp(omega);
out.Orientation = rot*Orientation;
out.AccelBias = AccelBias + offset.segment<3>(9);
out.Covariance = Covariance;
return out;
};
Task::Task(std::string const& name, RTT::ExecutionEngine* engine)
: TaskBase(name, engine)
{
map = 0;
grid_map = 0;
base::Matrix6d m;
m.setZero();
_param_sqDamp.set(m);
_param_sqDampNeg.set(m);
m(0,0) = 8.203187564;
m(1,1) = 24.94216;
_param_linDamp.set(m);
_param_linDampNeg.set(m);
Eigen::Vector3d v;
v.setZero();
_param_centerOfBuoyancy.set(v);
_param_centerOfGravity.set(v);
Eigen::Vector3d v_sonar, v_gps, v_buoy_cam, v_buoy_rotation, v_pipeline, v_dvl_rotation;
v_sonar << -0.5, 0.0, 0.0;
_sonar_position.set(v_sonar);
v_gps.setZero();
_gps_position.set(v_gps);
v_buoy_cam << 0.7, 0.0, 0.0;
_buoy_cam_position.set(v_buoy_cam);
v_buoy_rotation.setZero();
_buoy_cam_rotation.set(v_buoy_rotation);
v_pipeline << -0.7, 0.0, -2.0;
_pipeline_position.set(v_pipeline);
v_dvl_rotation << 0.0, 0.0, 0.25 * M_PI;
_dvl_rotation.set(v_dvl_rotation);
localizer = 0;
map = 0;
grid_map = 0;
}
//! The distance potential and the gradient is computed for a collision point
//! First the distance d of the collision point (sphere) to the closest
// obstacle
// is computed
//! Then 3 cases apear to compute the potential
//! - 0 if this distance is greater than the coll point clearance
//! -
bool CollisionSpace::getCollisionPointPotentialGradient(
const CollisionPoint& collision_point,
const Eigen::Vector3d& collision_point_pos,
double& field_distance,
double& potential,
Eigen::Vector3d& gradient) const {
Eigen::Vector3d field_gradient;
// Compute the distance gradient and distance to nearest obstacle
field_distance =
this->getDistanceGradient(collision_point_pos, field_gradient);
double d = field_distance - collision_point.getRadius();
// three cases below:
if (d >= collision_point.getClearance()) {
potential = 0.0;
gradient.setZero();
} else if (d >= 0.0) {
double diff = (d - collision_point.getClearance());
potential = 0.5 * collision_point.getInvClearance() * diff * diff;
gradient = diff * collision_point.getInvClearance() * field_gradient;
} else // if d < 0.0
{
gradient = field_gradient;
double offset = 0.5 * collision_point.getClearance();
potential = -d * 10 + offset;
// potential = -d + 0.5 * collision_point.getClearance();
}
// cout << "field_distance : " << field_distance
// << ", radius : " << collision_point.getRadius() << endl;
// true if point is in collision
return (field_distance <= collision_point.getRadius());
}
template<typename PointT> void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
const typename pcl::search::KdTree<PointT>::Ptr kdtree,
std::vector<Eigen::Matrix3d>& cloud_covariances)
{
if (k_correspondences_ > int (cloud->size ()))
{
PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_);
return;
}
Eigen::Vector3d mean;
std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);
// We should never get there but who knows
if(cloud_covariances.size () < cloud->size ())
cloud_covariances.resize (cloud->size ());
typename pcl::PointCloud<PointT>::const_iterator points_iterator = cloud->begin ();
std::vector<Eigen::Matrix3d>::iterator matrices_iterator = cloud_covariances.begin ();
for(;
points_iterator != cloud->end ();
++points_iterator, ++matrices_iterator)
{
const PointT &query_point = *points_iterator;
Eigen::Matrix3d &cov = *matrices_iterator;
// Zero out the cov and mean
cov.setZero ();
mean.setZero ();
// Search for the K nearest neighbours
kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
// Find the covariance matrix
for(int j = 0; j < k_correspondences_; j++) {
const PointT &pt = (*cloud)[nn_indecies[j]];
mean[0] += pt.x;
mean[1] += pt.y;
mean[2] += pt.z;
cov(0,0) += pt.x*pt.x;
cov(1,0) += pt.y*pt.x;
cov(1,1) += pt.y*pt.y;
cov(2,0) += pt.z*pt.x;
cov(2,1) += pt.z*pt.y;
cov(2,2) += pt.z*pt.z;
}
mean /= static_cast<double> (k_correspondences_);
// Get the actual covariance
for (int k = 0; k < 3; k++)
for (int l = 0; l <= k; l++)
{
cov(k,l) /= static_cast<double> (k_correspondences_);
cov(k,l) -= mean[k]*mean[l];
cov(l,k) = cov(k,l);
}
// Compute the SVD (covariance matrix is symmetric so U = V')
Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
cov.setZero ();
Eigen::Matrix3d U = svd.matrixU ();
// Reconstitute the covariance matrix with modified singular values using the column // vectors in V.
for(int k = 0; k < 3; k++) {
Eigen::Vector3d col = U.col(k);
double v = 1.; // biggest 2 singular values replaced by 1
if(k == 2) // smallest singular value replaced by gicp_epsilon
v = gicp_epsilon_;
cov+= v * col * col.transpose();
}
}
}
IMUOrientationMeasurement()
{
acceleration.setZero();
omega.setZero();
};
void insituFTSensorCalibrationThread::run()
{
yarp::os::LockGuard guard(threadMutex);
if( status == COLLECTING_DATASET )
{
double * p_timestamp = 0;
bool blocking = false;
if( this->readAccelerationFromSensor )
{
sensors->readSensor(wbi::SENSOR_ACCELEROMETER,0,raw_acc[0].data(),p_timestamp,blocking);
}
else
{
sensors->readSensors(wbi::SENSOR_ENCODER,joint_positions.data(),p_timestamp,blocking);
wbi::Frame H_world_sensor;
wbi::Rotation R_sensor_world;
model->computeH(joint_positions.data(),wbi::Frame::identity(),sensorFrameIndex,H_world_sensor);
R_sensor_world = H_world_sensor.R.getInverse();
Eigen::Vector3d g;
g.setZero();
g[2] = -9.78;
Eigen::Map< Eigen::Vector3d >(raw_acc[0].data()) =
Eigen::Map< Eigen::Matrix<double,3,3,Eigen::RowMajor> >(R_sensor_world.data)*g;
}
sensors->readSensor(wbi::SENSOR_FORCE_TORQUE,0,raw_ft[0].data(),p_timestamp,blocking);
smooth_acc[0] = acc_filters[0]->filt(raw_acc[0]);
smooth_ft[0] = ft_filters[0]->filt(raw_ft[0]);
/*
yDebug("Accelerometer read: %s \n",smooth_acc[0].toString().c_str());
yDebug("FT read: %s \n",smooth_ft[0].toString().c_str());
double mass = 4.4850;
yarp::sig::Vector deb = smooth_acc[0];
deb[0] = mass*deb[0];
deb[1] = mass*deb[1];
deb[2] = mass*deb[2];
yDebug("Predicted FT read: %s \n",(deb).toString().c_str());*/
if( this->dump )
{
for(int i=0; i < 6; i++ )
{
this->datasets_dump << smooth_ft[0][i] << ",";
}
this->datasets_dump << smooth_acc[0][0] << ",";
this->datasets_dump << smooth_acc[0][1] << ",";
this->datasets_dump << smooth_acc[0][2] << std::endl;
}
estimator_datasets[currentDataset]->addMeasurements(InSituFTCalibration::wrapVec(smooth_ft[0]),InSituFTCalibration::wrapVec(smooth_acc[0]));
}
else if( status == WAITING_NEW_DATASET_START )
{
static int run_count = 0;
if( run_count % 50 == 0 )
{
printf("InSitu FT sensor calibration: waiting for new dataset start.\n");
printf("Mount the desired added mass and start new dataset collection via the rpc port.\n");
fflush(stdout);
}
run_count++;
}
}
void customPreRefresh() override
{
if(mAnyMovement)
{
Eigen::Isometry3d old_tf = mAtlas->getBodyNode(0)->getWorldTransform();
Eigen::Isometry3d new_tf = Eigen::Isometry3d::Identity();
Eigen::Vector3d forward = old_tf.linear().col(0);
forward[2] = 0.0;
if(forward.norm() > 1e-10)
forward.normalize();
else
forward.setZero();
Eigen::Vector3d left = old_tf.linear().col(1);
left[2] = 0.0;
if(left.norm() > 1e-10)
left.normalize();
else
left.setZero();
const Eigen::Vector3d& up = Eigen::Vector3d::UnitZ();
const double linearStep = 0.01;
const double elevationStep = 0.2*linearStep;
const double rotationalStep = 2.0*M_PI/180.0;
if(mMoveComponents[MOVE_W])
new_tf.translate( linearStep*forward);
if(mMoveComponents[MOVE_S])
new_tf.translate(-linearStep*forward);
if(mMoveComponents[MOVE_A])
new_tf.translate( linearStep*left);
if(mMoveComponents[MOVE_D])
new_tf.translate(-linearStep*left);
if(mMoveComponents[MOVE_F])
new_tf.translate( elevationStep*up);
if(mMoveComponents[MOVE_Z])
new_tf.translate(-elevationStep*up);
if(mMoveComponents[MOVE_Q])
new_tf.rotate(Eigen::AngleAxisd( rotationalStep, up));
if(mMoveComponents[MOVE_E])
new_tf.rotate(Eigen::AngleAxisd(-rotationalStep, up));
new_tf.pretranslate(old_tf.translation());
new_tf.rotate(old_tf.rotation());
mAtlas->getJoint(0)->setPositions(FreeJoint::convertToPositions(new_tf));
}
bool solved = mAtlas->getIK(true)->solve();
if(!solved)
++iter;
else
iter = 0;
if(iter == 1000)
{
std::cout << "Failing!" << std::endl;
}
}
Eigen::Vector3d VisuoServoTask::get_desired_rotation_range(){
Eigen::Vector3d des;
des.setZero();
des = desired_pose_range;
return des;
}
void TrfmTranslateZ::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Vector3d& v ) {
v.setZero();
}
/* This callback is called everytime this node's simulation starts or restarts.
This is different from initialize() above. */
virtual void onInitialization() override {
// Initial state and output
x.setZero();
command = 0.0;
std::cout << "At " << _current_sim_time << " INIT" << std::endl;
}
void init(){
//find the path of config files
std::string selfpath = get_selfpath();
//select using normal control mode or psudogravity control mode
right_rmt = NormalMode;
//initialize FT sensor ptr
ft_gama = new gamaFT;
ft.setZero(6);
tool_vec_g.setZero();
axis_end_vec.setZero();
//show toolname
tn = hingedtool;
StopFlag = false;
//initialize the axis vec
local_hinged_axis_vec.setZero();
local_hinged_axis_vec(0) = -0.9472;
local_hinged_axis_vec(1) = 0.0494;
local_hinged_axis_vec(2) = -0.3166;
des_tm.setZero();
des_vec.setZero();
//declare the cb function
boost::function<void(boost::shared_ptr<std::string>)> button_sdh_moveto(sdh_moveto_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhaxisvec_moveto(sdhaxisvec_moveto_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhmaintainF(sdhmaintainF_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhslideX(sdhslideX_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhslideY(sdhslideY_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhfoldtool(sdhfoldtool_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_gamaftcalib(gamaftcalib_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdh_grav_comp_ctrl(sdh_grav_comp_ctrl_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdh_normal_ctrl(sdh_normal_ctrl_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_brake(brake_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_nobrake(nobrake_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_closeprog(closeprog_cb);
//specify controller configure file in order to load it(right kuka + shadow hand)
std::string config_filename = selfpath + "/etc/right_arm_param.xml";
std::cout<<"right arm config file name is: "<<config_filename<<std::endl;
//load controller parameters
right_pm = new ParameterManager(config_filename);
//initialize ptr to right kuka com okc
com_okc_right = new ComOkc(kuka_right,OKC_HOST,OKC_PORT);
//connect kuka right
com_okc_right->connect();
//initialize the kuka robot and let it stay in the init pose
kuka_right_arm = new KukaLwr(kuka_right,*com_okc_right,tn);
//initialize the robot state
right_rs = new RobotState(kuka_right_arm);
//get the initialize state of kuka right
kuka_right_arm->get_joint_position_act();
kuka_right_arm->update_robot_state();
right_rs->updated(kuka_right_arm);
right_ac_vec.push_back(new ProActController(*right_pm));
right_task_vec.push_back(new KukaSelfCtrlTask(RP_NOCONTROL));
right_task_vec.back()->mt = JOINTS;
right_task_vec.back()->mft = GLOBAL;
Eigen::Vector3d p,o;
p.setZero();
o.setZero();
//get start point position in cartesian space
p(0) = initP_x = right_rs->robot_position["eef"](0);
p(1) = initP_y= right_rs->robot_position["eef"](1);
p(2) = initP_z= right_rs->robot_position["eef"](2);
o = tm2axisangle(right_rs->robot_orien["eef"]);
initO_x = o(0);
initO_y = o(1);
initO_z = o(2);
right_task_vec.back()->set_desired_p_eigen(p);
right_task_vec.back()->set_desired_o_ax(o);
kuka_right_arm->setAxisStiffnessDamping(right_ac_vec.back()->pm.stiff_ctrlpara.axis_stiffness, \
right_ac_vec.back()->pm.stiff_ctrlpara.axis_damping);
com_rsb = new ComRSB();
rdtschunkjs = SchunkJS;
com_rsb->add_msg(rdtschunkjs);
gama_f_filter = new TemporalSmoothingFilter<Eigen::Vector3d>(10,Average,Eigen::Vector3d(0,0,0));
gama_t_filter = new TemporalSmoothingFilter<Eigen::Vector3d>(10,Average,Eigen::Vector3d(0,0,0));
//register cb function
com_rsb->register_external("/foo/sdhmoveto",button_sdh_moveto);
com_rsb->register_external("/foo/sdhaxisvecmoveto",button_sdhaxisvec_moveto);
com_rsb->register_external("/foo/sdhmaintainF",button_sdhmaintainF);
com_rsb->register_external("/foo/sdhslideX",button_sdhslideX);
com_rsb->register_external("/foo/sdhslideY",button_sdhslideY);
com_rsb->register_external("/foo/sdhfoldtool",button_sdhfoldtool);
com_rsb->register_external("/foo/gamaftcalib",button_gamaftcalib);
com_rsb->register_external("/foo/sdh_grav_comp_ctrl",button_sdh_grav_comp_ctrl);
com_rsb->register_external("/foo/sdh_normal_ctrl",button_sdh_normal_ctrl);
com_rsb->register_external("/foo/closeprog",button_closeprog);
#ifdef HAVE_ROS
std::string left_kuka_arm_name="la";
std::string right_kuka_arm_name="ra";
std::string left_schunk_hand_name ="lh";
js_la.name.push_back(left_kuka_arm_name+"_arm_0_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_1_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_2_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_3_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_4_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_5_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_6_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_0_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_1_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_2_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_3_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_4_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_5_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_6_joint");
//for schunk
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_knuckle_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_finger_22_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_finger_23_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_thumb_2_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_thumb_3_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_finger_12_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_finger_13_joint");
js_la.position.resize(7);
js_la.velocity.resize(7);
js_la.effort.resize(7);
js_ra.position.resize(7);
js_ra.velocity.resize(7);
js_ra.effort.resize(7);
js_schunk.position.resize(7);
js_schunk.velocity.resize(7);
js_schunk.effort.resize(7);
js_la.header.frame_id="frame_la";
js_ra.header.frame_id="frame_ra";
js_ra.header.frame_id="frame_lh";
gamma_force_marker_pub = nh->advertise<visualization_msgs::Marker>("gamma_force_marker", 2);
hingedtool_axis_marker_pub = nh->advertise<visualization_msgs::Marker>("hingedtool_axis_marker", 2);
jsPub_la = nh->advertise<sensor_msgs::JointState> ("/la/joint_states", 2);
jsPub_ra = nh->advertise<sensor_msgs::JointState> ("/ra/joint_states", 2);
jsPub_schunk = nh->advertise<sensor_msgs::JointState> ("/lh/joint_states", 2);
ros::spinOnce();
br = new tf::TransformBroadcaster();
std::cout<<"ros init finished"<<std::endl;
#endif
}
