示例#1
0
	/**
	 * transpose the matrix
	 */
	Matrix<N, M> transposed(void) const {
#ifdef CONFIG_ARCH_ARM
		Matrix<N, M> res;
		arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
		return res;
#else
		Eigen::Matrix<float, N, M, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, N, M, Eigen::RowMajor> >
				(this->arm_mat.pData);
		Me.transposeInPlace();
		Matrix<N, M> res(Me.data());
		return res;
#endif
	}
  void setGains(const hrl_pr2_force_ctrl::HybridCartesianGains::ConstPtr &msg) // khawkins
  {
    
    // Store gains...
    if (msg->p_gains.size() == 6)
      for (size_t i = 0; i < 6; ++i)
        Kp[i] = msg->p_gains[i];
    if (msg->d_gains.size() == 6)
      for (size_t i = 0; i < 6; ++i)
        Kd[i] = msg->d_gains[i];

    // Store force gains... khawkins
    if (msg->fp_gains.size() == 6)
      for (size_t i = 0; i < 6; ++i)
        Kfp[i] = msg->fp_gains[i];
    if (msg->fi_gains.size() == 6)
      for (size_t i = 0; i < 6; ++i)
        Kfi[i] = msg->fi_gains[i];
    if (msg->fi_max_gains.size() == 6)
      for (size_t i = 0; i < 6; ++i)
        Kfi_max[i] = msg->fi_max_gains[i];

    // Store selector matricies khawkins
    if (msg->force_selector.size() == 6)
        for (size_t i = 0; i < msg->force_selector.size(); ++i)
            if(msg->force_selector[i]) {
                force_selector[i] = 1;
                motion_selector[i] = 0;
            } else {
                force_selector[i] = 0;
                motion_selector[i] = 1;
            }

    // Store frame information
    if(!msg->header.frame_id.compare(root_name_))
    {
      use_tip_frame_ = false;
      ROS_INFO("New gains in root frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
               root_name_.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
      St.setIdentity();
    }
    else if(!msg->header.frame_id.compare(tip_name_))
    {
      use_tip_frame_ = true;
      ROS_INFO("New gains in tip frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
               tip_name_.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
      
    }
    else
    {
      use_tip_frame_ = false;
      
      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);

      St << 
          t(0,0),t(0,1),t(0,2),0,0,0,
          t(1,0),t(1,1),t(1,2),0,0,0,
          t(2,0),t(2,1),t(2,2),0,0,0,
          0,0,0,t(0,0),t(0,1),t(0,2),
          0,0,0,t(1,0),t(1,1),t(1,2),
          0,0,0,t(2,0),t(2,1),t(2,2);
    
      St.transposeInPlace();
  
      ROS_INFO("New gains in arbitrary frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
             msg->header.frame_id.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
    }
  }