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)); }