IGL_INLINE void igl::two_axis_valuator_fixed_up(
const int w,
const int h,
const double speed,
const Eigen::Quaterniond & down_quat,
const int down_x,
const int down_y,
const int mouse_x,
const int mouse_y,
Eigen::Quaterniond & quat)
{
using namespace Eigen;
Vector3d axis(0,1,0);
quat = down_quat *
Quaterniond(
AngleAxisd(
PI*((double)(mouse_x-down_x))/(double)w*speed/2.0,
axis.normalized()));
quat.normalize();
{
Vector3d axis(1,0,0);
if(axis.norm() != 0)
{
quat =
Quaterniond(
AngleAxisd(
PI*(mouse_y-down_y)/(double)h*speed/2.0,
axis.normalized())) * quat;
quat.normalize();
}
}
}
int main (int argc, char **argv)
{
// Here's all the input***********************************************************************************************************
//doesn't load this flipped version of the file (reversing coordinates makes negative dot product calculations)
vector<string> filenames = {wine_glass, paper_bowl, red_mug};
vector<string> modelnames = {"wine_glass", "paper_bowl", "red_mug"};
//make affine3d's
Eigen::Quaterniond q;
q = Eigen::Quaterniond(0.5, 0.5, 0, 0); //identity matrix
q.normalize();
Eigen::Affine3d aq = Eigen::Affine3d(q);
Eigen::Affine3d t(Eigen::Translation3d(Eigen::Vector3d(-4,0,1.25))); //wine_glass
Eigen::Affine3d a = (t*aq);
q = Eigen::Quaterniond(0.5, 0.5, 0, 0); //identity matrix
q.normalize();
aq = Eigen::Affine3d(q);
t = (Eigen::Translation3d(Eigen::Vector3d(0,0,1.1))); //paper bowl
Eigen::Affine3d b = (t*aq);
q = Eigen::Quaterniond(0.5, 0.5, 0, 0); //identity matrix
q.normalize();
aq = Eigen::Affine3d(q);
t = (Eigen::Translation3d(Eigen::Vector3d(4,0,0.66))); //red mug
Eigen::Affine3d c = (t*aq);
vector<Eigen::Affine3d> model_poses = {a,b,c}; //vector 1 of affines
q = Eigen::Quaterniond(0.3, 0.7, 0, 0); //NOT identity matrix
q.normalize();
aq = Eigen::Affine3d(q);
t = (Eigen::Translation3d(Eigen::Vector3d(-2,0, 1.59))); //wine_glass
Eigen::Affine3d a2 = (t*aq);
vector<Eigen::Affine3d> model_poses2 = {a2,b,c}; //vector of new affines
// Here's all the input^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// construct object and set parameters
SceneValidator *scene = new SceneValidator; //construct the object
scene->setParams("DRAW", true); //visualize what's actually happening
scene->setParams("PRINT_CHKR_RSLT", true); //print out the result of each check
scene->setParams("STEP1", 1000); //make the first check take 1000 steps so user has plenty of time to see what's going on
scene->setScale(1, 200); //set modelnames[1] to be scaled down by factor of 200
// API functions
scene->setModels(modelnames, filenames); //set all the models and get their data
scene->isValidScene(modelnames, model_poses); //check scene 1
//Press Ctrl-X when window pops up to exit above^ and start the simulation below
scene->isValidScene(modelnames, model_poses2); //check scene 2
return 0;
}
bool planning_models::quatFromMsg(const geometry_msgs::Quaternion &qmsg, Eigen::Quaterniond &q)
{
q = Eigen::Quaterniond(qmsg.w, qmsg.x, qmsg.y, qmsg.z);
double error = fabs(q.squaredNorm() - 1.0);
if (error > 0.05)
{
ROS_ERROR("Quaternion is NOWHERE CLOSE TO NORMALIZED [x,y,z,w], [%.2f, %.2f, %.2f, %.2f]. Can't do much, returning identity.",
qmsg.x, qmsg.y, qmsg.z, qmsg.w);
q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
return false;
}
else if(error > 1e-3)
q.normalize();
return true;
}
TEST (PCL, WarpPointRigid6DDouble)
{
WarpPointRigid6D<PointXYZ, PointXYZ, double> warp;
Eigen::Quaterniond q (0.4455, 0.9217, 0.3382, 0.3656);
q.normalize ();
Eigen::Vector3d t (0.82550, 0.11697, 0.44864);
Eigen::VectorXd p (6);
p[0] = t.x (); p[1] = t.y (); p[2] = t.z (); p[3] = q.x (); p[4] = q.y (); p[5] = q.z ();
warp.setParam (p);
PointXYZ pin (1, 2, 3), pout;
warp.warpPoint (pin, pout);
EXPECT_NEAR (pout.x, 4.15963, 1e-5);
EXPECT_NEAR (pout.y, -1.51363, 1e-5);
EXPECT_NEAR (pout.z, 0.922648, 1e-5);
}
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();
}
// update the camera state given a new camera pose
void MotionModel::UpdateCameraPose(const cv::Point3d& newPosition, const cv::Vec4d& newOrientation)
{
// Compute linear velocity
cv::Vec3d newLinearVelocity( newPosition - position_ );
// In order to be robust against fast camera movements linear velocity is smoothed over time
newLinearVelocity = (newLinearVelocity + linearVelocity_) * 0.5;
// compute rotation between q1 and q2: q2 * qInverse(q1);
Eigen::Quaterniond newAngularVelocity = cv2eigen( newOrientation ) * orientation_.inverse();
// In order to be robust against fast camera movements angular velocity is smoothed over time
newAngularVelocity = newAngularVelocity.slerp(0.5, angularVelocity_);
newAngularVelocity.normalize();
// Update the current state variables
position_ = newPosition;
orientation_ = cv2eigen( newOrientation );
linearVelocity_ = newLinearVelocity;
angularVelocity_ = newAngularVelocity;
}
TEST(TFEigenConversions, eigen_tf_transform)
{
tf::Transform t;
Eigen::Affine3d k;
Eigen::Quaterniond kq;
kq.coeffs()(0) = gen_rand(-1.0,1.0);
kq.coeffs()(1) = gen_rand(-1.0,1.0);
kq.coeffs()(2) = gen_rand(-1.0,1.0);
kq.coeffs()(3) = gen_rand(-1.0,1.0);
kq.normalize();
k.translate(Eigen::Vector3d(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
k.rotate(kq);
transformEigenToTF(k,t);
for(int i=0; i < 3; i++)
{
ASSERT_NEAR(t.getOrigin()[i],k.matrix()(i,3),1e-6);
for(int j=0; j < 3; j++)
{
ASSERT_NEAR(t.getBasis()[i][j],k.matrix()(i,j),1e-6);
}
}
}
Eigen::Matrix4d state2Matrix(state s) {
// take a state and return the equivalent 4x4 homogeneous transformation matrix
Eigen::Matrix4d mat = Eigen::Matrix4d::Zero();
// rotation part
Eigen::Quaterniond q;
q.x() = s.qx;
q.y() = s.qy;
q.z() = s.qz;
q.w() = s.qw;
q.normalize();
Eigen::Matrix3d rot = q.toRotationMatrix();
mat.block<3, 3>(0, 0) = rot;
// translation part
Eigen::Vector3d t;
t[0] = s.x;
t[1] = s.y;
t[2] = s.z;
mat.block<3, 1>(0, 3) = t;
// homogeneous
mat(3, 3) = 1;
return mat;
}
void CableDraw::DrawCylinder(Eigen::Vector3d a, Eigen::Vector3d b, double length)
{
Eigen::Vector3d axis_vector = b - a;
Eigen::Vector3d axis_vector_normalized = axis_vector.normalized();
Eigen::Vector3d up_vector(0.0, 0.0, 1.0);
Eigen::Vector3d right_axis_vector = axis_vector_normalized.cross(up_vector);
right_axis_vector.normalize();
double theta = axis_vector_normalized.dot(up_vector);
double angle_rotation = -acos(theta);
tf::Vector3 tf_right_axis_vector;
tf::vectorEigenToTF(right_axis_vector, tf_right_axis_vector);
// Create quaternion
tf::Quaternion tf_q(tf_right_axis_vector, angle_rotation);
// Convert back to Eigen
Eigen::Quaterniond q;
tf::quaternionTFToEigen(tf_q, q);
// Rotate so that vector points along line
q.normalize();
// Draw marker
visualization_msgs::Marker marker;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
marker.header.frame_id = frame_id_;
marker.header.stamp = ros::Time::now();
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
marker.ns = "basic_shapes";
marker.id = 0;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker.type = visualization_msgs::Marker::CYLINDER;
// Set the marker action. Options are ADD and DELETE
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 = (a[0] + b[0])/2.0;
marker.pose.position.y = (a[1] + b[1])/2.0;
marker.pose.position.z = (a[2] + b[2])/2.0;
marker.pose.orientation.x = q.x();
marker.pose.orientation.y = q.y();
marker.pose.orientation.z = q.z();
marker.pose.orientation.w = q.w();
// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker.scale.x = 2*radius_threshold_;
marker.scale.y = 2*radius_threshold_;
marker.scale.z = length;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 231.0/255.0;
marker.color.g = 79.0/255.0;
marker.color.b = 81.0/255.0;
marker.color.a = 0.25;
marker.lifetime = ros::Duration();
// Publish the marker
remove_area_pub_.publish(marker);
}
int main (int argc, char **argv)
{
// Here's all the input***********************************************************************************************************
/* Doesn't load flipped version of file (reversing coordinate order in faces makes negative dot product calculations)
Doing the flipping operation in some other software screws up center of mass calculations because
the cross product produced is negative. Just unflip the flipped figures in software (meshlab), and in the future don't flip
them at all. Or at the very least, just make sure your rotation/translation of the model in some other
software doesn't screw up the vertice order for a face. */
vector<string> filenames = {filename};
vector<string> modelnames = {"test_object"};
//make affine3d's
Eigen::Quaterniond q;
q = Eigen::Quaterniond(0.5, 0.5, 0, 0); //identity matrix
q.normalize();
Eigen::Affine3d aq = Eigen::Affine3d(q);
Eigen::Affine3d t(Eigen::Translation3d(Eigen::Vector3d(0,0,2)));
Eigen::Affine3d a = (t*aq);
q = Eigen::Quaterniond(0.7, 0.3, 0, 0); //NOT identity matrix
q.normalize();
aq = Eigen::Affine3d(q);
t = (Eigen::Translation3d(Eigen::Vector3d(0,0,2)));
Eigen::Affine3d b = (t*aq);
q = Eigen::Quaterniond(0.5, 0.5, 0, 0); //identity matrix
q.normalize();
aq = Eigen::Affine3d(q);
t = (Eigen::Translation3d(Eigen::Vector3d(0,0,1)));
Eigen::Affine3d c = (t*aq);
q = Eigen::Quaterniond(0.3, 0.7, 0, 0); //NOT identity matrix
q.normalize();
aq = Eigen::Affine3d(q);
t = (Eigen::Translation3d(Eigen::Vector3d(0,0,1.7)));
Eigen::Affine3d d = (t*aq);
vector<Eigen::Affine3d> model_poses1 = {a};
vector<Eigen::Affine3d> model_poses2 = {b};
vector<Eigen::Affine3d> model_poses3 = {c};
vector<Eigen::Affine3d> model_poses4 = {d};
// Here's all the input^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// construct object and set parameters
SceneValidator *scene = new SceneValidator; //construct the object
scene->setParams("DRAW", true); //visualize what's actually happening
scene->setParams("STEP1", 1000); //make the first check take 1000 steps (so viewer has plenty of time to see object's behavior)
scene->setScale(0, 0.1); //set modelnames[0] to be scaled down by factor of 100
// API functions
scene->setModels(modelnames, filenames); //set model and get its data
scene->isValidScene(modelnames, model_poses1); //check scene 1
scene->isValidScene(modelnames, model_poses2); //check scene 2
scene->isValidScene(modelnames, model_poses3); //check scene 3
scene->isValidScene(modelnames, model_poses4); //check scene 4
return 0;
}
bool GazeboInterface::readJointStates()
{
if(!ROSControlInterface::readJointStates())
return false;
if(m_last_modelStates)
{
std::vector<std::string>::const_iterator it = std::find(
m_last_modelStates->name.begin(), m_last_modelStates->name.end(),
m_modelName
);
if(it != m_last_modelStates->name.end())
{
int idx = it - m_last_modelStates->name.begin();
//
// Orientation feedback
//
// Retrieve the robot pose
const geometry_msgs::Pose& pose = m_last_modelStates->pose[idx];
// Set the robot orientation
Eigen::Quaterniond quat;
tf::quaternionMsgToEigen(pose.orientation, quat);
quat.normalize();
m_model->setRobotOrientation(quat);
// Provide /odom if wanted. Convention: /odom is on the floor.
if(m_publishOdom && m_last_modelStatesStamp - m_initTime > ros::Duration(3.0))
{
tf::StampedTransform trans;
trans.frame_id_ = "/odom";
trans.child_frame_id_ = "/ego_rot";
trans.stamp_ = m_last_modelStatesStamp;
trans.setIdentity();
tf::Vector3 translation;
tf::pointMsgToTF(pose.position, translation);
trans.setOrigin(translation);
Eigen::Quaterniond rot;
rot = Eigen::AngleAxisd(m_model->robotEYaw(), Eigen::Vector3d::UnitZ());
tf::Quaternion quat;
tf::quaternionEigenToTF(rot, quat);
trans.setRotation(quat);
ROS_DEBUG("robot pos: Z = %f, yaw: %f, stamp: %f", translation.z(), m_model->robotEYaw(), trans.stamp_.toSec());
ROS_DEBUG("robot pos tf: %f %f %f %f %f %f %f",
trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z(),
trans.getRotation().w(), trans.getRotation().x(), trans.getRotation().y(), trans.getRotation().z()
);
m_pub_tf.sendTransform(trans);
}
//
// Angular velocity feedback
//
// Retrieve the robot twist
const geometry_msgs::Twist& twist = m_last_modelStates->twist[idx];
// Retrieve the robot angular velocity in global coordinates
Eigen::Vector3d globalAngularVelocity;
tf::vectorMsgToEigen(twist.angular, globalAngularVelocity);
// Set the measured angular velocity (local coordinates)
m_model->setRobotAngularVelocity(quat.conjugate() * globalAngularVelocity);
//
// Acceleration feedback
//
// We'd need an extra Gazebo plugin for acceleration sensing.
// For now we can do without and simply calculate the current
// acceleration due to gravity (neglect inertial accelerations).
Eigen::Vector3d globalGravityAcceleration(0.0, 0.0, 9.81);
// Set the measured acceleration (local coordinates)
m_model->setAccelerationVector(quat.conjugate() * globalGravityAcceleration);
//
// Magnetic field vector feedback
//
// We assume that north is positive X. The values of 0.20G (horiz)
// and 0.44G (vert) are approximately valid for central europe.
Eigen::Vector3d globalMagneticVector(0.20, 0.00, -0.44); // In gauss
// Set the measured magnetic field vector (local coordinates)
m_model->setMagneticFieldVector(quat.conjugate() * globalMagneticVector);
}
}
// Return success
return true;
}
