Eigen::Matrix<Scalar, HOMOGENEOUS_TRANSFORM_SIZE, DerivedQdotToV::ColsAtCompileTime> dHomogTrans(
const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedS>& S,
const Eigen::MatrixBase<DerivedQdotToV>& qdot_to_v) {
const int nq_at_compile_time = DerivedQdotToV::ColsAtCompileTime;
int nq = qdot_to_v.cols();
auto qdot_to_twist = (S * qdot_to_v).eval();
const int numel = HOMOGENEOUS_TRANSFORM_SIZE;
Eigen::Matrix<Scalar, numel, nq_at_compile_time> ret(numel, nq);
const auto& Rx = T.linear().col(0);
const auto& Ry = T.linear().col(1);
const auto& Rz = T.linear().col(2);
const auto& qdot_to_omega_x = qdot_to_twist.row(0);
const auto& qdot_to_omega_y = qdot_to_twist.row(1);
const auto& qdot_to_omega_z = qdot_to_twist.row(2);
ret.template middleRows<3>(0) = -Rz * qdot_to_omega_y + Ry * qdot_to_omega_z;
ret.row(3).setZero();
ret.template middleRows<3>(4) = Rz * qdot_to_omega_x - Rx * qdot_to_omega_z;
ret.row(7).setZero();
ret.template middleRows<3>(8) = -Ry * qdot_to_omega_x + Rx * qdot_to_omega_y;
ret.row(11).setZero();
ret.template middleRows<3>(12) = T.linear() * qdot_to_twist.bottomRows(3);
ret.row(15).setZero();
return ret;
}
/**
Transforms this lifting surface.
@param[in] transformation Affine transformation.
*/
void
LiftingSurface::transform(const Eigen::Transform<double, 3, Eigen::Affine> &transformation)
{
// Call super:
this->Surface::transform(transformation);
// Transform bisectors and wake normals:
for (int i = 0; i < n_spanwise_nodes(); i++) {
Vector3d trailing_edge_bisector = trailing_edge_bisectors.row(i);
trailing_edge_bisectors.row(i) = transformation.linear() * trailing_edge_bisector;
Vector3d wake_normal = wake_normals.row(i);
wake_normals.row(i) = transformation.linear() * wake_normal;
}
}
Eigen::Matrix<Scalar, HOMOGENEOUS_TRANSFORM_SIZE, DerivedDT::ColsAtCompileTime> dHomogTransInv(
const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedDT>& dT) {
const int nq_at_compile_time = DerivedDT::ColsAtCompileTime;
int nq = dT.cols();
const auto& R = T.linear();
const auto& p = T.translation();
std::array<int, 3> rows {0, 1, 2};
std::array<int, 3> R_cols {0, 1, 2};
std::array<int, 1> p_cols {3};
auto dR = getSubMatrixGradient(dT, rows, R_cols, T.Rows);
auto dp = getSubMatrixGradient(dT, rows, p_cols, T.Rows);
auto dinvT_R = transposeGrad(dR, R.rows());
auto dinvT_p = (-R.transpose() * dp - matGradMult(dinvT_R, p)).eval();
const int numel = HOMOGENEOUS_TRANSFORM_SIZE;
Eigen::Matrix<Scalar, numel, nq_at_compile_time> ret(numel, nq);
setSubMatrixGradient(ret, dinvT_R, rows, R_cols, T.Rows);
setSubMatrixGradient(ret, dinvT_p, rows, p_cols, T.Rows);
// zero out gradient of elements in last row:
const int last_row = 3;
for (int col = 0; col < T.HDim; col++) {
ret.row(last_row + col * T.Rows).setZero();
}
return ret;
}
typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime>::type dTransformAdjointTranspose(
const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedX>& X,
const Eigen::MatrixBase<DerivedDT>& dT,
const Eigen::MatrixBase<DerivedDX>& dX) {
assert(dT.cols() == dX.cols());
int nq = dT.cols();
const auto& R = T.linear();
const auto& p = T.translation();
std::array<int, 3> rows {0, 1, 2};
std::array<int, 3> R_cols {0, 1, 2};
std::array<int, 1> p_cols {3};
auto dR = getSubMatrixGradient(dT, rows, R_cols, T.Rows);
auto dp = getSubMatrixGradient(dT, rows, p_cols, T.Rows);
auto Rtranspose = R.transpose();
auto dRtranspose = transposeGrad(dR, R.rows());
typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime>::type ret(X.size(), nq);
std::array<int, 3> Xomega_rows {0, 1, 2};
std::array<int, 3> Xv_rows {3, 4, 5};
for (int col = 0; col < X.cols(); col++) {
auto Xomega_col = X.template block<3, 1>(0, col);
auto Xv_col = X.template block<3, 1>(3, col);
std::array<int, 1> col_array {col};
auto dXomega_col = getSubMatrixGradient(dX, Xomega_rows, col_array, X.rows());
auto dXv_col = getSubMatrixGradient(dX, Xv_rows, col_array, X.rows());
auto dp_hatXv_col = (dp.colwise().cross(Xv_col) - dXv_col.colwise().cross(p)).eval();
auto Xomega_col_minus_p_cross_Xv_col = (Xomega_col - p.cross(Xv_col)).eval();
auto dXomega_transformed_col = (Rtranspose * (dXomega_col - dp_hatXv_col) + matGradMult(dRtranspose, Xomega_col_minus_p_cross_Xv_col)).eval();
auto dRtransposeXv_col = (Rtranspose * dXv_col + matGradMult(dRtranspose, Xv_col)).eval();
setSubMatrixGradient(ret, dXomega_transformed_col, Xomega_rows, col_array, X.rows());
setSubMatrixGradient(ret, dRtransposeXv_col, Xv_rows, col_array, X.rows());
}
return ret;
}
template <typename PointT, typename Scalar> double
pcl::getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud,
Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
Eigen::Matrix<Scalar, 4, 1> centroid;
pcl::computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid);
EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> eigen_vects;
Eigen::Matrix<Scalar, 3, 1> eigen_vals;
pcl::eigen33 (covariance_matrix, eigen_vects, eigen_vals);
double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1);
double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2);
transform.translation () = centroid.head (3);
transform.linear () = eigen_vects;
return (std::min (rel1, rel2));
}
void IterativeClosestPoint::execute() {
float error = std::numeric_limits<float>::max(), previousError;
uint iterations = 0;
// Get access to the two point sets
PointSetAccess::pointer accessFixedSet = ((PointSet::pointer)getStaticInputData<PointSet>(0))->getAccess(ACCESS_READ);
PointSetAccess::pointer accessMovingSet = ((PointSet::pointer)getStaticInputData<PointSet>(1))->getAccess(ACCESS_READ);
// Get transformations of point sets
AffineTransformation::pointer fixedPointTransform2 = SceneGraph::getAffineTransformationFromData(getStaticInputData<PointSet>(0));
Eigen::Affine3f fixedPointTransform;
fixedPointTransform.matrix() = fixedPointTransform2->matrix();
AffineTransformation::pointer initialMovingTransform2 = SceneGraph::getAffineTransformationFromData(getStaticInputData<PointSet>(1));
Eigen::Affine3f initialMovingTransform;
initialMovingTransform.matrix() = initialMovingTransform2->matrix();
// These matrices are Nx3
MatrixXf fixedPoints = accessFixedSet->getPointSetAsMatrix();
MatrixXf movingPoints = accessMovingSet->getPointSetAsMatrix();
Eigen::Affine3f currentTransformation = Eigen::Affine3f::Identity();
// Want to choose the smallest one as moving
bool invertTransform = false;
if(false && fixedPoints.cols() < movingPoints.cols()) {
reportInfo() << "switching fixed and moving" << Reporter::end;
// Switch fixed and moving
MatrixXf temp = fixedPoints;
fixedPoints = movingPoints;
movingPoints = temp;
invertTransform = true;
// Apply initial transformations
//currentTransformation = fixedPointTransform.getTransform();
movingPoints = fixedPointTransform*movingPoints.colwise().homogeneous();
fixedPoints = initialMovingTransform*fixedPoints.colwise().homogeneous();
} else {
// Apply initial transformations
//currentTransformation = initialMovingTransform.getTransform();
movingPoints = initialMovingTransform*movingPoints.colwise().homogeneous();
fixedPoints = fixedPointTransform*fixedPoints.colwise().homogeneous();
}
do {
previousError = error;
MatrixXf movedPoints = currentTransformation*(movingPoints.colwise().homogeneous());
// Match closest points using current transformation
MatrixXf rearrangedFixedPoints = rearrangeMatrixToClosestPoints(
fixedPoints, movedPoints);
// Get centroids
Vector3f centroidFixed = getCentroid(rearrangedFixedPoints);
//reportInfo() << "Centroid fixed: " << Reporter::end;
//reportInfo() << centroidFixed << Reporter::end;
Vector3f centroidMoving = getCentroid(movedPoints);
//reportInfo() << "Centroid moving: " << Reporter::end;
//reportInfo() << centroidMoving << Reporter::end;
Eigen::Transform<float, 3, Eigen::Affine> updateTransform = Eigen::Transform<float, 3, Eigen::Affine>::Identity();
if(mTransformationType == IterativeClosestPoint::RIGID) {
// Create correlation matrix H of the deviations from centroid
MatrixXf H = (movedPoints.colwise() - centroidMoving)*
(rearrangedFixedPoints.colwise() - centroidFixed).transpose();
// Do SVD on H
Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
// Estimate rotation as R=V*U.transpose()
MatrixXf temp = svd.matrixV()*svd.matrixU().transpose();
Matrix3f d = Matrix3f::Identity();
d(2,2) = sign(temp.determinant());
Matrix3f R = svd.matrixV()*d*svd.matrixU().transpose();
// Estimate translation
Vector3f T = centroidFixed - R*centroidMoving;
updateTransform.linear() = R;
updateTransform.translation() = T;
} else {
// Only translation
Vector3f T = centroidFixed - centroidMoving;
updateTransform.translation() = T;
}
// Update current transformation
currentTransformation = updateTransform*currentTransformation;
// Calculate RMS error
MatrixXf distance = rearrangedFixedPoints - currentTransformation*(movingPoints.colwise().homogeneous());
error = 0;
for(uint i = 0; i < distance.cols(); i++) {
error += pow(distance.col(i).norm(),2);
}
error = sqrt(error / distance.cols());
iterations++;
reportInfo() << "Error: " << error << Reporter::end;
// To continue, change in error has to be above min error change and nr of iterations less than max iterations
} while(previousError-error > mMinErrorChange && iterations < mMaxIterations);
if(invertTransform){
currentTransformation = currentTransformation.inverse();
}
mError = error;
mTransformation->matrix() = currentTransformation.matrix();
}
int main(int argc, char** argv){
ros::init(argc, argv, "workspace_transformation");
ros::NodeHandle node;
ros::Rate loop_rate(loop_rate_in_hz);
// Topics
// Publishers
ros::Publisher pub = node.advertise<geometry_msgs::PoseStamped>("poseSlaveWorkspace", 1);
ros::Publisher pub_poseSlaveWSOrigin = node.advertise<geometry_msgs::PoseStamped>("poseSlaveWorkspaceOrigin", 1);
ros::Publisher pub_set_camera_pose = node.advertise<geometry_msgs::PoseStamped>("Set_ActiveCamera_Pose", 1);
pub_OmniForceFeedback = node.advertise<geometry_msgs::Vector3>("set_forces", 1);
// Subscribers
ros::Subscriber sub_PoseMasterWS = node.subscribe("poseMasterWorkspace", 1, &PoseMasterWSCallback);
ros::Subscriber sub_BaseMasterWS = node.subscribe("baseMasterWorkspace", 1, &BaseMasterWSCallback);
ros::Subscriber sub_BaseSlaveWS = node.subscribe("baseSlaveWorkspace", 1, &BaseSlaveWSCallback);
ros::Subscriber sub_OriginSlaveWS = node.subscribe("originSlaveWorkspace", 1, &OriginSlaveWSCallback);
ros::Subscriber sub_scale = node.subscribe("scale", 1, &scaleCallback);
ros::Subscriber subOmniButtons = node.subscribe("button", 1, &omniButtonCallback);
ros::Subscriber sub_HapticAngles = node.subscribe("angles", 1, &HapticAnglesCallback);
// Services
ros::ServiceServer service_server_algorithm = node.advertiseService("set_algorithm", algorithmSrvCallback);
// INITIALIZATION ------------------------------------------------------------------------
// Haptic
omni_white_button_pressed = omni_grey_button_pressed = first_haptic_angle_read = false;
// Algorithm
algorithm_type = 0;
for (unsigned int i=0; i<3; i++) scale[i] = 1.0;
m_init = s_init = mi_init = s0_init = algorithm_set = false;
dm << 0,0,0;
ds << 0,0,0;
pm_im1 << 0,0,0;
ps_im1 << 0,0,0;
vm_i << 0,0,0;
ps_0 << 0.0, 0.0, 0.0;
quats_0.x() = quats_0.y() = quats_0.z() = 0.0;
quats_0.w() = 1.0;
Rs_0 = quats_0.toRotationMatrix();
// Auxiliary pose
geometry_msgs::PoseStamped outputPose;
outputPose.pose.position.x = outputPose.pose.position.y = outputPose.pose.position.z = 0.0;
outputPose.pose.orientation.x = outputPose.pose.orientation.y = outputPose.pose.orientation.z = 0.0;
outputPose.pose.orientation.w = 1.0;
// Workspace boundaries
Xmin = -5.0;
Ymin = -5.0;
Zmin = 0.0;
Xmax = 5.0;
Ymax = 5.0;
Zmax = 2.0;
// Default camera pose
geometry_msgs::PoseStamped cameraPose;
cameraPose.pose.position.x = cameraPose.pose.position.y = cameraPose.pose.position.z = 0.0;
cameraPose.pose.orientation.x = cameraPose.pose.orientation.y = cameraPose.pose.orientation.z = 0.0;
cameraPose.pose.orientation.w = 1.0;
Eigen::Vector3d origin_cam = 10.0 * Eigen::Vector3d(1.0, 0.0, 0.5);
T_camPose_S.translation() = Eigen::Vector3d(0.0, 0.0, 1.0) + origin_cam;
Eigen::Vector3d eigen_cam_axis_z = origin_cam.normalized();
Eigen::Vector3d eigen_cam_axis_x = ( Eigen::Vector3d::UnitZ().cross( eigen_cam_axis_z ) ).normalized();
Eigen::Vector3d eigen_cam_axis_y = ( eigen_cam_axis_z.cross( eigen_cam_axis_x ) ).normalized();
T_camPose_S.linear() << eigen_cam_axis_x(0), eigen_cam_axis_y(0), eigen_cam_axis_z(0),
eigen_cam_axis_x(1), eigen_cam_axis_y(1), eigen_cam_axis_z(1),
eigen_cam_axis_x(2), eigen_cam_axis_y(2), eigen_cam_axis_z(2);
// Time management
period = 1.0/(double)loop_rate_in_hz;
timeval past_time_, current_time_;
gettimeofday(¤t_time_, NULL);
time_increment_ = 0;
// File management
std::ofstream WTdataRecord;
WTdataRecord.open("/home/users/josep.a.claret/data/WTdataRecord.txt", std::ofstream::trunc);
// UPDATE -------------------------------------------------------------------------------
while (ros::ok())
{
if (m_init && s_init && mi_init)
{
// Time management
// past_time_ = current_time_;
// gettimeofday(¤t_time_, NULL);
// time_increment_ = ( (current_time_.tv_sec*1e6 + current_time_.tv_usec) - (past_time_.tv_sec*1e6 + past_time_.tv_usec) ) / 1e6;
time_increment_ = period;
// Velocity computation
vm_i = (pm_i - pm_im1)/time_increment_;
ws_tf_alg_scaling_Bubble_smoothposrotRateControl_camRateControl_FormatJAC();
// std::cout << "-- VISUALIZATION DATA ---------------------------------------------------" << std::endl;
// std::cout << "alg: " << algorithm_type
// std::cout << " time inc: " << time_increment_*1000 << std::endl;
// std::cout << " pm_0: " << 1000*pm_0.transpose()<< std::endl;
// std::cout << " pm_im1: " << 1000*pm_im1.transpose()<< std::endl;
// std::cout << " pm_i: " << 1000*pm_i.transpose()<< std::endl;
// std::cout << " ps_0: " << 1000*ps_0.transpose()<< std::endl;
// std::cout << " ps_im1: " << 1000*ps_im1.transpose()<< std::endl;
// std::cout << " ps_i: " << 1000*ps_i.transpose()<< std::endl;
// std::cout << " dm: " << 1000*dm.transpose()<< std::endl;
// std::cout << " ds: " << 1000*ds.transpose()<< std::endl;
// std::cout << " vm_i: " << 1000*vm_i.transpose()<< std::endl;
// std::cout << "Rm_0" << std::endl;
// std::cout << Rm_0 << std::endl;
// std::cout << "Rm" << std::endl;
// std::cout << Rm << std::endl;
// std::cout << "Rm * ds" << std::endl;
// std::cout << Rm * ds << std::endl;
// std::cout << "Rs.transpose() * Rm * ds" << std::endl;
// std::cout << Rs.transpose() * Rm * ds << std::endl;
//
// std::cout << "Rm_i" << std::endl;
// std::cout << Rm_i << std::endl;
// std::cout << "Rs_i" << std::endl;
// std::cout << Rs_i << std::endl;
// std::cout << "quat Rs_i" << std::endl;
// std::cout << quats_i.x() << " " << quats_i.y() << " " << quats_i.z() << " " << quats_i.w() << std::endl;
pm_im1 = pm_i;
ps_im1 = ps_i;
// Send data ***********************************************
// Slave pose
outputPose.header.stamp = ros::Time::now();
outputPose.pose.position.x = ps_i(0,0);
outputPose.pose.position.y = ps_i(1,0);
outputPose.pose.position.z = ps_i(2,0);
outputPose.pose.orientation.x = quats_i.x();
outputPose.pose.orientation.y = quats_i.y();
outputPose.pose.orientation.z = quats_i.z();
outputPose.pose.orientation.w = quats_i.w();
pub.publish(outputPose);
// Slave origin pose
outputPose.header.stamp = ros::Time::now();
outputPose.pose.position.x = ps_0(0,0);
outputPose.pose.position.y = ps_0(1,0);
outputPose.pose.position.z = ps_0(2,0);
outputPose.pose.orientation.x = quats_0.x();
outputPose.pose.orientation.y = quats_0.y();
outputPose.pose.orientation.z = quats_0.z();
outputPose.pose.orientation.w = quats_0.w();
pub_poseSlaveWSOrigin.publish(outputPose);
}
// Camera pose
cameraPose.header.stamp = ros::Time::now();
cameraPose.pose.position.x = T_camPose_S.translation()(0);
cameraPose.pose.position.y = T_camPose_S.translation()(1);
cameraPose.pose.position.z = T_camPose_S.translation()(2);
cameraPose.pose.orientation.x = Quaternion<double>(T_camPose_S.linear()).x();
cameraPose.pose.orientation.y = Quaternion<double>(T_camPose_S.linear()).y();
cameraPose.pose.orientation.z = Quaternion<double>(T_camPose_S.linear()).z();
cameraPose.pose.orientation.w = Quaternion<double>(T_camPose_S.linear()).w();
pub_set_camera_pose.publish(cameraPose);
ros::spinOnce();
loop_rate.sleep();
}
WTdataRecord.close();
return 0;
}