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 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;
}
/*!
* \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_moveTo2(const std::string &link_name,
                                    const Eigen::Affine3d& current_pose,
                                    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);
    }
}
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);
    }
}
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;
  }
}
TEST_F(LoadPlanningModelsPr2, InitOK)
{
  ASSERT_TRUE(urdf_ok_);
  ASSERT_EQ(urdf_model_->getName(), "pr2_test");

  robot_model::RobotModelPtr kmodel(new robot_model::RobotModel(urdf_model_, srdf_model_));
  robot_state::RobotState ks(kmodel);
  ks.setToRandomValues();
  ks.setToDefaultValues();


  robot_state::Transforms tf(kmodel->getModelFrame());

  Eigen::Affine3d t1;
  t1.setIdentity();
  t1.translation() = Eigen::Vector3d(10.0, 1.0, 0.0);
  tf.setTransform(t1, "some_frame_1");

  Eigen::Affine3d t2(Eigen::Translation3d(10.0, 1.0, 0.0)*Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitY()));
  tf.setTransform(t2, "some_frame_2");

  Eigen::Affine3d t3;
  t3.setIdentity();
  t3.translation() = Eigen::Vector3d(0.0, 1.0, -1.0);
  tf.setTransform(t3, "some_frame_3");


  EXPECT_TRUE(tf.isFixedFrame("some_frame_1"));
  EXPECT_FALSE(tf.isFixedFrame("base_footprint"));
  EXPECT_TRUE(tf.isFixedFrame(kmodel->getModelFrame()));

  Eigen::Affine3d x;
  x.setIdentity();
  tf.transformPose(ks, "some_frame_2", x, x);
  
  EXPECT_TRUE(t2.translation() == x.translation());
  EXPECT_TRUE(t2.rotation() == x.rotation());

  tf.transformPose(ks, kmodel->getModelFrame(), x, x);
  EXPECT_TRUE(t2.translation() == x.translation());
  EXPECT_TRUE(t2.rotation() == x.rotation());

  x.setIdentity();
  tf.transformPose(ks, "r_wrist_roll_link", x, x);

  EXPECT_NEAR(x.translation().x(), 0.585315, 1e-4);
  EXPECT_NEAR(x.translation().y(), -0.188, 1e-4);
  EXPECT_NEAR(x.translation().z(), 1.24001, 1e-4);
}
void planning_models::KinematicModel::RevoluteJointModel::computeJointStateValues(const Eigen::Affine3d& transf, std::vector<double> &joint_values) const
{
  joint_values.resize(1);
  Eigen::Quaterniond q(transf.rotation());
  q.normalize();
  joint_values[0] = acos(q.w())*2.0f;
}
Exemple #9
0
Eigen::Isometry3d toIsometry(const Eigen::Affine3d& pose)
{
  Eigen::Isometry3d p(pose.rotation());
  p.translation() = pose.translation();

  return p;
}
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 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;
}
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()), "");
      }
    }
  }
}
Exemple #13
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()) + ">";
 }
void SceneCloudView::setViewerPose(const Eigen::Affine3d& viewer_pose)
{
    Eigen::Vector3d pos_vector = viewer_pose * Eigen::Vector3d(0, 0, 0);
    Eigen::Vector3d look_at_vector = viewer_pose.rotation() * Eigen::Vector3d(0, 0, 0) + pos_vector;
    Eigen::Vector3d up_vector = viewer_pose.rotation() * Eigen::Vector3d(0, -1, 0);
    pcl17::visualization::Camera cam;
    cam.pos[0] = pos_vector[0];
    cam.pos[1] = pos_vector[1];
    cam.pos[2] = pos_vector[2];
    cam.focal[0] = look_at_vector[0];
    cam.focal[1] = look_at_vector[1];
    cam.focal[2] = look_at_vector[2];
    cam.view[0] = up_vector[0];
    cam.view[1] = up_vector[1];
    cam.view[2] = up_vector[2];
    cloud_viewer_->setCameraPosition((double)pos_vector[0], (double)pos_vector[1], (double)pos_vector[2], (double)look_at_vector[0],
                                     (double)look_at_vector[1], (double)look_at_vector[2], (double)up_vector[0], (double)up_vector[1],
                                     (double)up_vector[2]);
    cloud_viewer_->updateCamera();
}
void robot_model::FloatingJointModel::computeJointStateValues(const Eigen::Affine3d& transf, std::vector<double> &joint_values) const
{
  joint_values.resize(7);
  joint_values[0] = transf.translation().x();
  joint_values[1] = transf.translation().y();
  joint_values[2] = transf.translation().z();
  Eigen::Quaterniond q(transf.rotation());
  joint_values[3] = q.x();
  joint_values[4] = q.y();
  joint_values[5] = q.z();
  joint_values[6] = q.w();
}
Exemple #16
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());
}
	/**
	 * @brief Send attitude setpoint to FCU attitude controller
	 *
	 * @note ENU frame.
	 */
	void send_attitude_target(const ros::Time &stamp, const Eigen::Affine3d &tr) {
		/* Thrust + RPY, also bits numbering started from 1 in docs
		 */
		const uint8_t ignore_all_except_q = (1 << 6) | (7 << 0);
		float q[4];

		UAS::quaternion_to_mavlink(
				UAS::transform_orientation_enu_ned(
					UAS::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))),q);

		set_attitude_target(stamp.toNSec() / 1000000,
				ignore_all_except_q,
				q,
				0.0, 0.0, 0.0,
				0.0);
	}
void ParticleFilter3D::predict(Eigen::Affine3d Tmotion, double vx, double vy, double vz, double vroll, double vpitch, double vyaw){
	Eigen::Vector3d tr = Tmotion.translation();
	Eigen::Vector3d rot = Tmotion.rotation().eulerAngles(0,1,2);
	
	for(unsigned int i=0;i<pcloud.size();i++){
		double x = tr[0] + myrand.normalRandom() * vx;
		double y = tr[1] + myrand.normalRandom() * vy;
		double z = tr[2] + myrand.normalRandom() * vz;
		
		double roll 	= rot[0] + myrand.normalRandom() * vroll;
		double pitch = rot[1] + myrand.normalRandom() * vpitch;
		double yaw 	= rot[2] + myrand.normalRandom() * vyaw;
		
		pcloud[i].T = pcloud[i].T *(xyzrpy2affine(x,y,z,roll,pitch,yaw));
	}
}
Exemple #19
0
	/**
	 * @brief Send attitude setpoint to FCU attitude controller
	 *
	 * @note ENU frame.
	 */
	void send_attitude_target(const ros::Time &stamp, const Eigen::Affine3d &tr) {
		/* Thrust + RPY, also bits numbering started from 1 in docs
		 */
		const uint8_t ignore_all_except_q = (1 << 6) | (7 << 0);
		float q[4];

		// Eigen use same convention as mavlink: w x y z
		Eigen::Map<Eigen::Quaternionf> q_out(q);
		q_out = UAS::transform_frame_enu_ned(Eigen::Quaterniond(tr.rotation())).cast<float>();

		set_attitude_target(stamp.toNSec() / 1000000,
				ignore_all_except_q,
				q,
				0.0, 0.0, 0.0,
				0.0);
	}
void robot_model::PlanarJointModel::computeJointStateValues(const Eigen::Affine3d& transf, std::vector<double> &joint_values) const
{
  joint_values.resize(3);
  joint_values[0] = transf.translation().x();
  joint_values[1] = transf.translation().y();
  
  Eigen::Quaterniond q(transf.rotation());
  //taken from Bullet
  double s_squared = 1.0-(q.w()*q.w());
  if (s_squared < 10.0 * std::numeric_limits<double>::epsilon())
    joint_values[2] = 0.0;
  else
  {
    double s = 1.0/sqrt(s_squared);
    joint_values[2] = (acos(q.w())*2.0f)*(q.z()*s);
  }
}
Exemple #21
0
	/**
	 * @brief Send attitude setpoint and thrust to FCU attitude controller
	 */
	void send_attitude_quaternion(const ros::Time &stamp, const Eigen::Affine3d &tr, const float thrust)
	{
		/**
		 * @note RPY, also bits numbering started from 1 in docs
		 */
		const uint8_t ignore_all_except_q_and_thrust = (7 << 0);

		auto q = ftf::transform_orientation_enu_ned(
					ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))
					);

		set_attitude_target(stamp.toNSec() / 1000000,
					ignore_all_except_q_and_thrust,
					q,
					Eigen::Vector3d::Zero(),
					thrust);
	}
// Compute the desired orientation for the end effector given the handrail group
void getEndEffectorOrientation(const std::string& handrail_group, std::vector<double>& rpy)
{
    // "Normal" grasp orientation for R2 on a handrail at the origin
    Eigen::Affine3d pose = Eigen::Affine3d::Identity();
    pose *= (Eigen::AngleAxisd(3.141592653, Eigen::Vector3d::UnitX()) *
             Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) *
             Eigen::AngleAxisd(1.57079, Eigen::Vector3d::UnitZ()));

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

    if (handrail_group == "Handrail_starboard")
    {
        rotation *= (Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()) *
                     Eigen::AngleAxisd(-1.57079, Eigen::Vector3d::UnitY()) *
                     Eigen::AngleAxisd(-1.57079, Eigen::Vector3d::UnitZ()));
    }
    else if (handrail_group == "Handrail_port")
    {
        rotation *= (Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()) *
                     Eigen::AngleAxisd(1.57079, Eigen::Vector3d::UnitY()) *
                     Eigen::AngleAxisd(-1.57079, Eigen::Vector3d::UnitZ()));
    }
    else if (handrail_group == "Handrail_deck")
    {
        rotation *= (Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()) *
                     Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) *
                     Eigen::AngleAxisd(-1.57079, Eigen::Vector3d::UnitZ()));
    }
    else if (handrail_group == "Handrail_overhead")
    {
        rotation *= (Eigen::AngleAxisd(-3.141592653, Eigen::Vector3d::UnitX()) *
                     Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) *
                     Eigen::AngleAxisd(-1.57079, Eigen::Vector3d::UnitZ()));
    }
    else
        ROS_WARN("Unknown handrail group '%s'", handrail_group.c_str());

    pose = pose * rotation;

    Eigen::Vector3d euler = pose.rotation().eulerAngles(0, 1, 2);
    rpy.resize(3);
    rpy[0] = euler(0);
    rpy[1] = euler(1);
    rpy[2] = euler(2);
}
	/**
	 * @brief Send vision estimate transform to FCU position controller
	 */
	void send_vision_estimate(const ros::Time &stamp, const Eigen::Affine3d &tr)
	{
		/**
		 * @warning Issue #60.
		 * This now affects pose callbacks too.
		 */
		if (last_transform_stamp == stamp) {
			ROS_DEBUG_THROTTLE_NAMED(10, "vision_pose", "Vision: Same transform as last one, dropped.");
			return;
		}
		last_transform_stamp = stamp;

		auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
		auto rpy = ftf::quaternion_to_rpy(
				ftf::transform_orientation_enu_ned(Eigen::Quaterniond(tr.rotation())));

		vision_position_estimate(stamp.toNSec() / 1000, position, rpy);
	}
	/**
	 * @brief Send setpoint to FCU position controller.
	 *
	 * @warning Send only XYZ, Yaw. ENU frame.
	 */
	void send_position_target(const ros::Time &stamp, const Eigen::Affine3d &tr) {
		/* Documentation start from bit 1 instead 0;
		 * Ignore velocity and accel vectors, yaw rate.
		 *
		 * In past versions on PX4 there been bug described in #273.
		 * If you got similar issue please try update firmware first.
		 */
		const uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);

		auto p = UAS::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
		auto q = UAS::transform_frame_enu_ned(Eigen::Quaterniond(tr.rotation()));

		set_position_target_local_ned(stamp.toNSec() / 1000000,
				MAV_FRAME_LOCAL_NED,
				ignore_all_except_xyz_y,
				p.x(), p.y(), p.z(),
				0.0, 0.0, 0.0,
				0.0, 0.0, 0.0,
				UAS::quaternion_get_yaw(q), 0.0);
	}
Exemple #25
0
bool VoNode::initCb(){
    srv.request.timeA = time_first;
    srv.request.timeB = time_second;

    std::cout<<"\ntime_first:\n"<<(time_first)<<std::endl;
    std::cout<<"\ntime_second:\n"<<(time_second)<<std::endl;

    ROS_INFO("Calling service get_between_pose");

    if (client.call(srv)){
        ROS_INFO("Got inti pose from get_between_pose srv!");
        geometry_msgs::Pose m = srv.response.A2B;;
        Eigen::Affine3d e = Eigen::Translation3d(m.position.x,
                                               m.position.y,
                                               m.position.z) *
              Eigen::Quaterniond(m.orientation.w,
                                 m.orientation.x,
                                 m.orientation.y,
                                 m.orientation.z);

        Eigen::Matrix3d rot = e.rotation();
        Eigen::Vector3d trans = e.translation();

        std::cout<<"Time diff in first and second:\n"<<(time_second-time_first)<<std::endl;
        std::cout<<time_first<<std::endl<<time_second<<std::endl;

        SE3 current_trans = SE3(rot,trans);


        std::cout<<"trans:\n"<<trans<<std::endl;
        std::cout<<"rot:\n"<<rot<<std::endl;

        vo_->InitPose(current_trans);
        our_scale_ = current_trans.matrix().block<3,1>(0,3).norm();
        return true;
    }
    else{
        ROS_ERROR("Failed to call service get_between_pose");
        return false;
    }
}
 bool CaptureStereoSynchronizer::checkNearPose(
   const geometry_msgs::Pose& new_pose)
 {
   Eigen::Affine3d new_affine;
   tf::poseMsgToEigen(new_pose, new_affine);
   for (size_t i = 0; i < poses_.size(); i++) {
     // convert pose into eigen
     Eigen::Affine3d affine;
     tf::poseMsgToEigen(poses_[i], affine);
     // compute difference
     Eigen::Affine3d diff = affine.inverse() * new_affine;
     double positional_difference = diff.translation().norm();
     if (positional_difference < positional_bin_size_) {
       Eigen::AngleAxisd rotational_difference(diff.rotation());
       if (rotational_difference.angle() < rotational_bin_size_) {
         return true;
       }
     }
   }
   return false;
 }
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;
    std::vector<bool> other_region_matches_this(constraint_region_.size(), false);
    for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
    {
      bool some_match = false;
      //need to check against all other regions
      for(std::size_t j = 0 ; j < o.constraint_region_.size() ; ++j)
      {
        Eigen::Affine3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[j];
        if (diff.translation().norm() < margin
            && diff.rotation().isIdentity(margin)
            && constraint_region_[i]->getType() == o.constraint_region_[j]->getType()
            && fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[j]->computeVolume()) < margin)
        {
          some_match = true;
          //can't break, as need to do matches the other way as well
          other_region_matches_this[j] = true;
        }
      }
      if (!some_match)
        return false;
    }
    for (std::size_t i = 0 ; i < o.constraint_region_.size() ; ++i)
      if (!other_region_matches_this[i])
        return false;
    return true;
  }
  return false;
}
/**
 * @function setXform
 */
void KState::setXform( const Eigen::Affine3d& _t ) {
  body_pos = _t.translation();
  body_rot = _t.rotation();
}
    Eigen::Affine3d NDTFuserHMT::update(Eigen::Affine3d Tmotion, pcl::PointCloud<pcl::PointXYZ> &cloud)
    {
	if(!isInit){
	    fprintf(stderr,"NDT-FuserHMT: Call Initialize first!!\n");
	    return Tnow;
	}
	Todom = Todom * Tmotion; //we track this only for display purposes!
	double t0=0,t1=0,t2=0,t3=0,t4=0,t5=0,t6=0;
	///Set the cloud to sensor frame with respect to base
	lslgeneric::transformPointCloudInPlace(sensor_pose, cloud);
	t0 = getDoubleTime();
	///Create local map
	lslgeneric::NDTMap ndlocal(new lslgeneric::LazyGrid(resolution));
	ndlocal.guessSize(0,0,0,sensor_range,sensor_range,map_size_z);
	ndlocal.loadPointCloud(cloud,sensor_range);
	ndlocal.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
	//pass through ndlocal and set all cells with vertically pointing normals to non-gaussian :-O
	/*SpatialIndex *index = ndlocal.getMyIndex();
	  typename SpatialIndexctorItr it = index->begin();
	  while (it != index->end())
	  {
	  NDTCell *cell = dynamic_cast<NDTCell*> (*it);
	  if(cell!=NULL)
	  {
	  if(cell->hasGaussian_)
	  {
	  if(cell->getClass() == NDTCell::HORIZONTAL) {
	  cell->hasGaussian_ = false;
	  }
	  }
	  }
	  it++;
	  }*/

	t1 = getDoubleTime();
	Eigen::Affine3d Tinit = Tnow * Tmotion;
	if(disableRegistration) {
	    Tnow = Tinit;
	    lslgeneric::transformPointCloudInPlace(Tnow, cloud);
	    Eigen::Affine3d spose = Tnow*sensor_pose;
	    map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 25, 2*map_size_z, 0.06);
	    if(visualize) //&&ctr%20==0) 
	    {
#ifndef NO_NDT_VIZ
		if(ctr%50==0) {

		    viewer->plotNDTSAccordingToOccupancy(-1,map); 
		    //viewer->plotLocalNDTMap(cloud,resolution); 
		}
		viewer->addTrajectoryPoint(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+0.2,0,1,0);
		viewer->addTrajectoryPoint(Todom.translation()(0),Todom.translation()(1),Todom.translation()(2)+0.2,0.5,0,0.5);
		viewer->displayTrajectory();
		viewer->setCameraPointing(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+3);
		viewer->repaint();	
#endif
	    }
	    ctr++;
	    return Tnow;
	}

	if(doMultires) {
	    //create two ndt maps with resolution = 3*resolution (or 5?)
	    lslgeneric::NDTMap ndlocalLow(new lslgeneric::LazyGrid(3*resolution));
	    ndlocalLow.guessSize(0,0,0,sensor_range,sensor_range,map_size_z);
	    ndlocalLow.loadPointCloud(cloud,sensor_range);
	    ndlocalLow.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);

	    lslgeneric::NDTMap mapLow(new lslgeneric::LazyGrid(3*resolution));
	    //add distros
	    double cx,cy,cz;
	    if(!map->getCentroid(cx, cy, cz)){
		fprintf(stderr,"Centroid NOT Given-abort!\n");
	    }
	    mapLow.initialize(cx,cy,cz,3*map_size_x,3*map_size_y,map_size_z);

	    std::vector<lslgeneric::NDTCell*> ndts;
	    ndts = map->getAllCells(); //this copies cells?

	    for(int i=0; i<ndts.size(); i++)	
	    {
		NDTCell *cell = ndts[i];
		if(cell!=NULL)
		{
		    if(cell->hasGaussian_)
		    {
			Eigen::Vector3d m = cell->getMean();	
			Eigen::Matrix3d cov = cell->getCov();
			unsigned int nump = cell->getN();
			mapLow.addDistributionToCell(cov, m,nump);
		    }
		}
		delete cell;
	    }
	    //do match
	    if(matcher2D.match( mapLow, ndlocalLow,Tinit,true)){
		//if success, set Tmotion to result
		t2 = getDoubleTime();
		//std::cout<<"success: new initial guess! t= "<<t2-t1<<std::endl;
	    } else {
		Tinit = Tnow * Tmotion;
	    }	    
	}

	if(be2D) {
	    t2 = getDoubleTime();
	    if(matcher2D.match( *map, ndlocal,Tinit,true) || fuseIncomplete){
		t3 = getDoubleTime();
		Eigen::Affine3d diff = (Tnow * Tmotion).inverse() * Tinit;
		if((diff.translation().norm() > max_translation_norm || 
			    diff.rotation().eulerAngles(0,1,2).norm() > max_rotation_norm) && checkConsistency){
		    fprintf(stderr,"****  NDTFuserHMT -- ALMOST DEFINATELY A REGISTRATION FAILURE *****\n");
		    Tnow = Tnow * Tmotion;
		}else{
		    Tnow = Tinit;
		    lslgeneric::transformPointCloudInPlace(Tnow, cloud);
		    Eigen::Affine3d spose = Tnow*sensor_pose;
		    Eigen::Affine3d diff_fuse = Tlast_fuse.inverse()*Tnow;
		    if(diff_fuse.translation().norm() > translation_fuse_delta ||
			    diff_fuse.rotation().eulerAngles(0,1,2).norm() > rotation_fuse_delta)
		    {
			//std::cout<<"F: "<<spose.translation().transpose()<<" "<<spose.rotation().eulerAngles(0,1,2).transpose()<<std::endl;
			t4 = getDoubleTime();
			//TSV: originally this!
			//map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 1250, map_size_z/2, 0.06);
			map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 25, 2*map_size_z, 0.06);
			t5 = getDoubleTime();
			//map->addPointCloud(spose.translation(),cloud, 0.06, 25);
			//map->computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE, 1e5, 255, spose.translation(), 0.1);
			//t4 = getDoubleTime();
			//std::cout<<"match: "<<t3-t2<<" addPointCloud: "<<t5-t4<<" ndlocal "<<t1-t0<<" total: "<<t5-t0<<std::endl;
			Tlast_fuse = Tnow;
			if(visualize) //&&ctr%20==0) 
			{
#ifndef NO_NDT_VIZ
			    if(ctr%30==0) {
				viewer->plotNDTSAccordingToOccupancy(-1,map); 
				//viewer->plotLocalNDTMap(cloud,resolution); 
			    }
			    viewer->addTrajectoryPoint(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+0.2,0,1,0);
			    viewer->addTrajectoryPoint(Todom.translation()(0),Todom.translation()(1),Todom.translation()(2)+0.2,0.5,0,0.5);
			    viewer->displayTrajectory();
			    viewer->setCameraPointing(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+3);
			    viewer->repaint();
			    //viewer->win3D->process_events();
#endif
            }

			ctr++;
		    }
		}
	    }else{
		t3 = getDoubleTime();
		Tnow = Tnow * Tmotion;
	    }

	}
	else
	{

	    t2 = getDoubleTime();
	    if(matcher.match( *map, ndlocal,Tinit,true) || fuseIncomplete){
		t3 = getDoubleTime();
		Eigen::Affine3d diff = (Tnow * Tmotion).inverse() * Tinit;

		if((diff.translation().norm() > max_translation_norm || 
			    diff.rotation().eulerAngles(0,1,2).norm() > max_rotation_norm) && checkConsistency){
		    fprintf(stderr,"****  NDTFuserHMT -- ALMOST DEFINATELY A REGISTRATION FAILURE *****\n");
		    Tnow = Tnow * Tmotion;
		    //save offending map:
		    //map->writeToJFF("map.jff");
		    //ndlocal.writeToJFF("local.jff");
		}else{
		    Tnow = Tinit;
		    //Tnow = Tnow * Tmotion;
		    lslgeneric::transformPointCloudInPlace(Tnow, cloud);
		    Eigen::Affine3d spose = Tnow*sensor_pose;
		    Eigen::Affine3d diff_fuse = Tlast_fuse.inverse()*Tnow;
		    if(diff_fuse.translation().norm() > translation_fuse_delta ||
			    diff_fuse.rotation().eulerAngles(0,1,2).norm() > rotation_fuse_delta)
		    {
			//std::cout<<"F: "<<spose.translation().transpose()<<" "<<spose.rotation().eulerAngles(0,1,2).transpose()<<std::endl;
			t4 = getDoubleTime();
			map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 1250, map_size_z/2, 0.06);
			t5 = getDoubleTime();
			//map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 25, 2*map_size_z, 0.06);
			//map->addPointCloud(spose.translation(),cloud, 0.06, 25);
			//map->computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE, 1e5, 255, spose.translation(), 0.1);
			//t4 = getDoubleTime();
			//std::cout<<"match: "<<t3-t2<<" addPointCloud: "<<t5-t4<<" ndlocal "<<t1-t0<<" total: "<<t5-t0<<std::endl;
			Tlast_fuse = Tnow;
			if(visualize) //&&ctr%20==0) 
			{
#ifndef NO_NDT_VIZ
			    if(ctr%2==0) {
				viewer->plotNDTSAccordingToOccupancy(-1,map); 
				//viewer->plotLocalNDTMap(cloud,resolution); 
			    }
			    viewer->addTrajectoryPoint(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+0.2,0,1,0);
			    viewer->addTrajectoryPoint(Todom.translation()(0),Todom.translation()(1),Todom.translation()(2)+0.2,0.5,0,0.5);
			    viewer->displayTrajectory();
			    viewer->setCameraPointing(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+3);
			    viewer->repaint();
                viewer->win3D->process_events();
#endif
            }
			ctr++;
		    }
		}
	    }else{
		t3 = getDoubleTime();
		Tnow = Tnow * Tmotion;
	    }
	}

	t6 = getDoubleTime();
	if(fAddTimes!=NULL) {
	    fprintf(fAddTimes,"%lf %lf %lf\n",t3-t2,t5-t4,t6-t0);
	    fflush(fAddTimes);
	}

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