void
save_localize_transform()
{
    std::ofstream filed_transform;
	std::stringstream file_name;

	std::string path = ros::package::getPath("prx_localizer");
	file_name << path << "/transform/localize_transform.bin";
    filed_transform.open(file_name.str().c_str(), std::ofstream::binary|std::ofstream::trunc);

    tfScalar x, y, z, w;
    x = g_localize_transform.getRotation().getX();
    y = g_localize_transform.getRotation().getY();
    z = g_localize_transform.getRotation().getZ();
    w = g_localize_transform.getRotation().getW();
    filed_transform.write((char *) &x, sizeof(tfScalar));
    filed_transform.write((char *) &y, sizeof(tfScalar));
    filed_transform.write((char *) &z, sizeof(tfScalar));
    filed_transform.write((char *) &w, sizeof(tfScalar));

    x = g_localize_transform.getOrigin().getX();
    y = g_localize_transform.getOrigin().getY();
    z = g_localize_transform.getOrigin().getZ();
    filed_transform.write((char *) &x, sizeof(tfScalar));
    filed_transform.write((char *) &y, sizeof(tfScalar));
    filed_transform.write((char *) &z, sizeof(tfScalar));

    filed_transform.close();
}
Пример #2
0
inline tf::Transform interpolateTF(tf::Transform start, tf::Transform end, double ratio)
{
	tf::Vector3 lerp_pos = start.getOrigin().lerp(end.getOrigin(), ratio);
	tf::Quaternion lerp_rot = start.getRotation().slerp(end.getRotation(), ratio);
	
	return tf::Transform(lerp_rot, lerp_pos);
}
Пример #3
0
void Match_Filter::scanCB(const sensor_msgs::LaserScan::ConstPtr& scan){		
	sensor_msgs::PointCloud2 cloud_in,cloud_out;
	projector_.transformLaserScanToPointCloud("/lidar_link",*scan,cloud_in,tfListener_);
	rot_tf.header.frame_id = "/ChestLidar";
//	lidar mx28 axis aligned mode
//	rot_tf.transform.rotation.x = enc_tf_.getRotation().x();
//	rot_tf.transform.rotation.y = enc_tf_.getRotation().y();
//	rot_tf.transform.rotation.z = enc_tf_.getRotation().z();
//	rot_tf.transform.rotation.w = enc_tf_.getRotation().w();

//	lidar mx28 axis perpendicular modeg
	tf::Quaternion q1;
	q1.setRPY(-M_PI/2,0,0);
	tf::Transform m1(q1);
	tf::Quaternion q2(enc_tf_.getRotation().x(),enc_tf_.getRotation().y(),enc_tf_.getRotation().z(),enc_tf_.getRotation().w());
	tf::Transform m2(q2);

	tf::Transform m4;
	m4 = m2*m1; // rotate lidar axis and revolute mx28 axis
	rot_tf.transform.rotation.x = m4.getRotation().x();
	rot_tf.transform.rotation.y = m4.getRotation().y();
	rot_tf.transform.rotation.z = m4.getRotation().z();
	rot_tf.transform.rotation.w = m4.getRotation().w();
	
	tf2::doTransform(cloud_in,cloud_out,rot_tf);
	point_cloud_pub_.publish(cloud_out);		
	moving_now.data = dxl_ismove_;
	dxl_ismove_pub_.publish(moving_now);
}
Пример #4
0
void
fromTF(const tf::Transform &source, Eigen::Matrix4f &dest, geometry_msgs::Pose &pose_dest)
{
    Eigen::Quaternionf q(source.getRotation().getW(),source.getRotation().getX(), source.getRotation().getY(),source.getRotation().getZ());
    q.normalize();
    Eigen::Vector3f t(source.getOrigin().x(), source.getOrigin().y(), source.getOrigin().z());
    Eigen::Matrix3f R(q.toRotationMatrix());
    dest(0,0) = R(0,0);
    dest(0,1) = R(0,1);
    dest(0,2) = R(0,2);
    dest(1,0) = R(1,0);
    dest(1,1) = R(1,1);
    dest(1,2) = R(1,2);
    dest(2,0) = R(2,0);
    dest(2,1) = R(2,1);
    dest(2,2) = R(2,2);
    dest(3,0) = dest(3,1)= dest(3,2) = 0;
    dest(3,3) = 1;
    dest(0,3) = t(0);
    dest(1,3) = t(1);
    dest(2,3) = t(2);
    pose_dest.orientation.x = q.x();
    pose_dest.orientation.y = q.y();
    pose_dest.orientation.z = q.z();
    pose_dest.orientation.w = q.w();
    pose_dest.position.x = t(0);
    pose_dest.position.y = t(1);
    pose_dest.position.z = t(2);
}
std::vector<double> planning_models::KinematicModel::PlanarJointModel::computeJointStateValues(const tf::Transform& transform) const 
{
  std::vector<double> ret;
  ret.push_back(transform.getOrigin().x());
  ret.push_back(transform.getOrigin().y());
  ret.push_back(transform.getRotation().getAngle()*transform.getRotation().getAxis().z());
  return ret;
}
Пример #6
0
void leatherman::btTransformToPoseMsg(const tf::Transform &bt, geometry_msgs::Pose &pose)
{
  pose.position.x = bt.getOrigin().getX();
  pose.position.y = bt.getOrigin().getY();
  pose.position.z = bt.getOrigin().getZ();
  pose.orientation.x = bt.getRotation().x();
  pose.orientation.y = bt.getRotation().y();
  pose.orientation.z = bt.getRotation().z();
  pose.orientation.w = bt.getRotation().w();
}
Пример #7
0
void printTransform(tf::Transform& t){
	for(int i=0; i<3; i++){
		std::cout << t.getOrigin()[i] << "\t";
	}
	std::cout << t.getRotation()[3] << "\t";
	for(int i=0; i<3; i++){
		std::cout << t.getRotation()[i] << "\t";
	}
	std::cout << std::endl;
}
Пример #8
0
void
fromTF(const tf::Transform &source, geometry_msgs::Pose &dest)
{
    dest.orientation.x = source.getRotation().getX();
    dest.orientation.y = source.getRotation().getY();
    dest.orientation.z = source.getRotation().getZ();
    dest.orientation.w = source.getRotation().getW();
    dest.position.x = source.getOrigin().x();
    dest.position.y = source.getOrigin().y();
    dest.position.z = source.getOrigin().z();
}
Пример #9
0
    //FIXME
    //calculate the absolute orientation of the object
    //
    float getOrient(tf::Transform & pose1, tf::Transform & pose2)
    {
      float orient;

      /* The orientation is the angle of (x-axis) rotation about the z-axis,
       *  under the assumption that z-axis does not rotate on x or y */
      orient = acos(
          sqrt(pow(tf::Matrix3x3(pose1.getRotation()).getColumn(0).getX(),2) + pow(tf::Matrix3x3(pose1.getRotation()).getColumn(0).getY(),2)) /
          sqrt(pow(tf::Matrix3x3(pose2.getRotation()).getColumn(0).getX(),2) + pow(tf::Matrix3x3(pose2.getRotation()).getColumn(0).getY(),2))  );

      return orient;
    }
Пример #10
0
geometry_msgs::PoseWithCovarianceStamped
tfToPoseCovarianceStamped (const tf::Transform &pose)
{
	geometry_msgs::PoseWithCovarianceStamped pocv;
	pocv.pose.pose.position.x = pose.getOrigin().x();
	pocv.pose.pose.position.y = pose.getOrigin().y();
	pocv.pose.pose.position.z = pose.getOrigin().z();
	pocv.pose.pose.orientation.x = pose.getRotation().x();
	pocv.pose.pose.orientation.y = pose.getRotation().y();
	pocv.pose.pose.orientation.z = pose.getRotation().z();
	pocv.pose.pose.orientation.w = pose.getRotation().w();
	return pocv;
}
Пример #11
0
 void tfToPose(tf::Transform &trans, geometry_msgs::PoseStamped &msg)
 {
   tf::quaternionTFToMsg(trans.getRotation(), msg.pose.orientation);
   msg.pose.position.x = trans.getOrigin().x();
   msg.pose.position.y = trans.getOrigin().y();
   msg.pose.position.z = trans.getOrigin().z();
 }
bool computeMatrix(PointCloud::Ptr target,
                   PointCloud::Ptr world,
                   std::string target_name,
                   std::string world_name,
                   const bool broadcast)
{
    if ((!world_name.empty()) && (!target_name.empty()) &&
            (target->points.size() > 2) && (world->points.size() == target->points.size()))
    {
        Eigen::Matrix4f trMatrix;
        pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> svd;

        svd.estimateRigidTransformation(*target, *world, trMatrix);

        ROS_INFO("Registration completed and Registration Matrix is being broadcasted");

         transform=tf::Transform(tf::Matrix3x3(trMatrix(0, 0), trMatrix(0, 1), trMatrix(0, 2),
                                              trMatrix(1, 0), trMatrix(1, 1), trMatrix(1, 2),
                                              trMatrix(2, 0), trMatrix(2, 1), trMatrix(2, 2)),
                                tf::Vector3(trMatrix(0, 3), trMatrix(1, 3), trMatrix(2, 3)));

        Eigen::Vector3d origin(transform.getOrigin());
        double roll, pitch, yaw;
        tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw);
	std::cout << std::endl << "#################################################" << std::endl; 
	std::cout << std::endl << "########### TRANSFORMATION PARAMETERS ###########" << std::endl; 
	std::cout << std::endl << "#################################################" << std::endl; 
        std::cout << "origin: "<<origin.transpose() << std::endl;
        std::cout << "rpy: " << roll << " " << pitch << " " << yaw << std::endl;


    }

    return true;
}
Пример #13
0
	/**
	 * Send transform to FCU position controller
	 *
	 * Note: send only XYZ, Yaw
	 */
	void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) {
		// ENU frame
		tf::Vector3 origin = transform.getOrigin();
		tf::Quaternion q = transform.getRotation();

		/* Documentation start from bit 1 instead 0,
		 * Ignore velocity and accel vectors, yaw rate
		 */
		uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);

		if (uas->is_px4()) {
			/**
			 * Current PX4 has bug: it cuts throttle if there no velocity sp
			 * Issue #273.
			 *
			 * @todo Revesit this quirk later. Should be fixed in firmware.
			 */
			ignore_all_except_xyz_y = (1 << 11) | (7 << 6);
		}

		// ENU->NED. Issue #49.
		set_position_target_local_ned(stamp.toNSec() / 1000000,
				MAV_FRAME_LOCAL_NED,
				ignore_all_except_xyz_y,
				origin.y(), origin.x(), -origin.z(),
				0.0, 0.0, 0.0,
				0.0, 0.0, 0.0,
				tf::getYaw(q), 0.0);
	}
Пример #14
0
Affine3d eigen_from_tf(tf::Transform transform) {
    Affine3d x;
    x = Translation3d(vec2vec(transform.getOrigin()));
    tf::Quaternion q = transform.getRotation();
    x *= Quaterniond(q.w(), q.x(), q.y(), q.z());
    return x;
}
Пример #15
0
// Move the end effector (tf jaco_tool_position) to a certain position
void JacoCustom::moveToPoint(tf::Transform tf_){

    geometry_msgs::Twist arm;
    arm.linear.x = tf_.getOrigin().getX();
    arm.linear.y = tf_.getOrigin().getY();
    arm.linear.z = tf_.getOrigin().getZ();

//    double roll, pitch, yaw;
//    tf_.getBasis().getRPY(roll,pitch, yaw);
//    arm.angular.x = roll;
//    arm.angular.y = pitch;
//    arm.angular.z = yaw;



    wpi_jaco_msgs::QuaternionToEuler conv;
    geometry_msgs::Quaternion quat;
    tf::quaternionTFToMsg(tf_.getRotation(), quat);
    conv.request.orientation = quat;
    if(quaternion_to_euler_service_client.call(conv)){
        arm.angular.x = conv.response.roll;
        arm.angular.y = conv.response.pitch;
        arm.angular.z = conv.response.yaw;
    }

    moveToPoint(arm);
}
Пример #16
0
void PSMNode::publishOdom(const tf::Transform& transform,
			  const ros::Time& time,
			  const float cxx, const float cxy,
			  const float cyy, const float ctt,
			  const float dx, const float dy, const float dt) {

  nav_msgs::Odometry odom;
  tf::TransformBroadcaster odom_broadcaster;

  tf::Matrix3x3 m(transform.getRotation());
  double roll, pitch, yaw, x, y;
  m.getRPY(roll, pitch, yaw);

  x = transform.getOrigin().getX();
  y = transform.getOrigin().getY();
  
  geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(yaw);

  // there's almost certianly a better way to do this.
  // we're just republishing the existing transform as odom_topic_
  /*geometry_msgs::TransformStamped odom_trans;
  odom_trans.header.stamp = time;
  odom_trans.header.frame_id = odomTopic_;
  odom_trans.child_frame_id = "world";

  odom_trans.transform.translation.x = x;
  odom_trans.transform.translation.y = y;
  odom_trans.transform.translation.z = 0;
  odom_trans.transform.rotation = odom_quat;
  */
  //odom_broadcaster.sendTransform(odom_trans);

  odom.header.stamp = time;
  odom.header.frame_id = odomTopic_;
  odom.child_frame_id = "world";
  odom.pose.pose.position.x = x;
  odom.pose.pose.position.y = y;
  odom.pose.pose.position.z = 0.0;
  odom.pose.pose.orientation = odom_quat;


  double dtime = (time - last_time).toSec(); 
  odom.twist.twist.linear.x = dx / dtime;
  odom.twist.twist.linear.y = dy / dtime;
  odom.twist.twist.angular.z = dt / dtime;

  //cout << cxx << " " << cxy << " " << cyy << endl;
  odom.pose.covariance[0] = cxx / (ROS_TO_PM * ROS_TO_PM);
  odom.pose.covariance[1] = cxy / (ROS_TO_PM * ROS_TO_PM);
  odom.pose.covariance[6] = cxy / (ROS_TO_PM * ROS_TO_PM);
  odom.pose.covariance[7] = cyy / (ROS_TO_PM * ROS_TO_PM);
  odom.pose.covariance[35] = ctt;

  // cheat
  odom.pose.covariance[14] = 1e-6;
  odom.pose.covariance[21] = 99999999;
  odom.pose.covariance[28] = 99999999;

  odomPublisher_.publish(odom);
}
Пример #17
0
static carmen_orientation_3D_t get_carmen_orientation_from_tf_transform(tf::Transform transform)
{
	carmen_orientation_3D_t orientation;
	tf::Matrix3x3(transform.getRotation()).getEulerYPR(orientation.yaw, orientation.pitch, orientation.roll);
	
	return orientation;
}
void planning_models::KinematicState::printTransform(const std::string &st, const tf::Transform &t, std::ostream &out) const
{
  out << st << std::endl;
  const tf::Vector3 &v = t.getOrigin();
  out << "  origin: " << v.x() << ", " << v.y() << ", " << v.z() << std::endl;
  const tf::Quaternion &q = t.getRotation();
  out << "  quaternion: " << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << std::endl;
}
Пример #19
0
geometry_msgs::Pose transformToPose(tf::Transform &trans)
{
    geometry_msgs::Pose pose;
    pose.position.x = trans.getOrigin().x();
    pose.position.y = trans.getOrigin().y();
    pose.position.z = trans.getOrigin().z();
    tf::quaternionTFToMsg(trans.getRotation(), pose.orientation);
    return pose;
}
Пример #20
0
void PSMNode::tfToPose2D(const tf::Transform& t, geometry_msgs::Pose2D& pose)
{
  tf::Matrix3x3 m(t.getRotation());
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);

  pose.x = t.getOrigin().getX();
  pose.y = t.getOrigin().getY();
  pose.theta = yaw;
}
Пример #21
0
bool LaserScanMatcher::newKeyframeNeeded(const tf::Transform& d)
{
  if (fabs(tf::getYaw(d.getRotation())) > kf_dist_angular_) return true;

  double x = d.getOrigin().getX();
  double y = d.getOrigin().getY();
  if (x*x + y*y > kf_dist_linear_sq_) return true;

  return false;
}
Пример #22
0
Eigen::Matrix4f EigenFromTF(tf::Transform trans)
{
  Eigen::Matrix4f out;

  //btQuaternion quat = trans.getRotation();
  tf::Quaternion quat( trans.getRotation()[0], trans.getRotation()[1],
					 trans.getRotation()[2], trans.getRotation()[3] );

  //btVector3 origin = trans.getOrigin();
  tf::Vector3 origin( trans.getOrigin()[0], trans.getOrigin()[1], trans.getOrigin()[2]);
  
  Eigen::Quaternionf quat_out(quat.w(), quat.x(), quat.y(), quat.z());
  Eigen::Vector3f origin_out(origin.x(), origin.y(), origin.z());
  
  out.topLeftCorner<3,3>() = quat_out.toRotationMatrix();
  out.topRightCorner<3,1>() = origin_out;
  out(3,3) = 1;
  
  return out;
}
int main(int argc, char** argv)
{
  ros::init(argc, argv, "displacement");

  ros::NodeHandle node;
  ros::Subscriber sub1 = node.subscribe( "turtle1/pose", 10, &poseCallback1 );
  ros::Subscriber sub2 = node.subscribe( "turtle2/pose", 10, &poseCallback2 );
  ros::Publisher pub = node.advertise<geometry_msgs::Transform>( "displacement/transform", 1000 );

  //Set the loop at 10 HZ
  ros::Rate rate(10);

  while (ros::ok())
  {
      ros::spinOnce();//Call this function to process ROS incoming messages.

      //Calculate the transformation
      tf::Transform transformTmp;
      // transformTmp = transform1.inverse() * transform2;

      tf::Vector3 t1Origin = transform1.getOrigin();
      tf::Vector3 t2Origin = transform2.getOrigin();

      tf::Quaternion q1 = transform1.getRotation();
      tf::Quaternion q2 = transform2.getRotation();

      transformTmp.setOrigin( tf::Vector3(t2Origin.getX() - t1Origin.getX(), t2Origin.getY() - t1Origin.getY(), 0.0) );
      tf::Quaternion qtemp;
      qtemp.setRPY(0, 0, q2.getAngle() - q1.getAngle());
      transformTmp.setRotation(qtemp);

      //Publish the transformation
      geometry_msgs::Transform tg;
      tf::transformTFToMsg(transformTmp, tg);
      pub.publish(tg);

      rate.sleep();//Sleep the rest of the cycle until 10 Hz
    }

    return 0;
  }
std::vector<double> planning_models::KinematicModel::RevoluteJointModel::computeJointStateValues(const tf::Transform& transform) const
{
  std::vector<double> ret;
  ROS_DEBUG_STREAM("Transform angle is " << transform.getRotation().getAngle() 
                   << " axis x " << transform.getRotation().getAxis().x()
                   << " axis y " << transform.getRotation().getAxis().y()
                   << " axis z " << transform.getRotation().getAxis().z());
  double val = transform.getRotation().getAngle()*transform.getRotation().getAxis().dot(axis_);
  while(val < -M_PI) {
    if(val < -M_PI) {
      val += 2*M_PI;
    }
  }
  while(val > M_PI) {
    if(val > M_PI) {
      val -= 2*M_PI;
    }
  }
  ret.push_back(val);
  return ret;
}
moveit_msgs::PlaceLocation tf_transform_to_place(tf::Transform t)
{
    static int i = 0;

    moveit_msgs::PlaceLocation place;
    geometry_msgs::PoseStamped pose;

    tf::Vector3& origin = t.getOrigin();
    tf::Quaternion rotation = t.getRotation();

    tf::quaternionTFToMsg(rotation, pose.pose.orientation);

    pose.header.frame_id = "base_link";
    pose.header.stamp = ros::Time::now();
    pose.pose.position.x = origin.m_floats[0];
    pose.pose.position.y = origin.m_floats[1];
    pose.pose.position.z = origin.m_floats[2];

    place.place_pose = pose;
    double x = place.place_pose.pose.position.x;
    double y = place.place_pose.pose.position.y;
    hypo(x,y,0.1);

    place.id = boost::lexical_cast<std::string>(i);

    //place.pre_place_approach.direction.vector.x = 1.0;
    place.pre_place_approach.direction.vector.z = -1.0;
    place.pre_place_approach.direction.header.frame_id = "katana_gripper_tool_frame";
    place.pre_place_approach.min_distance = 0.07;
    place.pre_place_approach.desired_distance = 0.15;
    place.pre_place_approach.direction.header.stamp = ros::Time::now();

    place.post_place_retreat.direction.vector.x = -1.0;
    //place.post_place_retreat.direction.vector.z = 1.0;
    place.post_place_retreat.min_distance = 0.07;
    place.post_place_retreat.desired_distance = 0.15;
    place.post_place_retreat.direction.header.frame_id = "katana_gripper_tool_frame";
    place.post_place_retreat.direction.header.stamp = ros::Time::now();

    // TODO: fill in grasp.post_place_retreat (copy of post_grasp_retreat?)

    place.post_place_posture.joint_names.push_back("katana_l_finger_joint");
    place.post_place_posture.joint_names.push_back("katana_r_finger_joint");
    place.post_place_posture.points.resize(1);
    place.post_place_posture.points[0].positions.push_back(0.3);
    place.post_place_posture.points[0].positions.push_back(0.3);

    place.allowed_touch_objects.resize(1);
    place.allowed_touch_objects[0] = "testbox";

    i++;
    return place;
}
Пример #26
0
void tfToXYZRPY(
  const tf::Transform& t,
  double& x,    double& y,     double& z,
  double& roll, double& pitch, double& yaw)
{
  x = t.getOrigin().getX();
  y = t.getOrigin().getY();
  z = t.getOrigin().getZ();

  tf::Matrix3x3  m(t.getRotation());
  m.getRPY(roll, pitch, yaw);
}
Пример #27
0
void
fromTF(const tf::Transform &source, Eigen::Matrix4f &dest)
{
    Eigen::Quaternionf q(source.getRotation().getW(),source.getRotation().getX(), source.getRotation().getY(),source.getRotation().getZ());
    q.normalize();
    Eigen::Vector3f t(source.getOrigin().x(), source.getOrigin().y(), source.getOrigin().z());
    Eigen::Matrix3f R(q.toRotationMatrix());
    dest(0,0) = R(0,0);
    dest(0,1) = R(0,1);
    dest(0,2) = R(0,2);
    dest(1,0) = R(1,0);
    dest(1,1) = R(1,1);
    dest(1,2) = R(1,2);
    dest(2,0) = R(2,0);
    dest(2,1) = R(2,1);
    dest(2,2) = R(2,2);
    dest(3,0) = dest(3,1)= dest(3,2) = 0;
    dest(3,3) = 1;
    dest(0,3) = t(0);
    dest(1,3) = t(1);
    dest(2,3) = t(2);
}
void MotionEstimation::constrainMotion(tf::Transform& motion)
{
  // **** degree-of-freedom constraints

  if (motion_constraint_ == ROLL_PITCH)
  {
    tf::Quaternion q;
    q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation()));

    motion.setRotation(q); 
  }
  else if (motion_constraint_ == ROLL_PITCH_Z)
  {
    tf::Quaternion q;
    q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation()));
    
    tf::Vector3 p(motion.getOrigin().getX(), motion.getOrigin().getY(), 0.0);
    
    motion.setOrigin(p);
    motion.setRotation(q); 
  }
}
// calculates the linear and roatation speed out of given transform and duration and reverse drive information
geometry_msgs::Twist CalibrateAction::calcTwistFromTransform(tf::Transform _transform, ros::Duration _dur)
{
    geometry_msgs::Twist result_twist;
    result_twist.linear.x = (_transform.getOrigin().length() / _dur.toSec()) * ((_transform.getOrigin().getX() < 0) ? -1 : 1);
    result_twist.linear.y = 0;
    result_twist.linear.z = 0;

    result_twist.angular.x = 0;
    result_twist.angular.y = 0;
    result_twist.angular.z = (tf::getYaw(_transform.getRotation()) / _dur.toSec());

    return result_twist;
}
bool write_urdf() {
  //replace the urdf's camera pose with the calibrated pose
  double x,y,z,r,p,yaw;
  x=transform_gripper2camera.getOrigin().getX();
  y=transform_gripper2camera.getOrigin().getY();
  z=transform_gripper2camera.getOrigin().getZ();
  tf::Matrix3x3(transform_gripper2camera.getRotation()).getRPY(r,p,yaw);
  std::stringstream updated_line;
  updated_line << "                  <origin xyz=\"";
  updated_line << x << " " << y << " " << z;
  updated_line << "\" rpy=\"" << r << " " << p << " " << yaw << "\" />";
  ROS_INFO_STREAM("replacing old origin with: '" << updated_line.str() << "'");
  lines.at(whichline)=updated_line.str();

  //copy old file before writing in case something goes wrong
  ROS_INFO("Creating backup of urdf");
  std::ifstream src(urdf_location.c_str(), std::ios::binary);
  std::stringstream temp;
  temp << urdf_location << ".temp";
  std::ofstream dst(temp.str().c_str(),std::ios::binary);
  if (src.is_open() && dst.is_open()) dst << src.rdbuf();
  else {
    ROS_ERROR("Error creating backup of urdf");
    return false;
  }

  //write new urdf file with modified camera origin
  std::ofstream myfile;
  //open the file for writing, truncate because we're writing the whole thing
  myfile.open(urdf_location.c_str(), std::ios::out | std::ios::trunc);
  if (myfile.is_open()) {
    ROS_INFO("Urdf is open, writing...");
    for (std::vector<std::string>::iterator it = lines.begin(); it != lines.end(); ++it)
    {
      //write each line to the file
      if (myfile.good()) myfile<<(*it)<<std::endl;
      else
      {
        ROS_ERROR("Something went wrong with writing the urdf");
        return false;
      }
    } //end iterator
    myfile.close();
    ROS_INFO("Success");
    return true;
  } //end of 'if (myfile.is_open())'
  else {
    ROS_ERROR("Error opening the urdf file for writing");
    return false;
  }
}