void Hand_filter::update(tf::Vector3 p, tf::Quaternion& q){
if(b_first){
p_filter_buffer.push_back(p);
q_filter_buffer.push_back(q);
if(p_filter_buffer.size() == p_filter_buffer.capacity()){
b_first = false;
ROS_INFO("====== hand filter ======");
// ROS_INFO("buffer full: %d",p_filter_buffer.size());
ROS_INFO("p: %f %f %f",p.x(),p.y(),p.z());
ROS_INFO("q: %f %f %f %f",q.x(),q.y(),q.z(),q.w());
k_position(0) = p.x();k_position(1) = p.y(); k_position(2) = p.z();
kalman_filter.Init(k_position);
q_tmp = q;
p_tmp = p;
}
}else{
/// Orientation filter
if(jumped(q,q_tmp,q_threashold)){
ROS_INFO("q jumped !");
q = q_tmp;
}
q_attractor(q,up);
q = q_tmp.slerp(q,0.1);
/// Position filter
if(!jumped(p,p_tmp,p_threashold)){
k_measurement(0) = p.x();
k_measurement(1) = p.y();
k_measurement(2) = p.z();
}else{
ROS_INFO("p jumped !");
k_measurement(0) = p_tmp.x();
k_measurement(1) = p_tmp.y();
k_measurement(2) = p_tmp.z();
}
kalman_filter.Update(k_measurement,0.001);
kalman_filter.GetPosition(k_position);
p.setValue(k_position(0),k_position(1),k_position(2));
q_tmp = q;
p_tmp = p;
}
}
Socket_one::Socket_one(std::string name, const tf::Vector3& origin, const tf::Vector3 &rpy,float scale){
//const std::string& name, const geo::fCVec3& C, const geo::fMat33& R, float r
geo::fMat33 R;
R.eye();
disk_radius = 0.02 * scale;
hole_radius = 0.002 * scale;
std::array<geo::Disk,3> holes;
geo::fCVec3 position;
position.zeros();
geo::Disk plate("plate",position,R,disk_radius);
position.zeros();
position(0) = -0.01 * scale;
holes[0] = geo::Disk("hole_left",position,R,hole_radius);
position.zeros();
position(0) = 0.01 * scale;
holes[1] = geo::Disk("hole_right",position,R,hole_radius);
position.zeros();
position(1) = 0.005 * scale;
holes[2] = geo::Disk("hole_up",position,R,hole_radius);
wsocket = wobj::WSocket(name,plate,holes);
geo::fCVec3 dim = {{0.07,0.07,0.05}};
dim = dim * scale;
geo::fCVec3 orientation = {{0,0,0}};
position.zeros();
position(2) = -0.005/2 - (0.05 - 0.005)/2;
wbox = wobj::WBox("box_socket_one",dim,position,orientation);
// Transformation
orientation(0) = rpy.x();
orientation(1) = rpy.y();
orientation(2) = rpy.z();
geo::fCVec3 T = {{origin.x(),origin.y(),origin.z()}};
wsocket.transform(T,orientation);
wbox.transform(T,orientation);
}
//Create a ROS frame out of the known corners of a tag in the weird marker coord frame used by Alvar markers (x right y forward z up)
//p0-->p1 should point in Alvar's pos X direction
//p1-->p2 should point in Alvar's pos Y direction
int makeMasterTransform (const CvPoint3D64f& p0, const CvPoint3D64f& p1,
const CvPoint3D64f& p2, const CvPoint3D64f& p3,
tf::Transform &retT)
{
const tf::Vector3 q0(p0.x, p0.y, p0.z);
const tf::Vector3 q1(p1.x, p1.y, p1.z);
const tf::Vector3 q2(p2.x, p2.y, p2.z);
const tf::Vector3 q3(p3.x, p3.y, p3.z);
// (inverse) matrix with the given properties
const tf::Vector3 v = (q1-q0).normalized();
const tf::Vector3 w = (q2-q1).normalized();
const tf::Vector3 n = v.cross(w);
tf::Matrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]);
m = m.inverse();
//Translate to quaternion
if(m.determinant() <= 0)
return -1;
//Use Eigen for this part instead, because the ROS version of bullet appears to have a bug
Eigen::Matrix3f eig_m;
for(int i=0; i<3; i++){
for(int j=0; j<3; j++){
eig_m(i,j) = m[i][j];
}
}
Eigen::Quaternion<float> eig_quat(eig_m);
// Translate back to bullet
tfScalar ex = eig_quat.x();
tfScalar ey = eig_quat.y();
tfScalar ez = eig_quat.z();
tfScalar ew = eig_quat.w();
tf::Quaternion quat(ex,ey,ez,ew);
quat = quat.normalized();
double qx = (q0.x() + q1.x() + q2.x() + q3.x()) / 4.0;
double qy = (q0.y() + q1.y() + q2.y() + q3.y()) / 4.0;
double qz = (q0.z() + q1.z() + q2.z() + q3.z()) / 4.0;
tf::Vector3 origin (qx,qy,qz);
tf::Transform tform (quat, origin); //transform from master to marker
retT = tform;
return 0;
}
geometry_msgs::Vector3 MoveBaseDoorAction::toVector(const tf::Vector3& pnt)
{
geometry_msgs::Vector3 res;
res.x = pnt.x();
res.y = pnt.y();
res.z = pnt.z();
return res;
}
/**
* Convert tf::Vector3 to string
*/
template<> std::string toString<tf::Vector3>(const tf::Vector3& p_vec)
{
std::stringstream ss;
ss << "(" << p_vec.x() << ", " << p_vec.y() << ", " << p_vec.z() << ")";
return ss.str();
}
gm::Point toPoint (const tf::Vector3& p)
{
gm::Point pt;
pt.x = p.x();
pt.y = p.y();
pt.z = p.z();
return pt;
}
inline void transformPointMatVec(const tf::Vector3 &origin, const tf::Matrix3x3 &basis, const geometry_msgs::Point32 &in, geometry_msgs::Point32 &out)
{
// Use temporary variables in case &in == &out
double x = basis[0].x() * in.x + basis[0].y() * in.y + basis[0].z() * in.z + origin.x();
double y = basis[1].x() * in.x + basis[1].y() * in.y + basis[1].z() * in.z + origin.y();
double z = basis[2].x() * in.x + basis[2].y() * in.y + basis[2].z() * in.z + origin.z();
out.x = x; out.y = y; out.z = z;
}
bool control(PID_control::controlserver::Request &req,
PID_control::controlserver::Response &ret) {
tf::vector3MsgToTF(req.target_error,sp);
tf::transformMsgToTF(req.transform,pose);
tf::vector3MsgToTF(req.velocity,vel);
if (req.reset) {
pParam=2,iParam=0,dParam=0,vParam=-2,cumul=0,lastE=0,pAlphaE=0,pBetaE=0,psp2=0,psp1=0,prevYaw=0;
ROS_INFO("Controller reset");
}
e = (pose*sp).z() - pose.getOrigin().z();
cumul=cumul+e;
pv=pParam*e;
thrust=5.335+pv+iParam*cumul+dParam*(e-lastE)+vel.z()*vParam;
lastE=e;
// Horizontal control:
vx = pose*tf::Vector3(1,0,0);
vy = pose*tf::Vector3(0,1,0);
alphaE=(vy.z()-pose.getOrigin().z());
alphaCorr=0.25*alphaE+2.1*(alphaE-pAlphaE);
betaE=(vx.z()-pose.getOrigin().z());
betaCorr=-0.25*betaE-2.1*(betaE-pBetaE);
pAlphaE=alphaE;
pBetaE=betaE;
alphaCorr=alphaCorr+sp.y()*0.005+1*(sp.y()-psp2);
betaCorr=betaCorr-sp.x()*0.005-1*(sp.x()-psp1);
psp2=sp.y();
psp1=sp.x();
pose.getBasis().getRPY(t1, t2, yaw);
rotCorr=yaw*0.1+2*(yaw-prevYaw);
prevYaw = yaw;
// Decide of the motor velocities:
a=thrust*((double)1-alphaCorr+betaCorr+rotCorr);
b=thrust*((double)1-alphaCorr-betaCorr-rotCorr);
c=thrust*((double)1+alphaCorr-betaCorr+rotCorr);
d=thrust*((double)1+alphaCorr+betaCorr-rotCorr);
ret.a=a;
ret.b=b;
ret.c=c;
ret.d=d;
}
tf::Vector3 BasePositionPid::updateControl(const tf::Vector3& commanded_pos, const tf::Vector3& actual_pos, double time_elapsed)
{
double err_x = actual_pos.x() - commanded_pos.x() ;
double err_y = actual_pos.y() - commanded_pos.y() ;
double err_w = angles::shortest_angular_distance(commanded_pos.z(), actual_pos.z()) ;
tf::Vector3 velocity_cmd ;
double odom_cmd_x = pid_x_.updatePid(err_x, time_elapsed) ; // Translation X in the odometric frame
double odom_cmd_y = pid_y_.updatePid(err_y, time_elapsed) ; // Translation Y in the odometric frame
// Rotate the translation commands so that they're in the base frame (instead of the odom frame)
velocity_cmd.setX( odom_cmd_x*cos(actual_pos.z()) + odom_cmd_y*sin(actual_pos.z())) ;
velocity_cmd.setY(-odom_cmd_x*sin(actual_pos.z()) + odom_cmd_y*cos(actual_pos.z())) ;
velocity_cmd.setZ(pid_w_.updatePid(err_w, time_elapsed)) ; // Rotation command is same is Odom and Base frames
return velocity_cmd ;
}
static carmen_vector_3D_t
tf_vector3_to_carmen_vector3(tf::Vector3 tf_vector)
{
carmen_vector_3D_t carmen_vector;
carmen_vector.x = tf_vector.x();
carmen_vector.y = tf_vector.y();
carmen_vector.z = tf_vector.z();
return carmen_vector;
}
std::vector<geometry_msgs::Point> MoveBaseDoorAction::getOrientedFootprint(const tf::Vector3 pos, double theta_cost)
{
double cos_th = cos(theta_cost);
double sin_th = sin(theta_cost);
std::vector<geometry_msgs::Point> oriented_footprint;
for(unsigned int i = 0; i < footprint_.size(); ++i){
geometry_msgs::Point new_pt;
new_pt.x = pos.x() + (footprint_[i].x * cos_th - footprint_[i].y * sin_th);
new_pt.y = pos.y() + (footprint_[i].x * sin_th + footprint_[i].y * cos_th);
oriented_footprint.push_back(new_pt);
}
return oriented_footprint;
}
void MetalDetectorCallback(const std_msgs::Float32 v)
{
static int sMarkerID=0;
if(v.data>0.95) // Mine detected
{
//ROS_INFO("Found a mine");
// Check whether a mine at the same position has already been found
for(auto it = m_vMinesPositions.begin(); it!= m_vMinesPositions.end(); ++it)
{
if(squaredDistance2D(it->x(), it->y(), m_WorldSpaceToolPosition.x(), m_WorldSpaceToolPosition.y())<0.04)
return;
}
//ROS_INFO("Publishing marker");
visualization_msgs::Marker m;
m.header.stamp = m_MsgHeaderStamp;
m.header.frame_id = "/world";
m.ns = "mine";
m.id = sMarkerID++;
m_vMinesPositions.push_back(m_WorldSpaceToolPosition);
m.type = visualization_msgs::Marker::CYLINDER;
m.action = visualization_msgs::Marker::ADD;
m.pose.position.x = m_WorldSpaceToolPosition.x();
m.pose.position.y = m_WorldSpaceToolPosition.y();
m.pose.position.z = m_WorldSpaceToolPosition.z();
m.scale.x = 0.4;
m.scale.y = 0.4;
m.scale.z = 0.4;
m.color.a = 0.5;
m.color.r = 0.0;
m.color.g = 1.0;
m.color.b = 0.0;
//m.frame_locked = true;
// Finally publish the marker
m_MineMarkerPublisher.publish(m);
}
}
void CameraTransformOptimization::calculateMarkerError(CalibrationState state,
tf::Vector3 markerPoint, float& error) {
error = 0;
for (int i = 0; i < this->measurePoints.size(); i++) {
float currentError = 0;
MeasurePoint& current = this->measurePoints[i];
tf::Transform opticalToFixed = current.opticalToFixed(state);
tf::Vector3 transformedPoint = opticalToFixed
* current.measuredPosition;
currentError += pow(markerPoint.x() - transformedPoint.x(), 2);
currentError += pow(markerPoint.y() - transformedPoint.y(), 2);
currentError += pow(markerPoint.z() - transformedPoint.z(), 2);
error += currentError;
}
}
void MTMHaptics::convert_bodyForcetoSpatialForce(geometry_msgs::WrenchStamped &body_wrench){
visualize_haptic_force(body_force_pub);
rot_quat.setX(cur_mtm_pose.orientation.x);
rot_quat.setY(cur_mtm_pose.orientation.y);
rot_quat.setZ(cur_mtm_pose.orientation.z);
rot_quat.setW(cur_mtm_pose.orientation.w);
F7wrt0.setValue(body_wrench.wrench.force.x, body_wrench.wrench.force.y, body_wrench.wrench.force.z);
rot_matrix.setRotation(rot_quat);
F0wrt7 = rot_matrix.transpose() * F7wrt0;
body_wrench.wrench.force.x = F0wrt7.x();
body_wrench.wrench.force.y = F0wrt7.y();
body_wrench.wrench.force.z = F0wrt7.z();
visualize_haptic_force(spatial_force_pub);
}
void Jumps::update(tf::Vector3& origin,tf::Quaternion& orientation){
if(bFirst){
origin_buffer.push_back(origin);
orientation_buffer.push_back(orientation);
if(origin_buffer.size() == origin_buffer.capacity()){
bFirst = false;
ROS_INFO("====== jump filter full ======");
}
}else{
origin_tmp = origin_buffer[origin_buffer.size()-1];
orientation_tmp = orientation_buffer[orientation_buffer.size()-1];
if(bDebug){
std::cout<< "=== jum debug === " << std::endl;
std::cout<< "p : " << origin.x() << "\t" << origin.y() << "\t" << origin.z() << std::endl;
std::cout<< "p_tmp: " << origin_tmp.x() << "\t" << origin_tmp.y() << "\t" << origin_tmp.z() << std::endl;
std::cout<< "p_dis: " << origin.distance(origin_tmp) << std::endl;
std::cout<< "q : " << orientation.x() << "\t" << orientation.y() << "\t" << orientation.z() << "\t" << orientation.w() << std::endl;
std::cout<< "q_tmp: " << orientation_tmp.x() << "\t" << orientation_tmp.y() << "\t" << orientation_tmp.z() << "\t" << orientation_tmp.w() << std::endl;
std::cout<< "q_dis: " << dist(orientation,orientation_tmp) << std::endl;
}
/// Position jump
if(jumped(origin,origin_tmp,origin_threashold)){
ROS_INFO("position jumped !");
origin = origin_tmp;
// exit(0);
}else{
origin_buffer.push_back(origin);
}
/// Orientation jump
if(jumped(orientation,orientation_tmp,orientation_threashold)){
ROS_INFO("orientation jumped !");
orientation = orientation_tmp;
//exit(0);
}else{
orientation_buffer.push_back(orientation);
}
}
}
float JPCT_Math::calcAngle(tf::Vector3 v1, tf::Vector3 v2) {
float dot = v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z();
float lt = v1.x() * v1.x() + v1.y() * v1.y() + v1.z() * v1.z();
float lv = v2.x() * v2.x() + v2.y() * v2.y() + v2.z() * v2.z();
dot /= sqrt(lt * lv);
if(dot < -1.0f) {
dot = -1.0f;
} else if(dot > 1.0f) {
dot = 1.0f;
}
return acos(dot);
}
tf::Matrix3x3 JPCT_Math::rotateAxis(tf::Vector3 axis, tf::Matrix3x3& original, float angle) {
if(angle != lastRot) {
lastRot = angle;
lastSin = (float)sin((double)angle);
lastCos = (float)cos((double)angle);
}
float c = lastCos;
float s = lastSin;
float t = 1.0F - c;
//axis = axis.normalize(axis);
float x = axis.x();
float y = axis.y();
float z = axis.z();
tf::Matrix3x3 mat;
mat.setIdentity();
float sy = s * y;
float sx = s * x;
float sz = s * z;
float txy = t * x * y;
float txz = t * x * z;
float tyz = t * y * z;
mat[0][0] = t * x * x + c;
mat[1][0] = txy + sz;
mat[2][0] = txz - sy;
mat[0][1] = txy - sz;
mat[1][1] = t * y * y + c;
mat[2][1] = tyz + sx;
mat[0][2] = txz + sy;
mat[1][2] = tyz - sx;
mat[2][2] = t * z * z + c;
orthonormalize(mat);
original *= mat;
return original;
}
/**
* @brief Creates a sphere RViz marker for publishing
* @param color_g The color for the marker, in range of 0 to 1, 0 is pure red, 1 is pure green
* @param sphere_size The radius of the sphere
* @param pos The position of the marker on the map
* @return A marker
*/
visualization_msgs::Marker prepareSphereMarker(double color_g, double sphere_size, tf::Vector3& pos)
{
visualization_msgs::Marker marker;
marker.header.frame_id = "/map";
marker.header.stamp = ros::Time::now();
marker.ns = "wifi_heatmap";
marker.id = marker_id++;
marker.type = visualization_msgs::Marker::SPHERE;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
marker.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
marker.pose.position.x = pos.x();
marker.pose.position.y = pos.y();
marker.pose.position.z = pos.z();
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
// Set the scale of the marker
marker.scale.x = sphere_size;
marker.scale.y = sphere_size;
marker.scale.z = sphere_size;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 1.0 - color_g;
marker.color.g = color_g;
marker.color.b = 0.0f;
marker.color.a = 1.0;
marker.lifetime = ros::Duration();
return marker;
}
void gotIntersection(const tf::Vector3 &pt)
{
sendPoint(pt.x(), pt.y(), pt.z());
}
void Robot::move(tf::Vector3 speed){
geometry_msgs::Twist msg;
msg.linear.x = speed.x();
msg.linear.y = speed.y();
speeds_publisher.publish(msg);
}
void make_markers2(tf::Vector3 point1, tf::Vector3 point2, tf::Vector3 point3, tf::Vector3 origin)
{
visualization_msgs::Marker marker1a;
marker1a.header.frame_id = world_frame_;
marker1a.header.stamp = ros::Time();
marker1a.id = 10;
marker1a.type = visualization_msgs::Marker::SPHERE;
marker1a.action = visualization_msgs::Marker::ADD;
marker1a.pose.position.x = point1.x();
marker1a.pose.position.y = point1.y();
marker1a.pose.position.z = point1.z();
marker1a.color.a = 1.0;
marker1a.color.r = 0.0;
marker1a.color.g = 1.0;
marker1a.color.b = 0.0;
marker1a.scale.x = 0.04;
marker1a.scale.y = 0.04;
marker1a.scale.z = 0.04;
markers_.markers.push_back(marker1a);
visualization_msgs::Marker marker2a;
marker2a.header.frame_id = world_frame_;
marker2a.header.stamp = ros::Time();
marker2a.id = 11;
marker2a.type = visualization_msgs::Marker::SPHERE;
marker2a.action = visualization_msgs::Marker::ADD;
marker2a.pose.position.x = point2.x();
marker2a.pose.position.y = point2.y();
marker2a.pose.position.z = point2.z();
marker2a.color.a = 1.0;
marker2a.color.r = 1.0;
marker2a.color.g = 0.0;
marker2a.color.b = 0.0;
marker2a.scale.x = 0.04;
marker2a.scale.y = 0.04;
marker2a.scale.z = 0.04;
markers_.markers.push_back(marker2a);
visualization_msgs::Marker marker3a;
marker3a.header.frame_id = world_frame_;
marker3a.header.stamp = ros::Time();
marker3a.id = 12;
marker3a.type = visualization_msgs::Marker::SPHERE;
marker3a.action = visualization_msgs::Marker::ADD;
marker3a.pose.position.x = point3.x();
marker3a.pose.position.y = point3.y();
marker3a.pose.position.z = point3.z();
marker3a.color.a = 1.0;
marker3a.color.r = 0.0;
marker3a.color.g = 0.0;
marker3a.color.b = 1.0;
marker3a.scale.x = 0.04;
marker3a.scale.y = 0.04;
marker3a.scale.z = 0.04;
markers_.markers.push_back(marker3a);
visualization_msgs::Marker marker2;
marker2.header.frame_id = world_frame_;
marker2.header.stamp = ros::Time();
marker2.id = 13;
marker2.type = visualization_msgs::Marker::SPHERE;
marker2.action = visualization_msgs::Marker::ADD;
marker2.pose.position.x = origin.x();
marker2.pose.position.y = origin.y();
marker2.pose.position.z = origin.z();
marker2.color.a = 1.0;
marker2.color.r = 0.5;
marker2.color.g = 0.0;
marker2.color.b = 0.5;
marker2.scale.x = 0.02;
marker2.scale.y = 0.02;
marker2.scale.z = 0.02;
markers_.markers.push_back(marker2);
//vis_pub.publish(markers_);
}
void leatherman::tfVector3ToEigen(const tf::Vector3 &bt, Eigen::Vector3d &e)
{
e(0) = bt.x(); e(1) = bt.y(); e(2) = bt.z();
}
void aero_path_planning::vectorToPoint(const tf::Vector3& vector, aero_path_planning::Point& point)
{
point.x = vector.x();
point.y = vector.y();
point.z = vector.z();
}