void
Glx::Sphere::sph_interp(Glx::Vector& v1, Glx::Vector& v2,
float t, Glx::Vector& res)
{
double cur[16], inv[16];
float alpha = acos( dot(v1, v2)/(v1.magnitude()*v2.magnitude()) );
float delta = alpha*t;
Glx::Vector i,j,k,tmp, _v1, _v2;
i.set(v1[0],v1[1],v1[2]);
i.normalize();
/* calc the vector ortho to these two, the 'K' vector */
k = cross(i, v2);
/* calc a new 'J' vector bases using I and K*/
j = cross(k,i);
j.normalize();
buildMat(cur,i,j,k);
_v1[0] = projection(v1, i);
_v1[1] = projection(v1, j);
_v1[2] = projection(v1, k);
if( Glx::inv4x4(cur, inv) ){
rot_z(_v1, delta, _v2 );
Glx::xformVec(_v2, cur, res);
} else {
std::cerr << "! INV failed!" << std::endl;
}
}
int main(){
V3f x(0,0,1); V3f xr(rot_x(x, 0.87)); same("x rotation", x.dot(xr), cos(0.87));
V3f y(0,0,1); V3f yr(rot_y(y, 0.23)); same("y rotation", y.dot(yr), cos(0.23));
V3f z(1,0,0); V3f zr(rot_z(z, 0.19)); same("z rotation", z.dot(zr), cos(0.19));
V3f nx(3,2,5);
V3f ny(-2,3,4);
V3f nz(-4,4,3.8);
V3f nnx(3,2,5);
V3f nny(-2,3,4);
V3f nnz(-4,4,3.8);
ortoNormalize(nnx, nny, nnz);
same("x unit", nnx.length(), 1.0);
same("y unit", nny.length(), 1.0);
same("z unit", nnz.length(), 1.0);
V3f tmp; tmp.cross(nnx, nx);
same("x colinear", tmp.length(), 0.0);
tmp.cross(nnx, nny); tmp-=nnz; same("x orto", tmp.length(), 0);
tmp.cross(nny, nnz); tmp-=nnx; same("y orto", tmp.length(), 0);
tmp.cross(nnz, nnx); tmp-=nny; same("z orto", tmp.length(), 0);
};
void rotate(t_dpoint *coor, double angle_x,
double angle_y, double angle_z)
{
if (angle_x != 0.0)
rot_x(coor, cos(angle_x), sin(angle_x));
if (angle_y != 0.0)
rot_y(coor, cos(angle_y), sin(angle_y));
if (angle_z != 0.0)
rot_z(coor, cos(angle_z), sin(angle_z));
}
Attitude::Attitude(const float dipAngle, const float dip, const Vector3f position)
{
if ((dipAngle <= 0.0) | (dipAngle >= 90))
LOG(WARNING) << "dipAngle out of bounds. Are you sure" << std::endl;
Vector3f n = Vector3f::UnitZ(); // is a vertical vector
// we rotate a vertical vector on the two axis, of the given angles
AngleAxis<float> rot_x(dipAngle / 180 * M_PI, -Vector3f::UnitX());
AngleAxis<float> rot_z(dip / 180 * M_PI, -Vector3f::UnitZ());
Vector3f out_n = rot_z * rot_x * n;
this->setNormal(out_n.normalized());
this->setPosition(position);
}
// @Override
void Billboard::compute_world_mat() const
{
Matrix scale_mat;
scale_mat.scale(scale(), scale(), scale());
const D3DXMATRIX & view_rot = si3::Manager::camera().view_rot();
Matrix inverse_rot = inverse_matrix_of_rot(view_rot);
Matrix rot_mat;
rot_mat.rotate_z(rot_z());
rot_mat *= inverse_rot;
Matrix parallel_mat;
parallel_mat.parallel(x(), y(), z());
world_mat = scale_mat;
world_mat *= rot_mat;
world_mat *= parallel_mat;
}
glm::mat4 CreateRotateMatrix(float rx, float ry, float rz) {
glm::mat4 rot_x(1.0f);
rot_x[1][1] = cos(rx);
rot_x[2][1] = -sin(rx);
rot_x[1][2] = sin(rx);
rot_x[2][2] = cos(rx);
glm::mat4 rot_y(1.0f);
rot_y[0][0] = cos(ry);
rot_y[2][0] = sin(ry);
rot_y[0][2] = -sin(ry);
rot_y[2][2] = cos(ry);
glm::mat4 rot_z(1.0f);
rot_z[0][0] = cos(rz);
rot_z[1][0] = -sin(rz);
rot_z[0][1] = sin(rz);
rot_z[1][1] = cos(rz);
return rot_z * rot_y * rot_x;
}
void ForceServoController::get_desired_lv(Robot *robot, Task *t, Eigen::VectorXd ft, RobotState* rs){
ForceServoTask tst(t->curtaskname.forcet);
tst = *(ForceServoTask*)t;
Eigen::Vector3d v_ratio;
v_ratio.setZero();
//get the desired contact force
double desiredf;
tst.get_desired_cf_kuka(desiredf);
bool rot_by_force;
rot_by_force = true;
if(rot_by_force == true)
{
if ((ft(0) < 1e-05)&&(ft(1) < 1e-05)){
v_ratio.setZero();
// std::cout<<"no control before no contact force."<<std::endl;
}
else{
//desired rotation axis is not changed. so the online estimated global direction of rotation axis is:
//v_g_online = T_l2g_online * T_g2l_init * v_g
Eigen::Vector3d rot_z = rs->robot_orien["eef"] * m_init_tm.transpose() * tst.init_contact_frame.col(2);
Eigen::Matrix3d contact_frame;
contact_frame.setZero();
contact_frame = rs->robot_orien["eef"] * m_init_tm.transpose() * tst.init_contact_frame;
std::cout<<"contact frame "<<contact_frame<<std::endl;
Eigen::Vector3d ft_local,ft_global_nofriction;
ft_local.setZero();
ft_global_nofriction.setZero();
std::cout<<"ft head3 "<<ft(0)<<","<<ft(1)<<","<<ft(2)<<","<<std::endl;
ft_local = contact_frame.transpose() * ft.head(3);
std::cout<<"ft_local before "<<ft_local(0)<<","<<ft_local(1)<<","<<ft_local(2)<<","<<std::endl;
ft_local(1) = 0;
ft_local(2) = 0;
std::cout<<"ft_local after "<<ft_local(0)<<","<<ft_local(1)<<","<<ft_local(2)<<","<<std::endl;
ft_global_nofriction =contact_frame * ft_local;
std::cout<<"ft_global_nofriction "<<ft_global_nofriction(0)<<","<<ft_global_nofriction(1)<<","<<ft_global_nofriction(2)<<","<<std::endl;
Eigen::Vector3d norm_ft = ft_global_nofriction.normalized();
Eigen::Vector3d rot_axis = norm_ft.cross(rot_z);
double angle = M_PI/2.0 - acos(norm_ft.dot(rot_z));
vel_rec2<<angle<<","<<norm_ft(0)<<","<<norm_ft(1)<<","<<norm_ft(2)<<","<<rot_z(0)<<","<<rot_z(1)<<","<<rot_z(2)<<std::endl;
std::cout<<"angle is"<<angle<<std::endl;
v_ratio = 1 * rs->robot_orien["eef"].transpose() * rot_axis * angle;
// std::cout<<"v_ratio is "<<v_ratio(0)<<","<<v_ratio(1)<<","<<v_ratio(2)<<std::endl;
}
}
lov_f = v_ratio;
if(tst.mft == GLOBAL){
if(ft.head(3).norm() != 0)
{
tst.desired_surf_nv = 0.2 * old_ft_dir + 0.8 * ft.head(3).normalized();
}
else
{
tst.desired_surf_nv = tst.desired_init_surf_nv;
}
old_ft_dir = tst.desired_surf_nv;
Eigen::VectorXd delta_g;
delta_g.setZero(6);
delta_g.head(3) = (-1) * (desiredf * tst.desired_surf_nv - ft.head(3));
delta_g(3) = 0;
delta_g(4) = 0;
delta_g(5) = 0;
//vel from global to robot eef
//std::cout<<"global vel from force "<<delta_g(0)<<","<<delta_g(1)<<","<<delta_g(2)<<std::endl;
deltais.head(3) = rs->robot_orien["eef"].transpose() * delta_g.head(3);
deltais(3) = 0;
deltais(4) = 0;
deltais(5) = 0;
}
else{
deltais(0) = 1;
deltais(1) = 1;
deltais(2) = desiredf - ft(2);
//!this two value can be updated by other feedback in future
deltais(3) = 0;
deltais(4) = 0;
deltais(5) = 0;
}
//std::cout<<"current force "<<ft(0)<<","<<ft(1)<<","<<ft(2)<<","<<std::endl;
// std::cout<<"desiredis "<<deltais(0)<<","<<deltais(1)<<","<<deltais(2)<<","<<std::endl;
// std::cout<<"current task name "<<tst.curtaskname.forcet<<std::endl;
// std::cout<<"kop: "<<std::endl;
// std::cout<<Kop[tst.curtaskname.forcet]<<std::endl;
// std::cout<<"kpp: "<<std::endl;
// std::cout<<Kpp[tst.curtaskname.forcet]<<std::endl;
// std::cout<<"tjkm: "<<std::endl;
// std::cout<<tjkm[tst.curtaskname.forcet]<<std::endl;
// std::cout<<"sm: "<<std::endl;
// std::cout<<sm[tst.curtaskname.forcet]<<std::endl;
deltape = Kpp[tst.curtaskname.forcet] * sm[tst.curtaskname.forcet] * deltais + \
Kpi[tst.curtaskname.forcet] * sm[tst.curtaskname.forcet] * deltais_int + \
Kpd[tst.curtaskname.forcet] * sm[tst.curtaskname.forcet] * (deltais - deltais_old);
llv_f = deltape.head(3);
// std::cout<<"local vel from force "<<llv_f(0)<<","<<llv_f(1)<<","<<llv_f(2)<<std::endl;
//llv_f.setZero();
//lov_f = Kop[tst.curtaskname.forcet] * v_ratio;
//~ std::cout<<lov_f(0)<<","<<lov_f(1)<<","<<lov_f(2)<<std::endl;
//~ vel_rec2<<est_v_g(0)<<","<<est_v_g(1)<<","<<est_v_g(2)<<","\
//~ <<filtered_lv[0]<<","<<filtered_lv[1]<<","<<filtered_lv[2]<<","\
//~ <<v_ratio(0)<<","<<v_ratio(1)<<","<<v_ratio(2)<<","\
//~ <<llv_f(0)<<","<<llv_f(1)<<","<<llv_f(2)<<","\
//~ <<lov_f(0)<<","<<lov_f(1)<<","<<lov_f(2)<<std::endl;
//limit_vel(get_llv_limit(),llv_f,lov_f);
deltais_old = deltais;
}
void rot_vec_inv(t_vec *vec, t_vec *angle)
{
rot_x(vec, -angle->x);
rot_y(vec, -angle->y);
rot_z(vec, -angle->z);
}
void rot_vec(t_vec *vec, t_vec *angle)
{
rot_x(vec, angle->x);
rot_y(vec, angle->y);
rot_z(vec, angle->z);
}
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input) {
// std::cerr << "in cloud_cb" << std::endl;
/* 0. Importing input cloud */
std_msgs::Header header = input->header;
// std::string frame_id = input->header.frame_id;
// sensor_msgs::PointCloud2 input_cloud = *input;
pcl::PCLPointCloud2 *cloud = new pcl::PCLPointCloud2; // initialize object
pcl_conversions::toPCL(*input, *cloud); // from input, generate content of cloud
/* 1. Downsampling and Publishing voxel */
// LeafSize: should small enough to caputure a leaf of plants
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); // imutable pointer
pcl::PCLPointCloud2 cloud_voxel; // cloud_filtered to cloud_voxel
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr); // set input
sor.setLeafSize(0.02f, 0.02f, 0.02f); // 2cm, model equation
sor.filter(cloud_voxel); // set output
sensor_msgs::PointCloud2 output_voxel;
pcl_conversions::fromPCL(cloud_voxel, output_voxel);
pub_voxel.publish(output_voxel);
/* 2. Filtering with RANSAC */
// RANSAC initialization
pcl::SACSegmentation<pcl::PointXYZ> seg; // Create the segmentation object
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
seg.setOptimizeCoefficients(true); // Optional
// seg.setModelType(pcl::SACMODEL_PLANE);
seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); // Use SACMODEL_PERPENDICULAR_PLANE instead
seg.setMethodType(pcl::SAC_RANSAC);
// minimum number of points calculated from N and distanceThres
seg.setMaxIterations (1000); // N in RANSAC
// seg.setDistanceThreshold(0.025); // 0.01 - 0.05 default: 0.02 // 閾値（しきい値）
seg.setDistanceThreshold(0.050); // 0.01 - 0.05 default: 0.02 // 閾値（しきい値）
// param for perpendicular
seg.setAxis(Eigen::Vector3f (0.0, 0.0, 1.0));
seg.setEpsAngle(pcl::deg2rad (10.0f)); // 5.0 -> 10.0
// convert from PointCloud2 to PointXYZ
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxel_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(cloud_voxel, *cloud_voxel_xyz);
// RANSAC application
seg.setInputCloud(cloud_voxel_xyz);
seg.segment(*inliers, *coefficients); // values are empty at beginning
// inliers.indices have array index of the points which are included as inliers
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane_xyz (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = inliers->indices.begin ();
pit != inliers->indices.end (); pit++) {
cloud_plane_xyz->points.push_back(cloud_voxel_xyz->points[*pit]);
}
// Organized as an image-structure
cloud_plane_xyz->width = cloud_plane_xyz->points.size ();
cloud_plane_xyz->height = 1;
/* insert code to set arbitary frame_id setting
such as frame_id ="/assemble_cloud_1"
with respect to "/odom or /base_link" */
// Conversions: PointCloud<T>, PCLPointCloud2, sensor_msgs::PointCloud2
pcl::PCLPointCloud2 cloud_plane_pcl;
pcl::toPCLPointCloud2(*cloud_plane_xyz, cloud_plane_pcl);
sensor_msgs::PointCloud2 cloud_plane_ros;
pcl_conversions::fromPCL(cloud_plane_pcl, cloud_plane_ros);
// cloud_plane_ros.header.frame_id = "/base_link"; // odom -> /base_link
cloud_plane_ros.header.frame_id = header.frame_id;
cloud_plane_ros.header.stamp = header.stamp; // ros::Time::now() -> header.stamp
pub_plane.publish(cloud_plane_ros);
/* 3. PCA application to get eigen */
pcl::PCA<pcl::PointXYZ> pca;
pca.setInputCloud(cloud_plane_xyz);
Eigen::Matrix3f eigen_vectors = pca.getEigenVectors(); // 3x3 eigen_vectors(n,m)
/* 4. PCA Visualization */
visualization_msgs::Marker points;
// points.header.frame_id = "/base_link"; // odom -> /base_link
points.header.frame_id = header.frame_id;
points.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
points.ns = "pca"; // namespace + id
points.id = 0; // pca/0
points.action = visualization_msgs::Marker::ADD;
points.type = visualization_msgs::Marker::ARROW;
points.pose.orientation.w = 1.0;
points.scale.x = 0.05;
points.scale.y = 0.05;
points.scale.z = 0.05;
points.color.g = 1.0f;
points.color.r = 0.0f;
points.color.b = 0.0f;
points.color.a = 1.0;
geometry_msgs::Point p_0, p_1;
p_0.x = 0; p_0.y = 0; p_0.z = 0; // get from tf
p_1.x = eigen_vectors(0,0);
p_1.y = eigen_vectors(0,1); // always negative
std::cerr << "y = " << eigen_vectors(0,1) << std::endl;
p_1.z = eigen_vectors(0,2);
points.points.push_back(p_0);
points.points.push_back(p_1);
pub_marker.publish(points);
/* 5. Point Cloud Rotation */
eigen_vectors(0,2) = 0; // ignore very small z-value
double norm = pow((pow(eigen_vectors(0,0), 2) + pow(eigen_vectors(0,1), 2)), 0.5);
double nx = eigen_vectors(0,0) / norm;
double ny = eigen_vectors(0,1) / norm;
Eigen::Matrix4d rot_z; // rotation inversed, convert to Matrix4f
rot_z(0,0) = nx; rot_z(0,1) = ny; rot_z(0,2) = 0; rot_z(0,3) = 0; // ny: +/-
rot_z(1,0) = -ny; rot_z(1,1) = nx; rot_z(1,2) = 0; rot_z(1,3) = 0; // ny: +/-
rot_z(2,0) = 0; rot_z(2,1) = 0; rot_z(2,2) = 1; rot_z(2,3) = 0;
rot_z(3,0) = 0; rot_z(3,1) = 0; rot_z(3,2) = 0; rot_z(3,3) = 1;
// Transformation: Rotation, Translation
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_rot (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(*cloudPtr, *cloud_xyz); // from PointCloud2 to PointXYZ
pcl::transformPointCloud(*cloud_xyz, *cloud_xyz_rot, rot_z); // original, transformed, transformation
pcl::PCLPointCloud2 cloud_rot_pcl;
sensor_msgs::PointCloud2 cloud_rot_ros;
pcl::toPCLPointCloud2(*cloud_xyz_rot, cloud_rot_pcl);
pcl_conversions::fromPCL(cloud_rot_pcl, cloud_rot_ros);
pub_rot.publish(cloud_rot_ros);
/* 6. Point Cloud Reduction */
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr >
vector_cloud_separated_xyz = separate(cloud_xyz_rot, header);
pcl::PCLPointCloud2 cloud_separated_pcl;
sensor_msgs::PointCloud2 cloud_separated_ros;
pcl::toPCLPointCloud2(*vector_cloud_separated_xyz.at(1), cloud_separated_pcl);
pcl_conversions::fromPCL(cloud_separated_pcl, cloud_separated_ros);
// cloud_separated_ros.header.frame_id = "/base_link"; // odom -> /base_link
cloud_separated_ros.header.frame_id = header.frame_id;
cloud_separated_ros.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
pub_red.publish(cloud_separated_ros);
// setMarker
// visualization_msgs::Marker width_min_line;
// width_min_line.header.frame_id = "/base_link";
// width_min_line.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
// width_min_line.ns = "width_min";
// width_min_line.action = visualization_msgs::Marker::ADD;
// width_min_line.type = visualization_msgs::Marker::LINE_STRIP;
// width_min_line.pose.orientation.w = 1.0;
// width_min_line.id = 0;
// width_min_line.scale.x = 0.025;
// width_min_line.color.r = 0.0f;
// width_min_line.color.g = 1.0f;
// width_min_line.color.b = 0.0f;
// width_min_line.color.a = 1.0;
// width_min_line.points.push_back(point_vector.at(0));
// width_min_line.points.push_back(point_vector.at(2));
// pub_marker.publish(width_min_line);
// /* 6. Visualize center line */
// visualization_msgs::Marker line_strip;
// line_strip.header.frame_id = "/base_link"; // odom -> /base_link
// line_strip.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
// line_strip.ns = "center";
// line_strip.action = visualization_msgs::Marker::ADD;
// line_strip.type = visualization_msgs::Marker::LINE_STRIP;
// line_strip.pose.orientation.w = 1.0;
// line_strip.id = 0; // set id
// line_strip.scale.x = 0.05;
// line_strip.color.r = 1.0f;
// line_strip.color.g = 0.0f;
// line_strip.color.b = 0.0f;
// line_strip.color.a = 1.0;
// // geometry_msgs::Point p_stitch, p_min;
// p_s.x = 0; p_s.y = 0; p_s.z = 0;
// p_e.x = p_m.x; p_e.y = p_m.y; p_e.z = 0;
// line_strip.points.push_back(p_s);
// line_strip.points.push_back(p_e);
// pub_marker.publish(line_strip);
/* PCA Visualization */
// geometry_msgs::Pose pose; tf::poseEigenToMsg(pca.getEigenVectors, pose);
/* to use Pose marker in rviz */
/* Automatic Measurement */
// 0-a. stitch measurement: -0.5 < z < -0.3
// 0-b. min width measurement: 0.3 < z < 5m
// 1. iterate
// 2. pick point if y < 0
// 3. compare point with all points if 0 < y
// 4. pick point-pare recording shortest distance
// 5. compare the point with previous point
// 6. update min
// 7. display value in text in between 2 points
}
