Beispiel #1
0
void TestFind3DAffineTransform(){

  // Create datasets with known transform
  Eigen::Matrix3Xd in(3, 100), out(3, 100);
  Eigen::Quaternion<double> Q(1, 3, 5, 2);
  Q.normalize();
  Eigen::Matrix3d R = Q.toRotationMatrix();
  double scale = 2.0;
  for (int row = 0; row < in.rows(); row++) {
    for (int col = 0; col < in.cols(); col++) {
      in(row, col) = log(2*row + 10.0)/sqrt(1.0*col + 4.0) + sqrt(col*1.0)/(row + 1.0);
    }
  }
  Eigen::Vector3d S;
  S << -5, 6, -27;
  for (int col = 0; col < in.cols(); col++)
    out.col(col) = scale*R*in.col(col) + S;

  Eigen::Affine3d A = Find3DAffineTransform(in, out);

  // See if we got the transform we expected
  if ( (scale*R-A.linear()).cwiseAbs().maxCoeff() > 1e-13 ||
       (S-A.translation()).cwiseAbs().maxCoeff() > 1e-13)
    throw "Could not determine the affine transform accurately enough";
}
bool CartesianTrajectoryAction::checkTolerance(Eigen::Affine3d err, cartesian_trajectory_msgs::CartesianTolerance tol) {
  if ((tol.position.x > 0.0) && (fabs(err.translation().x()) > tol.position.x)) {
    return false;
  }

  if ((tol.position.y > 0.0) && (fabs(err.translation().y()) > tol.position.y)) {
    return false;
  }

  if ((tol.position.z > 0.0) && (fabs(err.translation().z()) > tol.position.z)) {
    return false;
  }

  Eigen::AngleAxisd ax(err.rotation());
  Eigen::Vector3d rot = ax.axis() * ax.angle();

  if ((tol.rotation.x > 0.0) && (fabs(rot(0)) > tol.rotation.x)) {
    return false;
  }

  if ((tol.rotation.y > 0.0) && (fabs(rot(1)) > tol.rotation.y)) {
    return false;
  }

  if ((tol.rotation.z > 0.0) && (fabs(rot(2)) > tol.rotation.z)) {
    return false;
  }

  return true;
}
	bool srvSetHuboObjectPose(HuboApplication::SetHuboObjectPose::Request &req,
						      HuboApplication::SetHuboObjectPose::Response &res)
	{
		bool response = true;
		Eigen::Affine3d tempPose;
		Eigen::Isometry3d armPose;

		tf::poseMsgToEigen(req.Target, tempPose);
		if (tempPose(0,3) != tempPose(0,3)) // null check
		{
			res.Success = false;
			return false;
		}

		armPose = tempPose.matrix();

		//std::cerr << tempPose.matrix() << std::endl;

		m_Manip.setControlMode(OBJECT_POSE);
		m_Manip.setAngleMode(QUATERNION);
		m_Manip.setPose(armPose, req.ObjectIndex);
		m_Manip.sendCommand();
		//std::cerr << armPose.matrix() << std::endl;
			res.Success = response;
		return response;
	}
	bool srvSetHuboArmPose(HuboApplication::SetHuboArmPose::Request &req,
						   HuboApplication::SetHuboArmPose::Response &res)
	{
		bool response = true;
		Eigen::Affine3d tempPose;
		Eigen::Isometry3d armPose;

		tf::poseMsgToEigen(req.Target, tempPose);
		if (tempPose(0,3) != tempPose(0,3)) // null check
		{
			res.Success = false;
			return false;
		}

		//std::cerr << tempPose.matrix() << std::endl;

		armPose = tempPose.matrix();
		//armPose.translation() = tempPose.translation();
		//armPose.linear() = tempPose.rotation();
		//std::cerr << armPose.matrix() << std::endl;

		m_Manip.setControlMode(END_EFFECTOR);
		m_Manip.setAngleMode(QUATERNION);
		m_Manip.setPose(armPose, req.ArmIndex);
		m_Manip.sendCommand();
		//std::cerr << armPose.matrix() << std::endl;
			res.Success = response;
		return response;
	}
void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position)
{
  QList<QListWidgetItem *> sel = ui_->collision_objects_list->selectedItems();
  if (sel.empty())
    return;
  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  if (ps)
  {
    collision_detection::CollisionWorld::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString());
    if (obj && obj->shapes_.size() == 1)
    {
      Eigen::Affine3d p;
      p.translation()[0] = ui_->object_x->value();
      p.translation()[1] = ui_->object_y->value();
      p.translation()[2] = ui_->object_z->value();

      p = Eigen::Translation3d(p.translation()) *
        (Eigen::AngleAxisd(ui_->object_rx->value(), Eigen::Vector3d::UnitX()) *
         Eigen::AngleAxisd(ui_->object_ry->value(), Eigen::Vector3d::UnitY()) *
         Eigen::AngleAxisd(ui_->object_rz->value(), Eigen::Vector3d::UnitZ()));

      ps->getWorldNonConst()->moveShapeInObject(obj->id_, obj->shapes_[0], p);
      planning_display_->queueRenderSceneGeometry();

      // Update the interactive marker pose to match the manually introduced one
      if (update_marker_position && scene_marker_)
      {
        Eigen::Quaterniond eq(p.rotation());
        scene_marker_->setPose(Ogre::Vector3(ui_->object_x->value(), ui_->object_y->value(), ui_->object_z->value()),
                               Ogre::Quaternion(eq.w(), eq.x(), eq.y(), eq.z()), "");
      }
    }
  }
}
void 
RGBID_SLAM::VisualizationManager::getPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr whole_point_cloud)
{  
  IdToPoseMap::iterator id2pose_map_it;        
  IdToPointCloudMap::const_iterator id2pointcloud_map_it;
  
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr aux_point_cloud;  
  aux_point_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
  
  for (id2pose_map_it = id2pose_map_.begin(); 
       id2pose_map_it != id2pose_map_.end();
       id2pose_map_it++)
  {
    int kf_id = (*id2pose_map_it).first;
    Eigen::Affine3d pose = (*id2pose_map_it).second;
    
    id2pointcloud_map_it = id2pointcloud_map_.find(kf_id);
    
    if (id2pointcloud_map_it != id2pointcloud_map_.end())
    {
      aux_point_cloud->clear();
      copyPointCloud((*id2pointcloud_map_it).second, aux_point_cloud);  
      (*id2pointcloud_map_it).second->clear();
      alignPointCloud(aux_point_cloud, pose.linear(), pose.translation());   
      *whole_point_cloud += *aux_point_cloud;
    }          
  }  
  
  
}
TEST(TFEigenConversions, tf_eigen_transform)
{
  tf::Transform t;
  tf::Quaternion tq;
  tq[0] = gen_rand(-1.0,1.0);
  tq[1] = gen_rand(-1.0,1.0);
  tq[2] = gen_rand(-1.0,1.0);
  tq[3] = gen_rand(-1.0,1.0);
  tq.normalize();
  t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
  t.setRotation(tq);

  Eigen::Affine3d k;
  transformTFToEigen(t,k);

  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);
    }
  }
  for (int col = 0 ; col < 3; col ++)
    ASSERT_NEAR(k.matrix()(3, col), 0, 1e-6);
  ASSERT_NEAR(k.matrix()(3,3), 1, 1e-6);
  
}
Beispiel #8
0
Eigen::Isometry3d toIsometry(const Eigen::Affine3d& pose)
{
  Eigen::Isometry3d p(pose.rotation());
  p.translation() = pose.translation();

  return p;
}
bool kinematic_constraints::VisibilityConstraint::equal(const KinematicConstraint &other, double margin) const
{
  if (other.getType() != type_)
    return false;
  const VisibilityConstraint &o = static_cast<const VisibilityConstraint&>(other);
  
  if (target_frame_id_ == o.target_frame_id_ && sensor_frame_id_ == o.sensor_frame_id_ &&
      cone_sides_ == o.cone_sides_ && sensor_view_direction_ == o.sensor_view_direction_)
  {
    if (fabs(max_view_angle_ - o.max_view_angle_) > margin ||
        fabs(target_radius_ - o.target_radius_) > margin)
      return false;
    Eigen::Affine3d diff = sensor_pose_.inverse() * o.sensor_pose_;
    if (diff.translation().norm() > margin)
      return false;
    if (!diff.rotation().isIdentity(margin))
      return false;
    diff = target_pose_.inverse() * o.target_pose_;
    if (diff.translation().norm() > margin)
      return false;
    if (!diff.rotation().isIdentity(margin))
      return false;
    return true;
  }
  return false;
}
void Irp6pInverseKinematic::updateHook() {

  if (port_input_wrist_pose_.read(wrist_pose_) == RTT::NewData) {

    Eigen::Affine3d trans;
    tf::poseMsgToEigen(wrist_pose_, trans);

    port_current_joint_position_.read(local_current_joints_);

    inverse_kinematics_single_iteration(local_current_joints_, trans,
                                        &local_desired_joints_);

    port_output_joint_position_.write(local_desired_joints_);

  } else if (port_input_end_effector_pose_.read(end_effector_pose_)
      == RTT::NewData) {

    port_tool_.read(tool_msgs_);

    Eigen::Affine3d tool;
    Eigen::Affine3d trans;
    Eigen::Affine3d wrist_pose;
    tf::poseMsgToEigen(end_effector_pose_, trans);
    tf::poseMsgToEigen(tool_msgs_, tool);

    wrist_pose = trans * tool.inverse();

    port_current_joint_position_.read(local_current_joints_);

    inverse_kinematics_single_iteration(local_current_joints_, wrist_pose,
                                        &local_desired_joints_);

    port_output_joint_position_.write(local_desired_joints_);
  }
}
bool kinematic_constraints::PositionConstraint::equal(const KinematicConstraint &other, double margin) const
{
  if (other.getType() != type_)
    return false;
  const PositionConstraint &o = static_cast<const PositionConstraint&>(other);
  
  if (link_model_ == o.link_model_ && constraint_frame_id_ == o.constraint_frame_id_)
  {
    if ((offset_ - o.offset_).norm() > margin)
      return false;
    if (constraint_region_.size() != o.constraint_region_.size())
      return false;
    for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
    {
      Eigen::Affine3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[i];
      if (diff.translation().norm() > margin)
        return false;
      if (!diff.rotation().isIdentity(margin))
        return false;
      if (fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[i]->computeVolume()) >= margin)
        return false;
    }
    return true;
  }
  return false;
}
bool SensorProcessorBase::updateTransformations(const std::string& sensorFrameId,
                                                const ros::Time& timeStamp)
{
  try {
    transformListener_.waitForTransform(sensorFrameId, mapFrameId_, timeStamp, ros::Duration(1.0));

    tf::StampedTransform transformTf;
    transformListener_.lookupTransform(mapFrameId_, sensorFrameId, timeStamp, transformTf);
    poseTFToEigen(transformTf, transformationSensorToMap_);

    transformListener_.lookupTransform(robotBaseFrameId_, sensorFrameId, timeStamp, transformTf);  // TODO Why wrong direction?
    Eigen::Affine3d transform;
    poseTFToEigen(transformTf, transform);
    rotationBaseToSensor_.setMatrix(transform.rotation().matrix());
    translationBaseToSensorInBaseFrame_.toImplementation() = transform.translation();

    transformListener_.lookupTransform(mapFrameId_, robotBaseFrameId_, timeStamp, transformTf);  // TODO Why wrong direction?
    poseTFToEigen(transformTf, transform);
    rotationMapToBase_.setMatrix(transform.rotation().matrix());
    translationMapToBaseInMapFrame_.toImplementation() = transform.translation();

    return true;
  } catch (tf::TransformException &ex) {
    ROS_ERROR("%s", ex.what());
    return false;
  }
}
/*!
* \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 UpperBodyPlanner::pose_moveTo(const std::string &link_name, const Eigen::Affine3d& desired_pose, const int &step_num, std::vector<geometry_msgs::Pose> &pose_sequence) {
    Eigen::Affine3d current_pose = kinematic_state->getGlobalLinkTransform(link_name);
    Eigen::Quaterniond current_q = Rmat2Quaternion(current_pose.rotation());
    Eigen::Quaterniond desired_q = Rmat2Quaternion(desired_pose.rotation());
    Eigen::Quaterniond step_q;

    std::cout << "**********************************" << std::endl;
    std::cout << "end_effector_name is: " << link_name << std::endl;
    std::cout << "Current translation is: " << current_pose.translation().transpose() << std::endl;
    std::cout << "current rotation is: " << std::endl;
    std::cout << current_pose.rotation() << std::endl;
    std::cout << "current quaternion is: "<< std::endl;
    std::cout << "w = " << current_q.w() << ", x = " << current_q.x() << ", y = " << current_q.y() << ", z = " << current_q.z() << std::endl;
    std::cout << "Desired translation is: " << desired_pose.translation().transpose() << std::endl;
    std::cout << "Desired rotation is: " << std::endl;
    std::cout << desired_pose.rotation() << std::endl;
    std::cout << "Desired quaternion is: " << std::endl;
    std::cout << "w = " << desired_q.w() << ", x = " << desired_q.x() << ", y = " << desired_q.y() << ", z = " << desired_q.z() << std::endl;
    std::cout << "**********************************" << std::endl;

    Eigen::Vector3d step_translation_movement = (desired_pose.translation() - current_pose.translation()) / step_num;
    for (int i = 0; i < step_num; i++) {
        Eigen::Affine3d step_target;
        step_target.translation() = current_pose.translation() + (i + 1) * step_translation_movement;
        step_q = current_q.slerp((i + 1.0) / step_num, desired_q);
        geometry_msgs::Pose target_pose = Eigen2msgPose(step_target.translation(), step_q);
        pose_sequence.push_back(target_pose);
    }
}
Beispiel #15
0
 /**
  * @brief transform matrix which represents the pose transform as a 4x4
  *  homogenous matrix
  *
  * @result 4x4 homogenous transform matrix
  */
 Eigen::Affine3d toTransform() const
 {
     Eigen::Affine3d t;
     t = orientation;
     t.pretranslate( position );
     return t;
 }
TEST_F(DistanceFieldCollisionDetectionTester, LinksInCollision)
{
  collision_detection::CollisionRequest req;
  collision_detection::CollisionResult res1;
  collision_detection::CollisionResult res2;
  collision_detection::CollisionResult res3;
  //req.contacts = true;
  //req.max_contacts = 100;
  req.group_name = "whole_body";

  planning_models::KinematicState kstate(kmodel_);
  kstate.setToDefaultValues();

  Eigen::Affine3d offset = Eigen::Affine3d::Identity();
  offset.translation().x() = .01;

  kstate.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
  kstate.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
  acm_->setEntry("base_link", "base_bellow_link", false);
  crobot_->checkSelfCollision(req, res1, kstate, *acm_);
  ASSERT_TRUE(res1.collision);

  acm_->setEntry("base_link", "base_bellow_link", true);
  crobot_->checkSelfCollision(req, res2, kstate, *acm_);
  ASSERT_FALSE(res2.collision);
  
  //  req.verbose = true;
  kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
  kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset);

  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
  crobot_->checkSelfCollision(req, res3, kstate, *acm_);
  ASSERT_TRUE(res3.collision);
}
  void commandForce(const geometry_msgs::WrenchStamped::ConstPtr &msg)
  {
    F_des_ << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, 
              msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z;
    if(msg->header.frame_id == root_name_)
      return;

    geometry_msgs::PoseStamped in_root;
    in_root.pose.orientation.w = 1.0;
    in_root.header.frame_id = msg->header.frame_id;

    try {
      tf_.waitForTransform(root_name_, msg->header.frame_id, msg->header.stamp, ros::Duration(0.1));
      tf_.transformPose(root_name_, in_root, in_root);
    }
    catch (const tf::TransformException &ex)
    {
      ROS_ERROR("Failed to transform: %s", ex.what());
      return;
    }
    
    Eigen::Affine3d t;
    
    tf::poseMsgToEigen(in_root.pose, t);

    F_des_.head<3>() = t.linear() * F_des_.head<3>();
    F_des_.tail<3>() = t.linear() * F_des_.tail<3>();
  }
void myCallback1(const interaction_cursor_msgs::InteractionCursorUpdate poseSubscribed) {
    // std::cout<<" i am in mycallback "<<std::endl;

    target1 = Eigen::Affine3d::Identity();



    Eigen::Quaterniond q(poseSubscribed.pose.pose.orientation.w,
            poseSubscribed.pose.pose.orientation.x,
            poseSubscribed.pose.pose.orientation.y,
            poseSubscribed.pose.pose.orientation.z);

    translation1 << poseSubscribed.pose.pose.position.x,
            poseSubscribed.pose.pose.position.y,
            poseSubscribed.pose.pose.position.z;
    std::cout << " 3 " << std::endl;
    rotation1 = q.toRotationMatrix();
    target1.translation() = translation1;
    target1.linear() = rotation1;




    std::cout << "please move hydra to a proper place and click the button. " << std::endl;

    if (poseSubscribed.button_state == 1) {
        button1 = true;

    }
}
/* checks if a given scene is in static equilibrium or not */
bool SceneValidator::isValidScene(std::vector<string> modelnames, std::vector<Eigen::Affine3d> model_poses){
    //set all the Objects's positions
    num = modelnames.size();
    for (int i =0; i < num; i++){
       auto mappedObject= m.find(modelnames[i]);   //get model from hashmap
       Eigen::Affine3d a = model_poses[i];
       const dMatrix3 R = {                        //convert affine info to a 3x3 rotation matrix and a x,y,z position array
         a(0,0), a(0,1), a(0,2),
         a(1,0), a(1,1), a(1,2),
         a(2,0), a(2,1), a(2,2)    };
       const dReal center[3] = {a.translation()[0],a.translation()[1], a.translation()[2]};
       translateObject(mappedObject->second, center, R);  //get the model name's MyObject info and feed it the position and rotation
    }

    //complete series of checks to see if scene is still stable or not
    if (!isStableStill(modelnames, STEP1)){   //check #1
         return false;
    } else
    if (!isStableStill(modelnames, STEP2)){   //check #2
         return false;
    } else
    if (!isStableStill(modelnames, STEP3)){   //check #3
         return false;
    } else
    if (!isStableStill(modelnames, STEP4)){   //check #4
         return false;
    }
    else{
         return true;
    }
}
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 VisualOdometry::resetposSrvCallback(
        Save::Request& request,
        Save::Response& response)
{

    const std::string& new_tr = request.filename;
//    f2b_
    double m00,m01,m02,m03,m10,m11,m12,m13,m20,m21,m22,m23,m30,m31,m32,m33;
    int res;

    res = sscanf(new_tr.c_str(), "%lg %lg %lg %lg\n%lg %lg %lg %lg\n%lg %lg %lg %lg\n%lg %lg %lg %lg\n)",
                     &m00,&m01,&m02,&m03,&m10,&m11,&m12,&m13,&m20,&m21,&m22,&m23,&m30,&m31,&m32,&m33);
    if (res != 16)
    {
         res = sscanf(new_tr.c_str(), "%lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg)",
                         &m00,&m01,&m02,&m03,&m10,&m11,&m12,&m13,&m20,&m21,&m22,&m23,&m30,&m31,&m32,&m33);
         if (res != 16)
         {
             return false;
         }
    }

//    Eigen::Affine3d pose;
//    pose(0,0) = m00;
//    pose(0,1) = m01;
//    pose(0,2) = m02;
//    pose(0,3) = m03;
//    pose(1,0) = m10;
//    pose(1,1) = m11;
//    pose(1,2) = m12;
//    pose(1,3) = m13;
//    pose(2,0) = m20;
//    pose(2,1) = m21;
//    pose(2,2) = m22;
//    pose(2,3) = m23;
//    pose(3,0) = m30;
//    pose(3,1) = m31;
//    pose(3,2) = m32;
//    pose(3,3) = m33;
    double yaw,pitch,roll;
    tf::Matrix3x3 rot(m00,m01,m02,m10,m11,m12,m20,m21,m22);
    tf::Matrix3x3 correction(0,0,1,-1,0,0,0,-1,0); //convert from camera pose to world
    rot = rot*correction.inverse();
    rot.getEulerYPR(yaw,pitch,roll);

    Eigen::Affine3d pose;
    pose.setIdentity();
    pose =  Eigen::Translation<double,3>(m03,m13,m23)*
            Eigen::AngleAxis<double>(yaw,Eigen::Vector3d::UnitZ()) *
            Eigen::AngleAxis<double>(pitch,Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxis<double>(roll,Eigen::Vector3d::UnitX());
//    pose = pose *   Eigen::AngleAxis<double>( M_PI,Eigen::Vector3d::UnitZ());
    tf::TransformEigenToTF(pose,f2b_);
    motion_estimation_.resetModel();
//    tf::Vector3 ori(m03,m13,m23);
//    f2b_.setBasis(rot);
//    f2b_.setOrigin(ori);
    return true;
}
/**
 * @function KState
 */
Eigen::Affine3d KState::xform() const {

  Eigen::Affine3d xform = Eigen::Affine3d::Identity();
  xform.linear() = body_rot.toRotationMatrix();
  xform.translation() = body_pos;
  
  return xform;
}
void mesh_core::PlaneProjection::getFrame(Eigen::Affine3d& frame) const
{
  frame.setIdentity();
  frame.translation() = origin_;
  frame.linear().col(0) = x_axis_;
  frame.linear().col(1) = y_axis_;
  frame.linear().col(2) = normal_;
}
Beispiel #24
0
 inline std::string PrettyPrint(const Eigen::Affine3d& transform_to_print, const bool add_delimiters, const std::string& separator)
 {
     UNUSED(add_delimiters);
     UNUSED(separator);
     Eigen::Vector3d vector_to_print = transform_to_print.translation();
     Eigen::Quaterniond quaternion_to_print(transform_to_print.rotation());
     return "Affine3d <x: " + std::to_string(vector_to_print.x()) + " y: " + std::to_string(vector_to_print.y()) + " z: " + std::to_string(vector_to_print.z()) + ">, <x: " + std::to_string(quaternion_to_print.x()) + " y: " + std::to_string(quaternion_to_print.y()) + " z: " + std::to_string(quaternion_to_print.z()) + " w: " + std::to_string(quaternion_to_print.w()) + ">";
 }
Beispiel #25
0
void drawRedDotsPlane()
{
    int planeTilt = (int)factors.at("Tilt");
    int planeSlant = (int)factors.at("Slant");
    double planeDef = factors.at("Def");
    double planeOnset=1;
    bool backprojectionActive=true;
    double theta=0.0;
    Eigen::Affine3d modelTransformation;

    glPushMatrix();
    //glLoadIdentity();
    //glTranslated(0,0,focalDistance);

    glMultMatrixd(stimTransformation.data());
    switch ( planeTilt )
    {
    case 0:
    {
        theta = -acos(exp(-0.346574+0.5*planeDef-planeDef/2*(1-planeOnset*periodicValue/oscillationAmplitude)));
        glRotated(toDegrees(theta),0,1,0);
        if (backprojectionActive)
            glScaled(1/sin(toRadians( -90-planeSlant)),1,1);	//backprojection phase
    }
    break;
    case 90:
    {
        theta = -acos(exp(-0.346574+0.5*planeDef-planeDef/2*(1-planeOnset*periodicValue/oscillationAmplitude)));
        glRotated( toDegrees(theta),1,0,0);
        if (backprojectionActive)
            glScaled(1,1/sin(toRadians( -90-planeSlant )),1); //backprojection phase
    }
    break;
    case 180:
    {
        theta = acos(exp(-0.346574+0.5*planeDef-planeDef/2*(1+planeOnset*periodicValue/oscillationAmplitude)));
        glRotated(toDegrees(theta),0,1,0);
        if (backprojectionActive)
            glScaled(1/sin(toRadians( -90-planeSlant )),1,1); //backprojection phase
    }
    break;
    case 270:
    {
        theta = acos(exp(-0.346574+0.5*planeDef-planeDef/2*(1+planeOnset*periodicValue/oscillationAmplitude)));
        glRotated( toDegrees(theta),1,0,0);
        if (backprojectionActive)
            glScaled(1,1/sin(toRadians( -90-planeSlant )),1); //backprojection phase
    }
    break;
    default:
        cerr << "Error, select tilt to be {0,90,180,270} only" << endl;
    }

    stimDrawer.draw();
    glGetDoublev(GL_MODELVIEW_MATRIX,modelTransformation.matrix().data());
    modelTransformation.matrix() = modelTransformation.matrix().eval();
    glPopMatrix();
}
Beispiel #26
0
// The input 3D points are stored as columns.
Eigen::Affine3d Find3DAffineTransform(Eigen::Matrix3Xd in, Eigen::Matrix3Xd out) {

  // Default output
  Eigen::Affine3d A;
  A.linear() = Eigen::Matrix3d::Identity(3, 3);
  A.translation() = Eigen::Vector3d::Zero();

  if (in.cols() != out.cols())
    throw "Find3DAffineTransform(): input data mis-match";

  // First find the scale, by finding the ratio of sums of some distances,
  // then bring the datasets to the same scale.
  double dist_in = 0, dist_out = 0;
  for (int col = 0; col < in.cols()-1; col++) {
    dist_in  += (in.col(col+1) - in.col(col)).norm();
    dist_out += (out.col(col+1) - out.col(col)).norm();
  }
  if (dist_in <= 0 || dist_out <= 0)
    return A;
  double scale = dist_out/dist_in;
  out /= scale;

  // Find the centroids then shift to the origin
  Eigen::Vector3d in_ctr = Eigen::Vector3d::Zero();
  Eigen::Vector3d out_ctr = Eigen::Vector3d::Zero();
  for (int col = 0; col < in.cols(); col++) {
    in_ctr  += in.col(col);
    out_ctr += out.col(col);
  }
  in_ctr /= in.cols();
  out_ctr /= out.cols();
  for (int col = 0; col < in.cols(); col++) {
    in.col(col)  -= in_ctr;
    out.col(col) -= out_ctr;
  }

  // SVD
  Eigen::MatrixXd Cov = in * out.transpose();
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(Cov, Eigen::ComputeThinU | Eigen::ComputeThinV);

  // Find the rotation
  double d = (svd.matrixV() * svd.matrixU().transpose()).determinant();
  if (d > 0)
    d = 1.0;
  else
    d = -1.0;
  Eigen::Matrix3d I = Eigen::Matrix3d::Identity(3, 3);
  I(2, 2) = d;
  Eigen::Matrix3d R = svd.matrixV() * I * svd.matrixU().transpose();

  // The final transform
  A.linear() = scale * R;
  A.translation() = scale*(out_ctr - R*in_ctr);

  return A;
}
Beispiel #27
0
void RockinPNPActionServer::tf2Affine(tf::StampedTransform& tf, Eigen::Affine3d& T)
{
  tf::Vector3 o=tf.getOrigin();
  tf::Quaternion q_tf=tf.getRotation();
  Eigen::Quaterniond q(q_tf[3],q_tf[0],q_tf[1],q_tf[2]);
  Eigen::Matrix3d R(q);
  Eigen::Vector3d t(o[0],o[1],o[2]);
  T.linear()=R; T.translation()=t;
  
}
Beispiel #28
0
void ICPLocalization::addScanLineToPointCloud(Eigen::Affine3d body2Odo, Eigen::Affine3d body2World, Eigen::Affine3d laser2Body, const ::base::samples::LaserScan &scan_reading)
{

    if(scansWithTransforms.size() == 0) 
    {
	addLaserScan(body2Odo,body2World,laser2Body,scan_reading); 
	return;
    }
    
    /*
    double max_rotation =  1.5 * conf_point_cloud.lines_per_point_cloud * conf_point_cloud.min_rotation_for_new_line;
    double max_translation = 1.5 * conf_point_cloud.lines_per_point_cloud * conf_point_cloud.min_distance_travelled_for_new_line;
    double max_head_movement = 1.5 * conf_point_cloud.lines_per_point_cloud * conf_point_cloud.min_rotation_head_for_new_line; 
    */
    bool add_laser_scan = true; 
    for( uint i = 0; i < scansWithTransforms.size(); i++) 
    {
	Eigen::Affine3d diference( body2Odo.inverse() * scansWithTransforms.at(i).body2Odo );

	Vector3d Ylaser2Body = laser2Body * Vector3d::UnitY() - laser2Body.translation();
	Ylaser2Body.normalize();
	Vector3d YlastLaser2Body = scansWithTransforms.back().laser2Body * Vector3d::UnitY() - scansWithTransforms.at(i).laser2Body.translation();
	YlastLaser2Body.normalize();
	
	double laserChange = acos(Ylaser2Body.dot(YlastLaser2Body));
	double translation =  diference.translation().norm(); 
	double rotation = fabs(Eigen::AngleAxisd( diference.rotation() ).angle()) ; 
	
	add_laser_scan = add_laser_scan && ( rotation > conf_point_cloud.min_rotation_for_new_line || translation > conf_point_cloud.min_distance_travelled_for_new_line || laserChange >  conf_point_cloud.min_rotation_head_for_new_line);
	 
	//if  the distance is to big means the old laser scan is not consistent anymore. 
// 	if( rotation > max_rotation|| translation > max_translation || laserChange > max_head_movement)
// 	{
// 	    scansWithTransforms.erase(scansWithTransforms.begin() + i); 
// 	    scanCount--; 
// 	    std::cout << " IcpLocalization.cpp erasing old laser scan " << std::endl; 
// 	}

/*	std::cout <<" add new scan " << add_laser_scan << std::endl; 
	std::cout << "\t translation " << (translation > conf_point_cloud.min_distance_travelled_for_new_line)<< " "  << translation << " > " << conf_point_cloud.min_distance_travelled_for_new_line << std::endl;
	std::cout << "\t rotation " << (rotation > conf_point_cloud.min_rotation_for_new_line) << " "  << rotation * 180 / M_PI << " > " << conf_point_cloud.min_rotation_for_new_line * 180 / M_PI << std::endl;
	std::cout << "\t head " << (laserChange >  conf_point_cloud.min_rotation_head_for_new_line) << "  "<< laserChange * 180 / M_PI << " > " << conf_point_cloud.min_rotation_head_for_new_line * 180 / M_PI<< std::endl; */
	
	if (!add_laser_scan) 
	    break; 
    }
    
    if ( add_laser_scan )
    {
	addLaserScan(body2Odo,body2World,laser2Body,scan_reading); 
    }
    
    return; 
  
}
void UpperBodyPlanner::addPose_moveTo(const Eigen::Affine3d& start_pose, const Eigen::Affine3d& end_pose, const int& step_num, std::vector<geometry_msgs::Pose>& pose_sequence) {
    Eigen::Quaterniond start_q = Rmat2Quaternion(start_pose.rotation());
    Eigen::Quaterniond end_q = Rmat2Quaternion(end_pose.rotation());
    Eigen::Quaterniond step_q;

    std::cout << "**********************************" << std::endl;
    std::cout << "start_pose translation is: " << start_pose.translation().transpose() << std::endl;
    std::cout << "start_pose rotation is: " << std::endl;
    std::cout << start_pose.rotation() << std::endl;
    std::cout << "start_pose quaternion is: "<< std::endl;
    std::cout << "w = " << start_q.w() << ", x = " << start_q.x() << ", y = " << start_q.y() << ", z = " << start_q.z() << std::endl;
    std::cout << "end_pose translation is: " << end_pose.translation().transpose() << std::endl;
    std::cout << "end_pose rotation is: " << std::endl;
    std::cout << end_pose.rotation() << std::endl;
    std::cout << "Desired quaternion is: " << std::endl;
    std::cout << "w = " << end_q.w() << ", x = " << end_q.x() << ", y = " << end_q.y() << ", z = " << end_q.z() << std::endl;
    std::cout << "**********************************" << std::endl;

    Eigen::Vector3d step_translation_movement = (end_pose.translation() - start_pose.translation()) / step_num;
    for (int i = 0; i < step_num; i++) {
        Eigen::Affine3d step_target;
        step_target.translation() = start_pose.translation() + (i + 1) * step_translation_movement;
        step_q = start_q.slerp((i + 1.0) / step_num, end_q);
        geometry_msgs::Pose target_pose = Eigen2msgPose(step_target.translation(), step_q);
        pose_sequence.push_back(target_pose);
    }
}
Beispiel #30
0
TEST(TfEigen, ConvertAffine3d)
{
  const Eigen::Affine3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));

  Eigen::Affine3d v1;
  geometry_msgs::Pose p1;
  tf2::convert(v, p1);
  tf2::convert(p1, v1);

  EXPECT_EQ(v.translation(), v1.translation());
  EXPECT_EQ(v.rotation(), v1.rotation());
}