void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
mPoseMap.lock();
vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(),
OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
localPoseMap[t] = localPose;
Eigen::Quaterniond globalQ;
globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;
Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),
globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};
globalPoseMap[t] = globalPose;
lastP = globalP;
lastQ = globalQ;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(t);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = lastP.x();
pose_stamped.pose.position.y = lastP.y();
pose_stamped.pose.position.z = lastP.z();
pose_stamped.pose.orientation.x = lastQ.x();
pose_stamped.pose.orientation.y = lastQ.y();
pose_stamped.pose.orientation.z = lastQ.z();
pose_stamped.pose.orientation.w = lastQ.w();
global_path.header = pose_stamped.header;
global_path.poses.push_back(pose_stamped);
mPoseMap.unlock();
}
/*!
* \brief affine3d2UrdfPose converts an Eigen affine 4x4 matrix o represent the pose into a urdf pose
* vparam pose eigen Affine3d pose
* \return urdf pose with position and rotation.
*/
RCS::Pose Affine3d2UrdfPose(const Eigen::Affine3d &pose) {
RCS::Pose p;
p.getOrigin().setX(pose.translation().x());
p.getOrigin().setY(pose.translation().y());
p.getOrigin().setZ(pose.translation().z());
Eigen::Quaterniond q (pose.rotation());
tf::Quaternion qtf(q.x(),q.y(),q.z(),q.w());
//std::cout << "Affine3d2UrdfPose Quaterion = \n" << q.x() << ":" << q.y() << ":" << q.z() << ":" << q.w() << std::endl;
p.setRotation(qtf);
//std::cout << "After Affine3d2UrdfPose Quaterion = \n" << p.getRotation().x() << ":" << p.getRotation().y() << ":" << p.getRotation().z() << ":" << p.getRotation().w() << std::endl;
#if 0
MatrixEXd m = pose.rotation();
Eigen::Quaterniond q = EMatrix2Quaterion(m);
Eigen::Quaterniond q(pose.rotation());
p.getRotation().setX(q.x());
p.getRotation().setY(q.y());
p.getRotation().setZ(q.z());
p.getRotation().setW(q.w());
#endif
return p;
}
void printQuatAngles(Eigen::Quaterniond& quat)
{
double heading = atan2(2*quat.y()*quat.w()-2*quat.x()*quat.z(), 1 -2 * quat.y()*quat.y()-2*quat.z()*quat.z());
double attitude = asin(2*quat.x()*quat.y() + 2 *quat.z()*quat.w());
double bank = atan2(2*quat.x()*quat.w()- 2 * quat.y()*quat.z(), 1 - 2 *quat.x() * quat.x() -2 * quat.z() * quat.z());
cout << "heading: " << Angle(heading).deg() << " attitude: " << Angle(attitude).deg() << " bank: " << Angle(bank).deg() << endl;
}
Eigen::Matrix3d FeatureModel::dR_by_dqz(const Eigen::Quaterniond &q)
{
Eigen::Matrix3d M;
M << -2*q.z(), -2*q.w(), 2*q.x(),
2*q.w(), -2*q.z(), 2*q.y(),
2*q.x(), 2*q.y(), 2*q.z();
return M;
}
static inline Eigen::Vector3d
_quat_to_roll_pitch_yaw(const Eigen::Quaterniond&q)
{
double roll_a = 2 * (q.w() * q.x() + q.y() * q.z());
double roll_b = 1 - 2 * (q.x() * q.x() + q.y() * q.y());
double roll = atan2(roll_a, roll_b);
double pitch_sin = 2 * (q.w() * q.y() - q.z() * q.x());
double pitch = asin(pitch_sin);
double yaw_a = 2 * (q.w() * q.z() + q.x() * q.y());
double yaw_b = 1 - 2 * (q.y() * q.y() + q.z() * q.z());
double yaw = atan2(yaw_a, yaw_b);
return Eigen::Vector3d(roll, pitch, yaw);
}
void App::getRobotState(drc::robot_state_t& robot_state_msg, int64_t utime_in, Eigen::VectorXd q, std::vector<std::string> jointNames){
robot_state_msg.utime = utime_in;
// Pelvis Pose:
robot_state_msg.pose.translation.x =q(0);
robot_state_msg.pose.translation.y =q(1);
robot_state_msg.pose.translation.z =q(2);
Eigen::Quaterniond quat = euler_to_quat( q(3), q(4), q(5));
robot_state_msg.pose.rotation.w = quat.w();
robot_state_msg.pose.rotation.x = quat.x();
robot_state_msg.pose.rotation.y = quat.y();
robot_state_msg.pose.rotation.z = quat.z();
robot_state_msg.twist.linear_velocity.x = 0;
robot_state_msg.twist.linear_velocity.y = 0;
robot_state_msg.twist.linear_velocity.z = 0;
robot_state_msg.twist.angular_velocity.x = 0;
robot_state_msg.twist.angular_velocity.y = 0;
robot_state_msg.twist.angular_velocity.z = 0;
// Joint States:
for (size_t i = 0; i < jointNames.size(); i++) {
robot_state_msg.joint_name.push_back( jointNames[i] );
robot_state_msg.joint_position.push_back( q(i) );
robot_state_msg.joint_velocity.push_back( 0);
robot_state_msg.joint_effort.push_back( 0 );
}
robot_state_msg.num_joints = robot_state_msg.joint_position.size();
}
static void subject_publish_callback(const ViconDriver::Subject &subject)
{
static std::set<std::string> defined_msgs;
std::string msgname = "vicon_" + subject.name;
if(defined_msgs.find(msgname) == defined_msgs.end())
{
if(pthread_mutex_trylock(&ipc_mutex) == 0)
{
IPC_defineMsg(msgname.c_str(), IPC_VARIABLE_LENGTH, ViconSubject::getIPCFormat());
pthread_mutex_unlock(&ipc_mutex);
defined_msgs.insert(msgname);
}
else
return;
}
if(loadCalib(subject.name))
{
Eigen::Affine3d current_pose = Eigen::Affine3d::Identity();
current_pose.translate(Eigen::Vector3d(subject.translation));
current_pose.rotate(Eigen::Quaterniond(subject.rotation));
current_pose = current_pose*calib_pose[subject.name];
const Eigen::Vector3d position(current_pose.translation());
const Eigen::Quaterniond rotation(current_pose.rotation());
ViconSubject subject_ipc;
subject_ipc.time_sec = subject.time_usec/1000000; // Integer division
subject_ipc.time_usec = subject.time_usec - subject_ipc.time_sec*1000000;
subject_ipc.frame_number = subject.frame_number;
subject_ipc.name = const_cast<char*>(subject.name.c_str());
subject_ipc.occluded = subject.occluded;
subject_ipc.position[0] = position.x();
subject_ipc.position[1] = position.y();
subject_ipc.position[2] = position.z();
subject_ipc.orientation[0] = rotation.x();
subject_ipc.orientation[1] = rotation.y();
subject_ipc.orientation[2] = rotation.z();
subject_ipc.orientation[3] = rotation.w();
subject_ipc.num_markers = subject.markers.size();
subject_ipc.markers = new ViconMarker[subject_ipc.num_markers];
for(int i = 0; i < subject_ipc.num_markers; i++)
{
subject_ipc.markers[i].name =
const_cast<char*>(subject.markers[i].name.c_str());
subject_ipc.markers[i].subject_name =
const_cast<char*>(subject.markers[i].subject_name.c_str());
subject_ipc.markers[i].position[0] = subject.markers[i].translation[0];
subject_ipc.markers[i].position[1] = subject.markers[i].translation[1];
subject_ipc.markers[i].position[2] = subject.markers[i].translation[2];
subject_ipc.markers[i].occluded = subject.markers[i].occluded;
}
if(pthread_mutex_trylock(&ipc_mutex) == 0)
{
IPC_publishData(msgname.c_str(), &subject_ipc);
pthread_mutex_unlock(&ipc_mutex);
}
delete[] subject_ipc.markers;
}
}
geometry_msgs::PoseStamped MutualPoseEstimation::generatePoseMessage(const Eigen::Vector3d &position, Eigen::Matrix3d rotation){
geometry_msgs::PoseStamped estimated_position;
estimated_position.header.frame_id = "ardrone_base_frontcam";//ardrone_base_link
//estimated_position.pose.position.x = position[2] + 0.21; // z
//estimated_position.pose.position.y = -position[0]; // x
//estimated_position.pose.position.z = -position[1]; //-y
estimated_position.pose.position.x = position[0] + 0.21;
estimated_position.pose.position.y = position[1];
estimated_position.pose.position.z = position[2];
/*rotation = Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())
* rotation;*/
Eigen::Quaterniond q = Eigen::Quaterniond(rotation);
// estimated_position.pose.orientation.x = q.z();
// estimated_position.pose.orientation.y = -q.x();
// estimated_position.pose.orientation.z = -q.y();
// estimated_position.pose.orientation.w = q.w();
estimated_position.pose.orientation.x = q.x();
estimated_position.pose.orientation.y = q.y();
estimated_position.pose.orientation.z = q.z();
estimated_position.pose.orientation.w = q.w();
return estimated_position;
}
/**
* @function pose2Affine3d
*/
Eigen::Affine3d DartLoader::pose2Affine3d( urdf::Pose _pose ) {
Eigen::Quaterniond quat;
_pose.rotation.getQuaternion(quat.x(), quat.y(), quat.z(), quat.w());
Eigen::Affine3d transform(quat);
transform.translation() = Eigen::Vector3d(_pose.position.x, _pose.position.y, _pose.position.z);
return transform;
}
Eigen::Quaterniond VizHelperNode::rotateSatelliteFrame(const iri_common_drivers_msgs::SatellitePseudorange &sat, ros::Time time_ros)
{
Eigen::Quaterniond rotation;
Eigen::Vector3d satVelocity(sat.v_x * scale, sat.v_y * scale, sat.v_z * scale);
//quaternione che fa ruotare l'asse x in modo che combaci con il vettore velocita'
rotation.setFromTwoVectors(Eigen::Vector3d::UnitX(), satVelocity);
geometry_msgs::Quaternion odom_quat;
odom_quat.x = rotation.x();
odom_quat.y = rotation.y();
odom_quat.z = rotation.z();
odom_quat.w = rotation.w();
// Publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = time_ros;
odom_trans.header.frame_id = WORLD_FRAME; //frame padre
odom_trans.child_frame_id = getSatelliteFrame(sat.sat_id); //frame figlio (che sto creando ora)
odom_trans.transform.translation.x = sat.x * scale;//traslazione dell'origine
odom_trans.transform.translation.y = sat.y * scale;
odom_trans.transform.translation.z = sat.z * scale;
odom_trans.transform.rotation = odom_quat; //rotazione
//send the transform
transBroadcaster.sendTransform(odom_trans);
return rotation;
}
void VizHelperNode::publishOdometry(const iri_common_drivers_msgs::SatellitePseudorange &sat, const Eigen::Quaterniond &rotation, ros::Time time_ros)
{
/// publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = time_ros;
odom.header.frame_id = WORLD_FRAME;
//set the position
odom.pose.pose.position.x = sat.x * scale;
odom.pose.pose.position.y = sat.y * scale;
odom.pose.pose.position.z = sat.z * scale;
//set the orientation
odom.pose.pose.orientation.x = rotation.x();
odom.pose.pose.orientation.y = rotation.y();
odom.pose.pose.orientation.z = rotation.z();
odom.pose.pose.orientation.w = rotation.w();
//set the velocity
odom.child_frame_id = getSatelliteFrame(sat.sat_id);
odom.twist.twist.linear.x = sat.v_x * scale;
odom.twist.twist.linear.y = sat.v_y * scale;
odom.twist.twist.linear.z = sat.v_z * scale;
odom.twist.twist.angular.x = 0;
odom.twist.twist.angular.y = 0;
odom.twist.twist.angular.z = 0;
//publish the message
//TODO fare i singoli odom publisher
// odomPub[index].publish(odom);
odomAllPub.publish(odom);
}
Eigen::Quaterniond pick_positive(const Eigen::Quaterniond& other) {
if (other.w() < 0) {
return Eigen::Quaterniond(-other.w(), -other.x(), -other.y(), -other.z());
} else {
return other;
}
}
void quaternionEigenToTF(const Eigen::Quaterniond& e, tf::Quaternion& t)
{
t[0] = e.x();
t[1] = e.y();
t[2] = e.z();
t[3] = e.w();
}
/**
* @function pose2Affine3d
*/
Eigen::Isometry3d DartLoader::toEigen(const urdf::Pose& _pose) {
Eigen::Quaterniond quat;
_pose.rotation.getQuaternion(quat.x(), quat.y(), quat.z(), quat.w());
Eigen::Isometry3d transform(quat);
transform.translation() = Eigen::Vector3d(_pose.position.x, _pose.position.y, _pose.position.z);
return transform;
}
void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m)
{
m.x = e.x();
m.y = e.y();
m.z = e.z();
m.w = e.w();
}
void RigidBody::set_attitude(Eigen::Quaterniond _attitude)
{
attitude.w() = _attitude.w();
attitude.x() = _attitude.x();
attitude.y() = _attitude.y();
attitude.z() = _attitude.z();
}
geometry_msgs::Pose PoseAffineToGeomMsg(const Eigen::Affine3d &e) {
geometry_msgs::Pose m;
m.position.x = e.translation().x();
m.position.y = e.translation().y();
m.position.z = e.translation().z();
// This is a column major vs row major matrice faux pas!
#if 0
MatrixEXd em = e.rotation();
Eigen::Quaterniond q = EMatrix2Quaterion(em);
#endif
Eigen::Quaterniond q(e.rotation());
m.orientation.x = q.x();
m.orientation.y = q.y();
m.orientation.z = q.z();
m.orientation.w = q.w();
#if 0
if (m.orientation.w < 0) {
m.orientation.x *= -1;
m.orientation.y *= -1;
m.orientation.z *= -1;
m.orientation.w *= -1;
}
#endif
}
bool constraint_samplers::IKConstraintSampler::sample(planning_models::KinematicState::JointStateGroup *jsg, const planning_models::KinematicState &ks, unsigned int max_attempts)
{
// make sure we at least have a chance of sampling using IK; we need at least some kind of constraint
if (!sampling_pose_.position_constraint_ && !sampling_pose_.orientation_constraint_)
return false;
// load an IK solver if we need to
if (!loadIKSolver())
{
kb_.reset();
return false;
}
for (unsigned int a = 0 ; a < max_attempts ; ++a)
{
// sample a point in the constraint region
Eigen::Vector3d point;
Eigen::Quaterniond quat;
if (!samplePose(point, quat, ks, max_attempts))
return false;
geometry_msgs::Pose ik_query;
ik_query.position.x = point.x();
ik_query.position.y = point.y();
ik_query.position.z = point.z();
ik_query.orientation.x = quat.x();
ik_query.orientation.y = quat.y();
ik_query.orientation.z = quat.z();
ik_query.orientation.w = quat.w();
if (callIK(ik_query, ik_timeout_, jsg))
return true;
}
return false;
}
bool RosMessageContext::getOdomPose(Eigen::Isometry3d& _trans, double time){
bool transformFound = true;
_tfListener->waitForTransform(_odomReferenceFrameId, _baseReferenceFrameId,
ros::Time(time), ros::Duration(1.0));
try{
tf::StampedTransform t;
_tfListener->lookupTransform(_odomReferenceFrameId, _baseReferenceFrameId,
ros::Time(time), t);
Eigen::Isometry3d transform;
transform.translation().x()=t.getOrigin().x();
transform.translation().y()=t.getOrigin().y();
transform.translation().z()=t.getOrigin().z();
Eigen::Quaterniond rot;
rot.x()=t.getRotation().x();
rot.y()=t.getRotation().y();
rot.z()=t.getRotation().z();
rot.w()=t.getRotation().w();
transform.linear()=rot.toRotationMatrix();
_trans = transform;
transformFound = true;
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
transformFound = false;
}
return transformFound;
}
void RectangleHandler::initRectangle(const Eigen::VectorXd& Sw, double lambda,
const Eigen::VectorXd& z, Eigen::VectorXd& shapeParams,
Eigen::VectorXd& F) {
//Get the points
Eigen::Vector3d m1(z[0], z[1], 1);
Eigen::Vector3d m2(z[2], z[3], 1);
Eigen::Vector3d m3(z[4], z[5], 1);
Eigen::Vector3d m4(z[6], z[7], 1);
Eigen::Vector3d Ct(Sw[0], Sw[1], Sw[2]);
Eigen::Quaterniond Cq(Sw[3], Sw[4], Sw[5], Sw[6]);
//compute normals
double c2 = (m1.cross(m3).transpose() * m4)[0]
/ (m2.cross(m3).transpose() * m4)[0];
double c3 = (m1.cross(m3).transpose() * m2)[0]
/ (m4.cross(m3).transpose() * m2)[0];
Eigen::Vector3d n2 = c2 * m2 - m1;
Eigen::Vector3d n3 = c3 * m4 - m1;
//Compute rotation matrix columns
Eigen::Vector3d R1 = _K.inverse() * n2;
R1 = R1 / R1.norm();
Eigen::Vector3d R2 = _K.inverse() * n3;
R2 = R2 / R2.norm();
Eigen::Vector3d R3 = R1.cross(R2);
//Compute frame quaternion
Eigen::Matrix3d R;
R << R1, R2, R3;
const Eigen::Quaterniond qOC(R);
Eigen::Quaterniond Fqhat = Cq * qOC;
//Compute frame transaltion
Eigen::Matrix3d omega = _K.transpose().inverse() * _K.inverse();
double ff = sqrt(
(n2.transpose() * omega * n2)[0] / (n3.transpose() * omega * n3)[0]);
Eigen::Vector3d CtOhat = lambda * _K.inverse() * m1;
Eigen::Vector3d Fthat = Ct + Cq.toRotationMatrix() * CtOhat;
//compute shape parameters
Eigen::Vector3d X = _K * R1;
Eigen::Vector3d Y = c2 * lambda * m2 - lambda * m1;
double w = ((X.transpose() * X).inverse() * X.transpose() * Y)[0];
double h = w / ff;
//Write the results
shapeParams << w, h;
F << Fthat[0], Fthat[1], Fthat[2], Fqhat.w(), Fqhat.x(), Fqhat.y(), Fqhat.z();
}
Eigen::Quaterniond getOrientation(geometry_msgs::TransformStampedConstPtr tf){
Eigen::Quaterniond orientation;
orientation.w() = tf->transform.rotation.w;
orientation.x() = tf->transform.rotation.x;
orientation.y() = tf->transform.rotation.y;
orientation.z() = tf->transform.rotation.z;
return orientation;
}
Eigen::Quaterniond getOrientation(geometry_msgs::PoseWithCovarianceStampedConstPtr ps){
Eigen::Quaterniond orientation;
orientation.w() = ps->pose.pose.orientation.w;
orientation.x() = ps->pose.pose.orientation.x;
orientation.y() = ps->pose.pose.orientation.y;
orientation.z() = ps->pose.pose.orientation.z;
return orientation;
}
Eigen::Isometry3d KDLToEigen(KDL::Frame tf){
Eigen::Isometry3d tf_out;
tf_out.setIdentity();
tf_out.translation() << tf.p[0], tf.p[1], tf.p[2];
Eigen::Quaterniond q;
tf.M.GetQuaternion( q.x() , q.y(), q.z(), q.w());
tf_out.rotate(q);
return tf_out;
}
YAML::Node SetOrientationYaml( const Eigen::Quaterniond& quat )
{
YAML::Node node;
node["qw"] = quat.w();
node["qx"] = quat.x();
node["qy"] = quat.y();
node["qz"] = quat.z();
return node;
}
void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& yaw) {
const double q0 = q.w();
const double q1 = q.x();
const double q2 = q.y();
const double q3 = q.z();
roll = atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
pitch = asin(2*(q0*q2-q3*q1));
yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
}
void scale_quaternion(double r,Eigen::Quaterniond q,Eigen::Quaterniond &q_out){
if (q.w() ==1){
// BUG: 15dec2011
// TODO: check if this is a rigerous solution
// q.w() ==1 mean no translation. so just return the input
// if q.w()==1 and r=0.5 ... what is created below will have w=0 ... invalid
//std::cout << "unit quaternion input:\n";
q_out = q;
return;
}
double theta = acos(q.w());
//double sin_theta_inv = 1/ sin(theta);
q_out.w() = sin((1-r)*theta) + sin(r*theta) * q.w();
q_out.x() = sin(r*theta) * q.x();
q_out.y() = sin(r*theta) * q.y();
q_out.z() = sin(r*theta) * q.z();
q_out.normalize();
}
void UpperBodyPlanner::msgPose2Eigen(const geometry_msgs::Pose& input, Eigen::Vector3d& translation, Eigen::Quaterniond& q) {
translation(0) = input.position.x;
translation(1) = input.position.y;
translation(2) = input.position.z;
q.w() = input.orientation.w;
q.x() = input.orientation.x;
q.y() = input.orientation.y;
q.z() = input.orientation.z;
}
void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e)
{
// // is it faster to do this?
k.GetQuaternion(e.x(), e.y(), e.z(), e.w());
// or this?
//double x, y, z, w;
//k.GetQuaternion(x, y, z, w);
//e = Eigen::Quaterniond(w, x, y, z);
}
void updateVizMarker(int id, Eigen::Vector3d _pose, Eigen::Quaterniond _attitude)
{
marker[id].pose.position.x = _pose(0);
marker[id].pose.position.y = _pose(1);
marker[id].pose.position.z = _pose(2);
marker[id].pose.orientation.x = _attitude.x();
marker[id].pose.orientation.y = _attitude.y();
marker[id].pose.orientation.z = _attitude.z();
marker[id].pose.orientation.w = _attitude.w();
pub_body[id].publish( marker[id] );
}
double quaternion_get_yaw(const Eigen::Quaterniond &q)
{
// to match equation from:
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
const double &q0 = q.w();
const double &q1 = q.x();
const double &q2 = q.y();
const double &q3 = q.z();
return std::atan2(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3));
}