visualization_msgs::Marker SatDetector3DMonitor::getMarker(int marker_id, Eigen::Vector4f centroid, Eigen::Matrix3f covariance_matrix){
//------- Compute Principal Componets for Marker publishing
//Get the principal components and the quaternion
Eigen::Matrix3f evecs;
Eigen::Vector3f evals;
//pcl::eigen33 (covariance_matrix, evecs, evals);
Eigen::EigenSolver<Eigen::Matrix3f> es(covariance_matrix);
evecs = es.eigenvectors().real();
evals = es.eigenvalues().real();
Eigen::Matrix3f rotation;
rotation.row (0) = evecs.col (0);
rotation.row (1) = evecs.col (1);
rotation.row (2) = rotation.row (0).cross (rotation.row (1));
rotation.transposeInPlace ();
Eigen::Quaternion<float> qt (rotation);
qt.normalize ();
//Publish Marker for cluster
visualization_msgs::Marker marker;
marker.header.frame_id = base_frame_;
marker.header.stamp = ros::Time().now();
marker.ns = "Perception";
marker.action = visualization_msgs::Marker::ADD;
marker.id = marker_id;
marker.lifetime = ros::Duration(1);
//centroid position
marker.type = visualization_msgs::Marker::SPHERE;
marker.pose.position.x = centroid[0];
marker.pose.position.y = centroid[1];
marker.pose.position.z = centroid[2];
marker.pose.orientation.x = qt.x();
marker.pose.orientation.y = qt.y();
marker.pose.orientation.z = qt.z();
marker.pose.orientation.w = qt.w();
//std::cout << "e1: " << evals(0) << " e2: " << evals(1) << " e3: " << evals(2) << std::endl;
marker.scale.x = sqrt(evals(0)) * 4;
marker.scale.y = sqrt(evals(1)) * 4;
marker.scale.z = sqrt(evals(2)) * 4;
//give it some color!
marker.color.a = 0.75;
satToRGB(marker.color.r, marker.color.g, marker.color.b);
//std::cout << "marker being published" << std::endl;
return marker;
}
bool checkRange(const Eigen::Quaternion<DerivedA>& qq,
double minVal, double maxVal)
{
assert(minVal < maxVal);
if (std::isnan(qq.w()) || std::isnan(qq.x()) || std::isnan(qq.y()) || std::isnan(qq.z()))
{
std::stringstream ss;
ss << "Quaternion [" << qq.w() << ", " << qq.x() << ", " << qq.y() << ", " << qq.z() << "] contains NaN values!\n";
ROS_WARN_STREAM(ss.str());
return false;
}
if (qq.w() > maxVal || qq.w() < minVal ||
qq.x() > maxVal || qq.x() < minVal ||
qq.y() > maxVal || qq.y() < minVal ||
qq.z() > maxVal || qq.z() < minVal)
{
std::stringstream ss;
ss << "Quaternion contains terms out of range:\n"
<< " - w: [" << qq.w() << ", " << qq.x() << ", " << qq.y() << ", " << qq.z() << "]\n"
<< " - minVal: " << minVal << "\n"
<< " - maxVel: " << maxVal;
ROS_WARN_STREAM(ss.str());
}
return true;
}
void computeEr(const Eigen::Quaternion<double>& q, Eigen::MatrixXd& Er)
{
Er.resize(4, 3);
Er << -q.x(), -q.y(), -q.z(),
q.w(), q.z(), -q.y(),
-q.z(), q.w(), q.x(),
q.y(), -q.x(), q.w();
Er = 0.5 * Er;
}
SE3<> IMU2camWorldfromQuat(Eigen::Quaternion<double> atti){// use ini attitude info from EKF node to ini ptam pose
Vector<4> attivec = makeVector(atti.w(), atti.x(), atti.y(), atti.z());//Rw1i
Matrix<3> iniOrientationEKF;
iniOrientationEKF = tag::quaternionToMatrix(attivec);
Matrix<3> roll = TooN::Data(1.0, 0, 0,//Rww1, because the roll and pitch angles are in
0, -1, 0, // a world frame which pointing downward.
0, 0, -1);
SE3<> camWorld = SE3<>();
Matrix<3> rotation;
if (tracker_->attitude_get)
rotation = iniOrientationEKF; //
else
rotation = roll * iniOrientationEKF;//Rwi = Rww1*Rw1i
camWorld.get_rotation() = rotation*se3IMUfromcam.get_rotation().get_matrix();//Rwc = (Rwi * Ric)
Vector<3> twr = makeVector(0.0, 0.0, 0.198);// twc = twr + Rwr * trc
Vector<3> twc = twr + rotation * se3IMUfromcam.get_translation();
camWorld.get_translation()[0] = 0.0;//twc[0]; //twc
camWorld.get_translation()[1] = 0.0;//twc[1];
camWorld.get_translation()[2] = twc[2];
camWorld = camWorld.inverse();//Tcw
cout<< "TCW INITIALIZED. TWC: " << twc[0]<< ", " << twc[1]<< ", " << twc[2]<<endl;
// cout<< camWorld.get_rotation().get_matrix()(2,2)<<endl;
return camWorld;
}
void HLManager::start()
{
ros::Rate rate(10);
while (nh.ok())
{
if (fixValidated)
{
tf::Transform transform;
transform.setOrigin(tf::Vector3(originLon, originLat, 0));
transform.setRotation(tf::createQuaternionFromRPY(0,0,0));
broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/worldLatLon", "/world"));
transform.setOrigin(tf::Vector3(0, 0, 0));
Eigen::Quaternion<float> q;
labust::tools::quaternionFromEulerZYX(M_PI,0,M_PI/2,q);
transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "local"));
}
this->safetyTest();
this->step();
hlMessagePub.publish(hlDiagnostics);
rate.sleep();
ros::spinOnce();
}
}
void HLManager::onVTTwist(const geometry_msgs::TwistStamped::ConstPtr& twist)
{
if (mode == circle || mode == vtManual)
{
//\todo Generalize this 0.1 with Ts.
s +=twist->twist.linear.x*0.1;
//Circle
if (s>=2*circleRadius*M_PI) s=s-2*circleRadius*M_PI;
else if (s<0) s=2*circleRadius*M_PI-s;
double xRabbit = point.point.x + circleRadius*cos(s/circleRadius);
double yRabbit = point.point.y + circleRadius*sin(s/circleRadius);
double gammaRabbit=labust::math::wrapRad(s/circleRadius)+M_PI/2;
tf::Transform transform;
transform.setOrigin(tf::Vector3(xRabbit, yRabbit, 0));
Eigen::Quaternion<float> q;
labust::tools::quaternionFromEulerZYX(0,0,gammaRabbit, q);
transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "local", "serret_frenet_frame"));
auv_msgs::NavStsPtr msg(new auv_msgs::NavSts());
msg->position.north = xRabbit;
msg->position.east = yRabbit;
msg->body_velocity.x = twist->twist.linear.x;
msg->orientation.yaw = gammaRabbit;
sfPub.publish(msg);
}
}
bool CloudMatcher::match(modular_cloud_matcher::MatchClouds::Request& req, modular_cloud_matcher::MatchClouds::Response& res)
{
// get and check reference
size_t referenceGoodCount;
DP referenceCloud(rosMsgToPointMatcherCloud(req.reference, referenceGoodCount));
const unsigned referencePointCount(req.reference.width * req.reference.height);
const double referenceGoodRatio(double(referenceGoodCount) / double(referencePointCount));
if (referenceGoodCount == 0)
{
ROS_ERROR("I found no good points in the reference cloud");
return false;
}
if (referenceGoodRatio < 0.5)
{
ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - referenceGoodRatio*100.0 << "% of the cloud (received " << referenceGoodCount << ")");
}
// get and check reading
size_t readingGoodCount;
DP readingCloud(rosMsgToPointMatcherCloud(req.readings, readingGoodCount));
const unsigned readingPointCount(req.readings.width * req.readings.height);
const double readingGoodRatio(double(readingGoodCount) / double(readingPointCount));
if (readingGoodCount == 0)
{
ROS_ERROR("I found no good points in the reading cloud");
return false;
}
if (readingGoodRatio < 0.5)
{
ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - readingGoodRatio*100.0 << "% of the cloud (received " << readingGoodCount << ")");
}
// call icp
TP transform;
try
{
transform = icp(readingCloud, referenceCloud);
ROS_INFO_STREAM("match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl);
}
catch (PM::ConvergenceError error)
{
ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
return false;
}
// fill return value
res.transform.translation.x = transform.coeff(0,3);
res.transform.translation.y = transform.coeff(1,3);
res.transform.translation.z = transform.coeff(2,3);
const Eigen::Quaternion<Scalar> quat(Matrix3(transform.block(0,0,3,3)));
res.transform.rotation.x = quat.x();
res.transform.rotation.y = quat.y();
res.transform.rotation.z = quat.z();
res.transform.rotation.w = quat.w();
return true;
}
void quaternion2vector(const Eigen::Quaternion<T_qt> &quat_In, std::vector<T_vec> &quat_Vec )
{
quat_Vec.resize(4,T_vec(0.0));
quat_Vec[0] = T_vec( quat_In.w() );
quat_Vec[1] = T_vec( quat_In.x() );
quat_Vec[2] = T_vec( quat_In.y() );
quat_Vec[3] = T_vec( quat_In.z() );
};
void YouBot::updateWheels(const ros::TimerEvent& e)
{
try
{
if(q_base_.rows() != robot_->getMacroManipulatorDOF()) return;
if(tau_base_.rows() != robot_->getMacroManipulatorDOF()) return;
if(!gazebo_interface_wheel_->subscribed()) return;
Eigen::VectorXd q = gazebo_interface_wheel_->getJointStates();
robot_->updateWheel(q);
Eigen::Vector3d base_pos;
base_pos << q_base_[0], q_base_[1], 0.0;
Eigen::Quaternion<double> base_ori;
base_ori.x() = 0.0;
base_ori.y() = 0.0;
base_ori.z() = sin(0.5 * q_base_[2]);
base_ori.w() = cos(0.5 * q_base_[2]);
robot_->updateBase(base_pos, base_ori);
Eigen::VectorXd v_base;
controller_->computeBaseVelocityFromTorque(tau_base_, v_base, 3);
Eigen::VectorXd v_wheel;
controller_->computeWheelVelocityFromBaseVelocity(v_base, v_wheel);
Eigen::VectorXd q_wheel_d = robot_->getMobility()->update_rate * v_wheel;
std::vector<Eigen::Quaternion<double> > quat_d;
for(unsigned int i = 0; i < q_wheel_d.rows(); ++i)
{
double rad = q_wheel_d[i];
Eigen::Quaternion<double> quat;
quat.x() = 0.0;
quat.y() = sin(0.5 * rad);
quat.z() = 0.0;
quat.w() = cos(0.5 * rad);
quat_d.push_back(quat);
}
gazebo_interface_wheel_->rotateLink(quat_d);
}
catch(ahl_robot::Exception& e)
{
ROS_ERROR_STREAM(e.what());
}
catch(ahl_ctrl::Exception& e)
{
ROS_ERROR_STREAM(e.what());
}
}
Eigen::Matrix3f
motion_controller::qToRotation(Eigen::Quaternion<float> q)
{
Eigen::Matrix3f temp;
temp << 1-2*(q.y()*q.y()+q.z()*q.z()), 2*(q.x()*q.y()-q.w()*q.z()) , 2*(q.w()*q.y()+q.x()*q.z()) ,
2*(q.x()*q.y()+q.w()*q.z()) , 1-2*(q.x()*q.x()+q.z()*q.z()), 2*(q.y()*q.z()-q.w()*q.x()) ,
2*(q.x()*q.z()-q.w()*q.y()) , 2*(q.y()*q.z()+q.w()*q.x()) , 1-2*(q.x()*q.x()+q.y()*q.y());
return temp;
}
void rpyToQuaternion(const Eigen::Vector3d& rpy, Eigen::Quaternion<double>& q)
{
double a = rpy.coeff(0);
double b = rpy.coeff(1);
double g = rpy.coeff(2);
double sin_b_half = sin(0.5 * b);
double cos_b_half = cos(0.5 * b);
double diff_a_g_half = 0.5 * (a - g);
double sum_a_g_half = 0.5 * (a + g);
q.x() = sin_b_half * cos(diff_a_g_half);
q.y() = sin_b_half * sin(diff_a_g_half);
q.z() = cos_b_half * sin(sum_a_g_half);
q.w() = cos_b_half * cos(sum_a_g_half);
}
base::Vector6d AuvMotion::gravity_buoyancy(const Eigen::Quaternion<double> q)
{
base::Vector6d gravitybuoyancy;
double mass;
control->nodes->getNodeMass(vehicle_id ,&mass);
double buoyancy = _buoyancy_force;
base::Vector3d pos = control->nodes->getPosition(vehicle_id);
// In Quaternion form
float e1 = q.x();
float e2 = q.y();
float e3 = q.z();
float e4 = q.w();
float xg = 0.0;
float yg = 0.0;
float zg = 0.0;
float xb = centerOfBuoyancy[0];
float yb = centerOfBuoyancy[1];
float zb = centerOfBuoyancy[2];;
gravitybuoyancy(0) = 0.0;
gravitybuoyancy(1) = 0.0;
if(pos[2] + centerOfBuoyancy[2] < 0.0){ //The vehicle is completly under water
gravitybuoyancy(2) = buoyancy - mass;
}else if(pos[2] < 0.0 && centerOfBuoyancy[2] != 0){ //The vehicle is partwise underwater -> apply buoyancy partwise
gravitybuoyancy(2) = (buoyancy * (-pos[2]/ centerOfBuoyancy[2] ) ) - mass;
}else{ //The vehicle is over the surface -> no buoyancy
gravitybuoyancy(2) = -mass;
}
//Angular buoancy forces
gravitybuoyancy(3) = ((-(e4 * e4)+(e1 * e1)+(e2 * e2)-(e3 * e3))*((yg*mass)-(yb*buoyancy)))+(2*((e4*e1)+(e2*e3))*((zg*mass)-(zb*buoyancy)));
gravitybuoyancy(4) =-((-(e4 * e4)+(e1 * e1)+(e2 * e2)-(e3 * e3))*((xg*mass)-(xb*buoyancy)))+(2*((e4*e2)-(e1*e3))*((zg*mass)-(zb*buoyancy)));
gravitybuoyancy(5) =-(2*((e4*e1)+(e2*e3))*((xg*mass)-(xb*buoyancy)))-(2*((e4*e2)-(e1*e3))*((yg*mass)-(yb*buoyancy)));
//Convert angular forces to world frame
gravitybuoyancy.block<3,1>(3,0) = q * gravitybuoyancy.block<3,1>(3,0);
return gravitybuoyancy;
}
void publishNode(unsigned int index, Eigen::Matrix<double,4,1> trans,
Eigen::Quaternion<double> fq,
const frame_common::CamParams &cp,
bool isFixed, sba::CameraNode &msg)
{
msg.index = index;
msg.transform.translation.x = trans.x();
msg.transform.translation.y = trans.y();
msg.transform.translation.z = trans.z();
msg.transform.rotation.x = fq.x();
msg.transform.rotation.y = fq.y();
msg.transform.rotation.z = fq.z();
msg.transform.rotation.w = fq.w();
msg.fx = cp.fx;
msg.fy = cp.fy;
msg.cx = cp.cx;
msg.cy = cp.cy;
msg.baseline = cp.tx;
msg.fixed = isFixed;
}
bool operator()(T const* const* parameters, T* residuals) const {
// Map spline
UniformSpline<T> spline(spline_dt_, spline_offset_);
for (size_t i=0; i < num_knots_; ++i) {
spline.add_knot((T*) ¶meters[i][0]);
}
Eigen::Matrix<T, 3, 1> landmark(T(5.0), T(1.0), T(3.0));
Eigen::Matrix<T, 3, 1> expected(T(0.0), T(0.0), T(1.0));
// Distance to center
Sophus::SE3Group<T> P;
Eigen::Matrix<T, 4, 4> dP, d2P;
for (size_t i=0; i < eval_times_.size(); ++i) {
spline.evaluate(T(eval_times_[i]), P, dP, d2P);
Eigen::Matrix<T, 3, 1> X_spline = P.inverse() * landmark;
Eigen::Matrix<T, 3, 1> diff = X_spline - expected;
residuals[3*i + 0] = diff(0);
residuals[3*i + 1] = diff(1);
residuals[3*i + 2] = diff(2);
}
const size_t poff = 3 * eval_times_.size();
// Constant angular difference
for (size_t i=1; i < eval_times_.size(); ++i) {
SE3Group<T> P0, P1, delta;
spline.evaluate(T(eval_times_[i-1]), P0, dP, d2P);
spline.evaluate(T(eval_times_[i]), P1, dP, d2P);
delta = P0.inverse() * P1;
Eigen::Quaternion<T> q = delta.unit_quaternion();
residuals[poff + 4*(i - 1) + 0] = T(5.0) * (q.w() - T(cos(0.5 * expected_angular_difference_)));
residuals[poff + 4*(i - 1) + 1] = T(5.0) * (q.x() - T(sin(0.5 * expected_angular_difference_)));
residuals[poff + 4*(i - 1) + 2] = T(5.0) * q.y(); //- 0;
residuals[poff + 4*(i - 1) + 3] = T(5.0) * q.z(); //- 0;
}
return true;
}
bool base_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf)
{
assert(q_urdf.size()==7);
assert(q_sot.size()==6);
// ********* RPY to Quat *********
const double r = q_sot[3];
const double p = q_sot[4];
const double y = q_sot[5];
const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;
q_urdf[0 ]=q_sot[0 ]; //BASE
q_urdf[1 ]=q_sot[1 ];
q_urdf[2 ]=q_sot[2 ];
q_urdf[3 ]=quat.x();
q_urdf[4 ]=quat.y();
q_urdf[5 ]=quat.z();
q_urdf[6 ]=quat.w();
return true;
}
int main(int argc, char **argv)
{
// initialize the node and various publishers
ros::init(argc, argv, "fovis_odom");
ros::NodeHandle n;
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
tf::TransformBroadcaster odom_broadcaster;
// initialize the device
fovis_example::DataCapture* cap = new fovis_example::DataCapture();
if(!cap->initialize()) {
fprintf(stderr, "Unable to initialize OpenNI sensor\n");
return 1;
}
if(!cap->startDataCapture()) {
fprintf(stderr, "Unable to start data capture\n");
return 1;
}
// get the RGB camera parameters of our device
fovis::Rectification rect(cap->getRgbParameters());
fovis::VisualOdometryOptions options =
fovis::VisualOdometry::getDefaultOptions();
// If we wanted to play around with the different VO parameters, we could set
// them here in the "options" variable.
// setup the visual odometry
fovis::VisualOdometry* odom = new fovis::VisualOdometry(&rect, options);
// exit cleanly on CTL-C
struct sigaction new_action;
new_action.sa_sigaction = sig_action;
sigemptyset(&new_action.sa_mask);
new_action.sa_flags = 0;
sigaction(SIGINT, &new_action, NULL);
sigaction(SIGTERM, &new_action, NULL);
sigaction(SIGHUP, &new_action, NULL);
while(!shutdown_flag) {
if(!cap->captureOne()) {
fprintf(stderr, "Capture failed\n");
break;
}
odom->processFrame(cap->getGrayImage(), cap->getDepthImage());
// get the integrated pose estimate.
Eigen::Isometry3d cam_to_local = odom->getPose();
Eigen::Quaternion<double> quat = Eigen::Quaternion<double>(cam_to_local.rotation());
double w = quat.w();
double qx = quat.z();
double qy = -quat.x();
double qz = -quat.y();
Eigen::Vector3d xyz = cam_to_local.translation();
double x = xyz(2);
double y = -xyz(0);
double z = -xyz(1);
tf::Quaternion tf_odom_quat = tf::Quaternion(qx,qy,qz,w);
geometry_msgs::Quaternion gm_odom_quat;
tf::quaternionTFToMsg(tf_odom_quat, gm_odom_quat);
//first, we'll publish the transform over tf
std::string odomName = "odom";
std::string baseName = "base_link";
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = ros::Time::now();;
odom_trans.header.frame_id = odomName;
odom_trans.child_frame_id = baseName;
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = z;
odom_trans.transform.rotation = gm_odom_quat;
//send the transform
odom_broadcaster.sendTransform(odom_trans);
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = ros::Time::now();
odom_msg.header.frame_id = odomName;
//set the position
odom_msg.pose.pose.position.x = x;
odom_msg.pose.pose.position.y = y;
odom_msg.pose.pose.position.z = z;
odom_msg.pose.pose.orientation = gm_odom_quat;
//set the velocity
odom_msg.child_frame_id = baseName;
//odom_msg.twist.twist.linear.x = vx;
//odom_msg.twist.twist.linear.y = vy;
//odom_msg.twist.twist.angular.z = vth;
//publish the message
odom_pub.publish(odom_msg);
// get the motion estimate for this frame to the previous frame.
Eigen::Isometry3d motion_estimate = odom->getMotionEstimate();
// display the motion estimate. These values are all given in the RGB
// camera frame, where +Z is forward, +X points right, +Y points down, and
// the origin is located at the focal point of the RGB camera.
std::cout << isometryToString(cam_to_local) << " " <<
isometryToString(motion_estimate) << "\n";
}
printf("Shutting down\n");
cap->stopDataCapture();
delete odom;
delete cap;
return 0;
}
void
motion_controller::running(void)
{
cycle_start = ros::Time::now();
// receive path and when received block path receiver until mission completed
if (m_path && !block_path_receiver)
{
nav_msgs::Path temp_path = *m_path;
std::vector<geometry_msgs::PoseStamped> temp_path_vector;
extracted_path.clear();
for( std::vector<geometry_msgs::PoseStamped>::iterator itr = temp_path.poses.begin() ; itr != temp_path.poses.end(); ++itr)
{
temp_path_vector.push_back(*itr);
}
if(!temp_path_vector.empty())
{
ROS_INFO("Path Received!");
}
p_count++;
while(!temp_path_vector.empty())
{
extracted_path.push_back(temp_path_vector.back());
temp_path_vector.pop_back();
}
block_path_receiver = true;
}
// switch status according to the command
switch (m_move)
{
// move forward
case FORWARD:
{
// v is linear speed and gain is angular velocity
v = sqrt(dist)*v_max/(1 + gamma*k*k)*10000/PI;
gain = 0.3265*v*k;
if (fabs(gain) > 600)
{
gain = boost::math::sign(gain) * 600;
}
if(fabs(v)>2000)
{
v = boost::math::sign(v)*2000;
}
// set motor rotation velocity
locomotor->set_vel(floor(v)+floor(gain),-floor(v)+floor(gain));
// when the euler distance is less than 100mm, achieved waypoint
if (distance2d(m_state, m_cmd) < 0.09 && indicator(m_state[2],m_cmd[2],0.5))
{
m_move = IDLE;
}
break;
}
case BACKWARD:
{
v = sqrt(dist) * 0.75/(1+0.2*k_back*k_back) *10000/3.1415926;
gain = 0.3265*v*k_back;
if (fabs(gain) > 600)
{
gain = boost::math::sign(gain) * 600;
}
if(fabs(v)>2000)
{
v = boost::math::sign(v)*2000;
}
locomotor->set_vel(-floor(v)+floor(gain),floor(v)+floor(gain));
if (distance2d(m_state, m_cmd) < 0.09 && indicator(m_state[2],m_cmd[2],0.5))
{
m_move = IDLE;
}
break;
}
case LIFTFORK:
{
// TODO: Lift height should be determined, fork stroke should be determined
printf("Lift and fork!\n");
locomotor->set_vel(0, 0);
sleep(3);
// locomotor->shut_down();
lift_motor->power_on();
if ((fork_count%2) == 1)
// change -243720 to be -223720, wants to liftfork a little bit lower
lift_motor->set_pos(-223720);
else
lift_motor->set_pos(-293720);
sleep(10);
printf("Start Test\n");
// pose correction code inserted here first make sure tag is attached vertically, second camera has no pitch angle relative to the vehicle
if ((fork_count%2) == 1)
{
printf("Start Correction!\n");
int count_detect = 0;
while(ros::ok())
{
if(abs_pose1)
{
Eigen::Quaternion<float> quat;
quat.w() = abs_pose1->pose.pose.orientation.w;
quat.x() = abs_pose1->pose.pose.orientation.x;
quat.y() = abs_pose1->pose.pose.orientation.y;
quat.z() = abs_pose1->pose.pose.orientation.z;
Eigen::Matrix3f Rotation;
Rotation = qToRotation(quat);
Eigen::Matrix3f FixTF;
FixTF << 1, 0, 0,
0, 0, -1,
0, 1, 0;
Rotation = FixTF * Rotation.inverse();
float yaw_error;
yaw_error = getYawFromMat(Rotation);
gain = -3265*(k_angle*yaw_error)/3.1415926;
if(fabs(gain)>150)
{
gain = boost::math::sign(gain) * 150;
}
locomotor->set_vel(floor(gain), floor(gain));
if (fabs(yaw_error*180/3.1415926) < 0.5)
{
locomotor->set_vel(0,0);
printf("Yaw %f corrected!\n", yaw_error*180/3.1415926);
break;
}
}
else
{
locomotor->set_vel(0, 0);
usleep(10000);
count_detect++;
}
ros::spinOnce();
if (count_detect>1000)
{
ROS_WARN("No Tag detected when stoped");
ros::shutdown();
exit(1);
}
}
count_detect = 0;
while(ros::ok())
{
if(abs_pose1)
{
Eigen::Quaternion<float> quat;
quat.w() = abs_pose1->pose.pose.orientation.w;
quat.x() = abs_pose1->pose.pose.orientation.x;
quat.y() = abs_pose1->pose.pose.orientation.y;
quat.z() = abs_pose1->pose.pose.orientation.z;
Eigen::Matrix3f Rotation;
Eigen::Vector3f translation;
translation << abs_pose1->pose.pose.position.x,
abs_pose1->pose.pose.position.y,
abs_pose1->pose.pose.position.z;
Rotation = qToRotation(quat);
translation = translation;
float x_error;
x_error = translation[0];
v = 0.25*(x_error-0.003)*10000/3.1415926;
// printf("x is %f\n", x_error);
// printf("v is %f\n\n", floor(v));
if(fabs(v)>150)
{
v = boost::math::sign(v) * 150;
}
locomotor->set_vel(floor(v), -floor(v));
if (fabs(x_error-0.003) < 0.003)
{
locomotor->set_vel(0, 0);
printf("x %f corrected!\n", (x_error-0.006));
break;
}
}
else
{
locomotor->set_vel(0, 0);
usleep(10000);
count_detect++;
}
ros::spinOnce();
if (count_detect>1000)
{
ROS_WARN("No Tag detected when stoped");
ros::shutdown();
exit(1);
}
}
}
if ((fork_count%2) == 0)
{
int count_detect = 0;
while(ros::ok())
{
if (abs_pose)
{
Eigen::Quaternion<float> quat;
quat.w() = abs_pose->pose.pose.orientation.w;
quat.x() = abs_pose->pose.pose.orientation.x;
quat.y() = abs_pose->pose.pose.orientation.y;
quat.z() = abs_pose->pose.pose.orientation.z;
Eigen::Matrix3f Rotation;
Rotation = qToRotation(quat);
Eigen::Matrix3f fixTF;
fixTF << 1, 0, 0,
0, -1, 0,
0, 0, -1;
Rotation = Rotation.inverse()*fixTF;
m_state[2] = getYawFromMat(Rotation);
gain = 3265*(k_angle*angle(m_cmd[2],m_state[2]))/PI;
if (fabs(gain) > 100)
{
gain = boost::math::sign(gain) * 100;
}
if (fabs(gain) >100)
{
ros::shutdown();
exit(1);
}
locomotor->set_vel(floor(gain),floor(gain));
if (indicator(m_cmd[2], m_state[2], 0.008))
{
locomotor->set_vel(0, 0);
printf("Corrected!\n");
break;
}
}
else
{
locomotor->set_vel(0, 0);
usleep(10000);
count_detect++;
}
ros::spinOnce();
if (count_detect>1000)
{
locomotor->set_vel(0, 0);
ROS_WARN("No Tag detected when stoped");
//ros::shutdown();
//exit(1);
}
}
}
// if we carry out hand hold barcode test, after correction, stop
// ros::shutdown();
// exit(1);
locomotor->shut_down();
sleep(0.5);
// comment following two lines can set make telefork not strech out
telefork_motor->set_pos( boost::math::sign(lift_param)*(-386000) );
sleep(15);
if ((fork_count%2) == 1)
lift_motor->set_pos(-293720);
else
lift_motor->set_pos(-223720);
sleep(3);
telefork_motor->set_pos(0);
sleep(15);
// ros::shutdown();
// exit(1);
lift_motor->shut_down();
locomotor->power_on();
sleep(0.5);
m_move = IDLE;
break;
}
case TURN:
{
v = cos(alpha) * dist* k_dist *10000/PI;
if(fabs(v)>300)
{
v = boost::math::sign(v)*300;
}
gain = 3265*(k_angle*angle(m_cmd[2],m_state[2]))/PI;
if (fabs(gain) > 400)
{
gain = boost::math::sign(gain) * 400;
}
locomotor->set_vel(floor(v)+floor(gain),-floor(v)+floor(gain));
if (indicator(m_state[2],m_cmd[2],0.01))
{
m_move = IDLE;
}
break;
}
case IDLE:
{
locomotor->set_vel(0,0);
if (extracted_path.size()!=0)
{
geometry_msgs::PoseStamped temp_pose = extracted_path.back();
float yaw_ = 2*atan2(temp_pose.pose.orientation.z,temp_pose.pose.orientation.w);
m_cmd << temp_pose.pose.position.x * m_resolution,
temp_pose.pose.position.y * m_resolution,
angle(yaw_,0);
printf("Next Commanded Pose is (%f, %f, %f)\n", m_cmd[0], m_cmd[1], m_cmd[2]);
if ( (fabs(m_cmd[0] - m_state[0])>0.5) && (fabs(m_cmd[1] - m_state[1])>0.5) )
{
printf("Invalid commanded position received!\n");
locomotor->shut_down();
ros::shutdown();
exit(1);
}
if ( (fabs(m_cmd[0] - m_state[0])>0.5) || (fabs(m_cmd[1] - m_state[1])>0.5) )
{
if (fabs(angle(m_cmd[2],m_state[2]))>0.5)
{
printf("Invalid commanded pose orientation received!\n");
locomotor->shut_down();
ros::shutdown();
exit(1);
}
if (fabs(m_cmd[0] - m_state[0])>0.5)
{
if (cos(m_state[2]) * (m_cmd[0] - m_state[0]) > 0)
m_move = FORWARD;
else
m_move = BACKWARD;
}
else
{
if (sin(m_state[2]) * (m_cmd[1] - m_state[1]) > 0)
m_move = FORWARD;
else
m_move = BACKWARD;
}
if (m_move == FORWARD)
printf("Move Forward!\n");
else
printf("Move Backward!\n");
}
else if (fabs(m_cmd[2] - m_state[2])>0.5)
{
m_move = TURN;
printf("Turn Around!\n");
}
else if (temp_pose.pose.position.z!=0)
{
m_move = LIFTFORK;
fork_count++;
lift_param = temp_pose.pose.position.z;
}
else
m_move = IDLE;
extracted_path.pop_back();
}
else
{
if (p_count == 2)
{
lift_motor->power_on();
lift_motor->set_pos(0);
sleep(12);
lift_motor->shut_down();
sleep(0.5);
}
block_path_receiver = false;
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = "world";
pose_msg.header.stamp = ros::Time::now();
pose_msg.pose.position.x = m_state[0];
pose_msg.pose.position.y = m_state[1];
if (block_path_receiver)
pose_msg.pose.position.z = 1;
else
pose_msg.pose.position.z = 0;
pose_msg.pose.orientation.w = cos(m_state[2]/2);
pose_msg.pose.orientation.x = 0;
pose_msg.pose.orientation.y = 0;
pose_msg.pose.orientation.z = sin(m_state[2]/2);
m_posePub.publish(pose_msg);
printf("IDLE!\n");
sleep(1);
m_move = IDLE;
}
break;
}
}
vel.first = (vel.first + locomotor->get_vel().first)/2;
vel.second = (vel.second + locomotor->get_vel().second)/2;
cycle_period = (ros::Time::now() - cycle_start).toSec();
m_state[0] = m_state[0] + (-vel.second+vel.first)/2 * cos(m_state[2]) * 0.018849555 * cycle_period/60;
m_state[1] = m_state[1] + (-vel.second+vel.first)/2 * sin(m_state[2]) * 0.018849555 * cycle_period/60;
m_state[2] = m_state[2] + (vel.first + vel.second)/0.653 * 0.018849555 * cycle_period/60;
if (abs_pose)
{
int id;
id = abs_pose->id - 3;
Eigen::Quaternion<float> quat;
quat.w() = abs_pose->pose.pose.orientation.w;
quat.x() = abs_pose->pose.pose.orientation.x;
quat.y() = abs_pose->pose.pose.orientation.y;
quat.z() = abs_pose->pose.pose.orientation.z;
Eigen::Matrix3f Rotation;
Eigen::Vector3f translation;
translation << abs_pose->pose.pose.position.x,
abs_pose->pose.pose.position.y,
abs_pose->pose.pose.position.z;
Rotation = qToRotation(quat);
translation = -Rotation.inverse()*translation;
Eigen::Matrix3f fixTF;
fixTF << 1, 0, 0,
0, -1, 0,
0, 0, -1;
Rotation = Rotation.inverse()*fixTF;
m_state[0] = translation[0] + m_resolution * (id%10);
m_state[1] = translation[1] + m_resolution * floor(id/10.0);
m_state[2] = getYawFromMat(Rotation) + 0.04;
}
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = "world";
pose_msg.header.stamp = ros::Time::now();
pose_msg.pose.position.x = m_state[0];
pose_msg.pose.position.y = m_state[1];
if (block_path_receiver)
pose_msg.pose.position.z = 1;
else
pose_msg.pose.position.z = 0;
pose_msg.pose.orientation.w = cos(m_state[2]/2);
pose_msg.pose.orientation.x = 0;
pose_msg.pose.orientation.y = 0;
pose_msg.pose.orientation.z = sin(m_state[2]/2);
m_posePub.publish(pose_msg);
delta_y = m_cmd[1]-m_state[1];
if(fabs(m_cmd[1]-m_state[1]) > 1.6)
{
delta_y = boost::math::sign(m_cmd[1]-m_state[1]) * 1.6;
}
delta_x = m_cmd[0]-m_state[0];
if(fabs(m_cmd[0]-m_state[0]) > 1.6)
{
delta_x = boost::math::sign(m_cmd[0]-m_state[0]) * 1.6;
}
beta = angle(m_cmd[2], atan2(delta_y, delta_x));
alpha = angle(m_state[2], atan2(delta_y, delta_x));
beta1 = angle(m_cmd[2]+PI, atan2(delta_y, delta_x));
alpha1 = angle(m_state[2]+3.1415926, atan2(delta_y, delta_x));
dist = sqrt(delta_x*delta_x + delta_y* delta_y);
k = -1/dist * (k2*(alpha-atan(-k1*beta)) + sin(alpha)*(1 + k1/(1+(k1*beta) * (k1*beta))));
k_back = -1/dist * (k2*(alpha1-atan2(-k1*beta1,1)) + sin(alpha1)*(1 + k1/(1+(k1*beta1) * (k1*beta1))));
}
void SurfelMapPublisher::publishSurfelMarkers(const boost::shared_ptr<MapType>& map)
{
if (m_markerPublisher.getNumSubscribers() == 0)
return;
unsigned int markerId = 0;
std::vector<float> cellSizes;
typename MapType::AlignedCellVectorVector occupiedCells;
std::vector<std::vector<pcl::PointXYZ>> occupiedCellsCenters;
int levels = map->getLevels();
for (unsigned int i = 0; i < m_lastSurfelMarkerCount; ++i)
{
for (unsigned int l = 0; l < levels; l++)
{
visualization_msgs::Marker marker;
marker.header.frame_id = map->getFrameId();
marker.header.stamp = map->getLastUpdateTimestamp();
marker.ns = boost::lexical_cast<std::string>(l);
marker.id = i;
marker.type = marker.SPHERE;
marker.action = marker.DELETE;
m_markerPublisher.publish(marker);
}
}
for (unsigned int l = 0; l < levels; l++)
{
cellSizes.push_back(map->getCellSize(l));
typename MapType::AlignedCellVector occupiedCellsTemp;
std::vector<pcl::PointXYZ> occupiedCellsCentersTemp;
map->getOccupiedCells(occupiedCellsTemp, l);
map->getOccupiedCells(occupiedCellsCentersTemp, l);
occupiedCells.push_back(occupiedCellsTemp);
occupiedCellsCenters.push_back(occupiedCellsCentersTemp);
}
for (unsigned int l = 0; l < cellSizes.size(); l++)
{
float cellSize = cellSizes[l];
for (size_t i = 0; i < occupiedCells[l].size(); i++)
{
const mrs_laser_maps::Surfel& surfel = occupiedCells[l][i].surfel_;
// if (surfel.num_points_ < 15 )
// continue;
//
// if ( surfel.unevaluated_ ) {
// ROS_ERROR("not unevaluated surfel");
// continue;
// }
Eigen::Matrix<double, 3, 3> cov = surfel.cov_.block(0, 0, 3, 3);
Eigen::EigenSolver<Eigen::Matrix3d> solver(cov);
Eigen::Matrix<double, 3, 3> eigenvectors = solver.eigenvectors().real();
// double eigenvalues[3];
Eigen::Matrix<double, 3, 1> eigenvalues = solver.eigenvalues().real();
// for(int j = 0; j < 3; ++j) {
// Eigen::Matrix<double, 3, 1> mult = cov * eigenvectors.col(j);
// eigenvalues[j] = mult(0,0) / eigenvectors.col(j)(0);
// }
if (eigenvectors.determinant() < 0)
{
eigenvectors.col(0)(0) = -eigenvectors.col(0)(0);
eigenvectors.col(0)(1) = -eigenvectors.col(0)(1);
eigenvectors.col(0)(2) = -eigenvectors.col(0)(2);
}
Eigen::Quaternion<double> q = Eigen::Quaternion<double>(eigenvectors);
visualization_msgs::Marker marker;
marker.header.frame_id = map->getFrameId();
marker.header.stamp = map->getLastUpdateTimestamp();
marker.ns = boost::lexical_cast<std::string>(l);
marker.id = markerId++;
marker.type = marker.SPHERE;
marker.action = marker.ADD;
marker.pose.position.x = occupiedCellsCenters[l][i].x - cellSize / 2 + surfel.mean_(0);
marker.pose.position.y = occupiedCellsCenters[l][i].y - cellSize / 2 + surfel.mean_(1);
marker.pose.position.z = occupiedCellsCenters[l][i].z - cellSize / 2 + surfel.mean_(2);
marker.pose.orientation.w = q.w();
marker.pose.orientation.x = q.x();
marker.pose.orientation.y = q.y();
marker.pose.orientation.z = q.z();
marker.scale.x = std::max(sqrt(eigenvalues[0]) * 3, 0.01);
marker.scale.y = std::max(sqrt(eigenvalues[1]) * 3, 0.01);
marker.scale.z = std::max(sqrt(eigenvalues[2]) * 3, 0.01);
marker.color.a = 1.0;
double dot = surfel.normal_.dot(Eigen::Vector3d(0., 0., 1.));
if (surfel.normal_.norm() > 1e-10)
dot /= surfel.normal_.norm();
double angle = acos(fabs(dot));
double angle_normalized = 2. * angle / M_PI;
marker.color.r = ColorMapJet::red(angle_normalized); // fabs(surfel.normal_(0));
marker.color.g = ColorMapJet::green(angle_normalized);
marker.color.b = ColorMapJet::blue(angle_normalized);
m_markerPublisher.publish(marker);
}
}
m_lastSurfelMarkerCount = markerId;
}
btQuaternion ToBullet(const Eigen::Quaternion<float>& q) { return btQuaternion((btScalar)q.x(), (btScalar)q.y(), (btScalar)q.z(), (btScalar)q.w()); }
void get3DMoments(vector<Point> feature_points, int feat_index, int index)
{
// for(int i=0; i<feature_points.size(); i++)
// ROS_INFO("%d --> %d,%d",i,feature_points[i].x,feature_points[i].y);
//ROS_INFO("Getting 3D Moments : %d --> %d,%d", feature_points.size(), width, height);
//Extract the indices for the points in the point cloud data
pcl::PointIndices point_indices;
for(int i=0; i<feature_points.size(); i++)
{
//ROS_INFO("Feature Index : %d, %d",feature_points[i].x, feature_points[i].y);
point_indices.indices.push_back(feature_points[i].y * width + feature_points[i].x);
}
//ROS_INFO("Computing 3D Centroid : %d",point_indices.indices.size());
Eigen::Vector4f centroid;
Eigen::Matrix3f covariance_matrix;
// Estimate the XYZ centroid
pcl::compute3DCentroid (pcl_in, point_indices, centroid);
#ifdef DEBUG
ROS_INFO("Centroid %d: %f, %f, %f, %f",index,centroid(0),centroid(1),centroid(2),centroid(3));
#endif
//ROS_INFO("Computing Covariance ");
//Compute the centroid and the covariance of the points
computeCovarianceMatrix(pcl_in, point_indices.indices, centroid, covariance_matrix);
//Print the 3D Moments
//ROS_INFO("Centroid : %f, %f, %f, %f",centroid(0),centroid(1),centroid(2),centroid(3));
#ifdef DEBUG
std::cout<<"Covariance : "<<std::endl<<covariance_matrix <<std::endl;
#endif
for(int i=0; i<3; i++)
{
feedback_.features[feat_index].moments[index].mean[i] = centroid(i);
for(int j=0; j<3; j++)
{
feedback_.features[feat_index].moments[index].covariance[i*3+j] = covariance_matrix(i,j);
}
}
//Get the principal components and the quaternion
Eigen::Matrix3f evecs;
Eigen::Vector3f evals;
pcl::eigen33 (covariance_matrix, evecs, evals);
Eigen::Matrix3f rotation;
rotation.row (0) = evecs.col (0);
rotation.row (1) = evecs.col (1);
//rotation.row (2) = evecs.col (2);
rotation.row (2) = rotation.row (0).cross (rotation.row (1));
//rotation.transposeInPlace ();
#ifdef DEBUG
std::cerr << "Rotation matrix: " << endl;
std::cerr << rotation << endl;
std::cout<<"Eigen vals : "<<evals<<std::endl;
#endif
rotation.transposeInPlace ();
Eigen::Quaternion<float> qt (rotation);
qt.normalize ();
//Publish Marker
visualization_msgs::Marker marker;
marker.header.frame_id = "/openni_rgb_optical_frame";
marker.header.stamp = ros::Time().now();
marker.ns = "Triangulation";
marker.id = index+1;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(5);
//centroid position
marker.type = visualization_msgs::Marker::SPHERE;
marker.pose.position.x = centroid(0);
marker.pose.position.y = centroid(1);
marker.pose.position.z = centroid(2);
marker.pose.orientation.x = qt.x();
marker.pose.orientation.y = qt.y();
marker.pose.orientation.z = qt.z();
marker.pose.orientation.w = qt.w();
marker.scale.x = sqrt(evals(0)) *2;
marker.scale.y = sqrt(evals(1)) *2;
marker.scale.z = sqrt(evals(2)) *2;
//red
marker.color.a = 0.5;
marker.color.r = rand()/((double)RAND_MAX + 1);
marker.color.g = rand()/((double)RAND_MAX + 1);
marker.color.b = rand()/((double)RAND_MAX + 1);
object_pose_marker_pub_.publish(marker);
}
void quaternion2eigen(const Eigen::Quaternion<T_qt> &quat_In, Eigen::Matrix<T_evec,4,1> &quat_Vec )
{
quat_Vec = Eigen::Matrix<T_evec,4,1>( T_evec(quat_In.w()), T_evec(quat_In.x()), T_evec(quat_In.y()), T_evec(quat_In.z()) );
};
void CartesianImpedance::updateHook() {
RESTRICT_ALLOC;
// ToolMass toolsM[K];
Eigen::Affine3d r[K];
// read inputs
port_joint_position_.read(joint_position_);
if (!joint_position_.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "joint_position_ contains NaN or inf" << RTT::endlog();
return;
}
port_joint_velocity_.read(joint_velocity_);
if (!joint_velocity_.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "joint_velocity_ contains NaN or inf" << RTT::endlog();
return;
}
port_nullspace_torque_command_.read(nullspace_torque_command_);
if (!nullspace_torque_command_.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "nullspace_torque_command_ contains NaN or inf" << RTT::endlog();
return;
}
for (size_t i = 0; i < K; i++) {
geometry_msgs::Pose pos;
if (port_cartesian_position_command_[i]->read(pos) == RTT::NewData) {
tf::poseMsgToEigen(pos, r_cmd[i]);
}
if (port_tool_position_command_[i]->read(pos) == RTT::NewData) {
tools[i](0) = pos.position.x;
tools[i](1) = pos.position.y;
tools[i](2) = pos.position.z;
tools[i](3) = pos.orientation.w;
tools[i](4) = pos.orientation.x;
tools[i](5) = pos.orientation.y;
tools[i](6) = pos.orientation.z;
}
cartesian_trajectory_msgs::CartesianImpedance impedance;
if (port_cartesian_impedance_command_[i]->read(impedance)
== RTT::NewData) {
Kc(i * 6 + 0) = impedance.stiffness.force.x;
Kc(i * 6 + 1) = impedance.stiffness.force.y;
Kc(i * 6 + 2) = impedance.stiffness.force.z;
Kc(i * 6 + 3) = impedance.stiffness.torque.x;
Kc(i * 6 + 4) = impedance.stiffness.torque.y;
Kc(i * 6 + 5) = impedance.stiffness.torque.z;
Dxi(i * 6 + 0) = impedance.damping.force.x;
Dxi(i * 6 + 1) = impedance.damping.force.y;
Dxi(i * 6 + 2) = impedance.damping.force.z;
Dxi(i * 6 + 3) = impedance.damping.torque.x;
Dxi(i * 6 + 4) = impedance.damping.torque.y;
Dxi(i * 6 + 5) = impedance.damping.torque.z;
}
}
port_mass_matrix_inv_.read(M);
if (!M.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "M contains NaN or inf" << RTT::endlog();
return;
}
// calculate robot data
robot_->jacobian(J, joint_position_, &tools[0]);
if (!J.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "J contains NaN or inf" << RTT::endlog();
return;
}
robot_->fkin(r, joint_position_, &tools[0]);
JT = J.transpose();
lu_.compute(M);
Mi = lu_.inverse();
if (!Mi.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "Mi contains NaN or inf" << RTT::endlog();
return;
}
// calculate stiffness component
for (size_t i = 0; i < K; i++) {
Eigen::Affine3d tmp;
tmp = r[i].inverse() * r_cmd[i];
p(i * 6) = tmp.translation().x();
p(i * 6 + 1) = tmp.translation().y();
p(i * 6 + 2) = tmp.translation().z();
Eigen::Quaternion<double> quat = Eigen::Quaternion<double>(
tmp.rotation());
p(i * 6 + 3) = quat.x();
p(i * 6 + 4) = quat.y();
p(i * 6 + 5) = quat.z();
}
F.noalias() = (Kc.array() * p.array()).matrix();
joint_torque_command_.noalias() = JT * F;
if (!joint_torque_command_.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "joint_torque_command_ contains NaN or inf" << RTT::endlog();
return;
}
// calculate damping component
#if 1
tmpNK_.noalias() = J * Mi;
A.noalias() = tmpNK_ * JT;
luKK_.compute(A);
A = luKK_.inverse();
tmpKK_ = Kc.asDiagonal();
UNRESTRICT_ALLOC;
es_.compute(tmpKK_, A);
RESTRICT_ALLOC;
K0 = es_.eigenvalues();
luKK_.compute(es_.eigenvectors());
Q = luKK_.inverse();
tmpKK_ = Dxi.asDiagonal();
Dc.noalias() = Q.transpose() * tmpKK_;
tmpKK_ = K0.cwiseSqrt().asDiagonal();
tmpKK2_.noalias() = Dc * tmpKK_;
Dc.noalias() = tmpKK2_ * Q;
tmpK_.noalias() = J * joint_velocity_;
F.noalias() = Dc * tmpK_;
if (!F.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "F contains NaN or inf" << RTT::endlog();
return;
}
joint_torque_command_.noalias() -= JT * F;
#else
UNRESTRICT_ALLOC;
tmpNN_.noalias() = JT * Kc.asDiagonal() * J;
es_.compute(tmpNN_, M, Eigen::ComputeEigenvectors | Eigen::BAx_lx);
K0 = es_.eigenvalues();
Q = es_.eigenvectors();
RESTRICT_ALLOC;
tmpNN_ = K0.cwiseSqrt().asDiagonal();
UNRESTRICT_ALLOC;
Dc.noalias() = Q * 0.7 * tmpNN_ * Q.adjoint();
RESTRICT_ALLOC;
joint_torque_command_.noalias() -= Dc * joint_velocity_;
#endif
// calculate null-space component
tmpNK_.noalias() = J * Mi;
tmpKK_.noalias() = tmpNK_ * JT;
luKK_.compute(tmpKK_);
tmpKK_ = luKK_.inverse();
tmpKN_.noalias() = Mi * JT;
Ji.noalias() = tmpKN_ * tmpKK_;
P.noalias() = Eigen::MatrixXd::Identity(P.rows(), P.cols());
P.noalias() -= J.transpose() * A * J * Mi;
if (!P.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "P contains NaN or inf" << RTT::endlog();
return;
}
joint_torque_command_.noalias() += P * nullspace_torque_command_;
// write outputs
UNRESTRICT_ALLOC;
port_joint_torque_command_.write(joint_torque_command_);
for (size_t i = 0; i < K; i++) {
geometry_msgs::Pose pos;
tf::poseEigenToMsg(r[i], pos);
port_cartesian_position_[i]->write(pos);
}
}
lwr_impedance_controller::CartImpTrajectoryPoint sampleInterpolation() {
lwr_impedance_controller::CartImpTrajectoryPoint next_point;
double timeFromStart =
(double) (now() - trajectory_.header.stamp).toSec();
double segStartTime = last_point_.time_from_start.toSec();
double segEndTime =
trajectory_.trajectory[trajectory_index_].time_from_start.toSec();
next_point = setpoint_;
// interpolate position
// x
next_point.pose.position.x = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.position.x,
trajectory_.trajectory[trajectory_index_].pose.position.x);
// y
next_point.pose.position.y = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.position.y,
trajectory_.trajectory[trajectory_index_].pose.position.y);
// z
next_point.pose.position.z = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.position.z,
trajectory_.trajectory[trajectory_index_].pose.position.z);
// interpolate orientation
Eigen::Quaternion<double> start = Eigen::Quaternion<double>(last_point_.pose.orientation.w,
last_point_.pose.orientation.x, last_point_.pose.orientation.y,
last_point_.pose.orientation.z);
Eigen::Quaternion<double> end = Eigen::Quaternion<double>(
trajectory_.trajectory[trajectory_index_].pose.orientation.w,
trajectory_.trajectory[trajectory_index_].pose.orientation.x,
trajectory_.trajectory[trajectory_index_].pose.orientation.y,
trajectory_.trajectory[trajectory_index_].pose.orientation.z);
double t = linearlyInterpolate(timeFromStart, segStartTime, segEndTime, 0,
1);
Eigen::Quaternion<double> rot = start.slerp(t, end);
next_point.pose.orientation.x = rot.x();
next_point.pose.orientation.y = rot.y();
next_point.pose.orientation.z = rot.z();
next_point.pose.orientation.w = rot.w();
/*
// x
next_point.pose.orientation.x = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.orientation.x,
trajectory_.trajectory[trajectory_index_].pose.orientation.x);
// y
next_point.pose.orientation.y = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.orientation.y,
trajectory_.trajectory[trajectory_index_].pose.orientation.y);
// z
next_point.pose.orientation.z = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.orientation.z,
trajectory_.trajectory[trajectory_index_].pose.orientation.z);
// w
next_point.pose.orientation.w = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.pose.orientation.w,
trajectory_.trajectory[trajectory_index_].pose.orientation.w);
*/
//interpolate stiffness
// x
next_point.impedance.stiffness.force.x = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.impedance.stiffness.force.x,
trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.x);
next_point.impedance.stiffness.force.y = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.impedance.stiffness.force.y,
trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.y);
next_point.impedance.stiffness.force.z = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.impedance.stiffness.force.z,
trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.z);
next_point.impedance.stiffness.torque.x = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.impedance.stiffness.torque.x,
trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.x);
next_point.impedance.stiffness.torque.y = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.impedance.stiffness.torque.y,
trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.y);
next_point.impedance.stiffness.torque.z = linearlyInterpolate(timeFromStart,
segStartTime, segEndTime, last_point_.impedance.stiffness.torque.z,
trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.z);
next_point.impedance.damping =
trajectory_.trajectory[trajectory_index_].impedance.damping;
next_point.wrench = trajectory_.trajectory[trajectory_index_].wrench;
return next_point;
}
visualization_msgs::Marker rviz_arrow(const Eigen::Vector3d & arrow, const Eigen::Vector3d & arrow_origin, int id, std::string name_space )
{
Eigen::Quaternion<double> rotation;
if(arrow.norm()<0.0001)
{
rotation=Eigen::Quaternion<double>(1,0,0,0);
}
else
{
double rotation_angle=acos(arrow.normalized().dot(Eigen::Vector3d::UnitX()));
Eigen::Vector3d rotation_axis=arrow.normalized().cross(Eigen::Vector3d::UnitX()).normalized();
rotation=Eigen::AngleAxisd(-rotation_angle+PI,rotation_axis);
}
visualization_msgs::Marker marker;
marker.header.frame_id = "/base_link";
marker.header.stamp = ros::Time();
marker.id = id;
if(id==0)
{
marker.color.r = 0.0;
marker.color.g = 0.0;
marker.color.b = 1.0;
marker.ns = name_space;
}
else
{
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker.ns = name_space;
}
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = arrow_origin.x();
marker.pose.position.y = arrow_origin.y();
marker.pose.position.z = arrow_origin.z();
marker.pose.orientation.x = rotation.x();
marker.pose.orientation.y = rotation.y();
marker.pose.orientation.z = rotation.z();
marker.pose.orientation.w = rotation.w();
//std::cout <<"position:" <<marker.pose.position << std::endl;
//std::cout <<"orientation:" <<marker.pose.orientation << std::endl;
//marker.pose.orientation.x = 0;
//marker.pose.orientation.y = 0;
//marker.pose.orientation.z = 0;
//marker.pose.orientation.w = 1;
if(arrow.norm()<0.0001)
{
marker.scale.x = 0.001;
marker.scale.y = 0.001;
marker.scale.z = 0.001;
}
else
{
marker.scale.x = arrow.norm();
marker.scale.y = 0.1;
marker.scale.z = 0.1;
}
marker.color.a = 1.0;
return marker;
}
void NavQuestSocketNode::publishDvlData(const NQRes& data)
{
geometry_msgs::TwistStamped::Ptr twist(new geometry_msgs::TwistStamped());
bool testDVL = data.velo_instrument[0] == data.velo_instrument[1];
testDVL = testDVL && (data.velo_instrument[1] == data.velo_instrument[2]);
testDVL = testDVL && (data.velo_instrument[2] == 0);
//Data validity
bool beamValidity = true;
for (int i=0; i<4; ++i)
{
beamValidity = beamValidity && (data.beam_status[i] == 1);
//ROS_INFO("Beam validity %d: %d", i, data.beam_status[i]);
//ROS_INFO("Water vel credit %d: %f", i, data.wvelo_credit[i]);
}
if (testDVL)
{
ROS_INFO("All zero dvl. Ignore measurement.");
return;
}
if (!beamValidity)
{
//ROS_INFO("One or more beams are invalid. Ignore measurement: %f %f", data.velo_instrument[0]/1000, data.velo_instrument[1]/1000);
return;
}
else
{
ROS_INFO("Beams are valid. Accept measurement: %f %f", data.velo_instrument[0]/1000, data.velo_instrument[1]/1000);
}
(*beam_pub["velo_rad"])(data.velo_rad);
(*beam_pub["wvelo_rad"])(data.wvelo_rad);
(*speed_pub["velo_instrument"])(data.velo_instrument);
(*speed_pub["velo_earth"])(data.velo_earth);
(*speed_pub["water_velo_instrument"])(data.water_velo_instrument);
(*speed_pub["water_velo_earth"])(data.water_velo_earth);
enum{valid_flag=3};
bool water_lock= (data.velo_instrument[valid_flag]==2) || (data.velo_earth[valid_flag]==2);
bool valid=data.velo_instrument[valid_flag] && data.velo_earth[valid_flag];
std_msgs::Bool bottom_lock;
bottom_lock.data = !water_lock && valid;
lock.publish(bottom_lock);
//Altitude
if (data.altitude_estimate > 0)
{
std_msgs::Float32Ptr alt(new std_msgs::Float32());
alt->data = data.altitude_estimate;
altitude.publish(alt);
}
//TF frame
//Either use a fixed rotation here or the DVL measurements
enum {roll=0, pitch, yaw};
tf::Transform transform;
//Moved 1.4m from the rotation origin.
transform.setOrigin(tf::Vector3(1.4,0,0));
//transform.setOrigin(tf::Vector3(0,0,0));
if (useFixed)
{
transform.setRotation(tf::createQuaternionFromRPY(0,0, base_orientation));
broadcast.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "dvl_frame"));
}
else
{
Eigen::Quaternion<float> quat;
labust::tools::quaternionFromEulerZYX(labust::math::wrapRad(data.rph[roll]/180*M_PI),
labust::math::wrapRad(data.rph[pitch]/180*M_PI),
labust::math::wrapRad(data.rph[yaw]/180*M_PI + magnetic_declination), quat);
transform.setRotation(tf::Quaternion(quat.x(), quat.y(), quat.z(), quat.w()));
broadcast.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "local", "dvl_frame"));
}
//RPY
auv_msgs::RPY::Ptr rpy(new auv_msgs::RPY());
rpy->roll = labust::math::wrapRad(data.rph[roll]/180*M_PI);
rpy->pitch = labust::math::wrapRad(data.rph[pitch]/180*M_PI);
rpy->yaw = labust::math::wrapRad(data.rph[yaw]/180*M_PI);
imuPub.publish(rpy);
}