void ScColorEngine::getShadeColorCMYK(const ScColor& color, const ScribusDoc* doc, 
										  CMYKColor& cmyk, double level)
{
	if (color.getColorModel() == colorModelRGB)
	{
		RGBColor rgb;
		getShadeColorRGB(color, doc, rgb, level);
		ScColor tmpR(rgb.r, rgb.g, rgb.b);
		getCMYKValues(tmpR, doc, cmyk);
	}
	else if (color.getColorModel() == colorModelCMYK)
	{
		cmyk.c = qRound(color.CR * level / 100.0);
		cmyk.m = qRound(color.MG * level / 100.0);
		cmyk.y = qRound(color.YB * level / 100.0);
		cmyk.k = qRound(color.K * level / 100.0);
	}
	else if (color.getColorModel() == colorModelLab)
	{
		ScColorTransform trans = doc ? doc->stdLabToCMYKTrans : ScCore->defaultLabToCMYKTrans;
		double inC[3];
		inC[0] = color.L_val * (level / 100.0);
		inC[1] = color.a_val;
		inC[2] = color.b_val;
		quint16 outC[4];
		trans.apply(inC, outC, 1);
		cmyk.c = outC[0] / 257;
		cmyk.m = outC[1] / 257;
		cmyk.y = outC[2] / 257;
		cmyk.k = outC[3] / 257;
	}
}
Exemple #2
0
returnValue OCP::minimizeLSQ( const Matrix&S, const Function &h, const Vector &r ){

    MatrixVariablesGrid tmpS(S);
    VariablesGrid       tmpR(r);

    return objective.addLSQ( &tmpS, h, &tmpR );
}
Exemple #3
0
returnValue OCP::minimizeLSQ( const Matrix&S, const Function &h, const Vector &r ){

	if ( S.isPositiveSemiDefinite() == BT_FALSE )
		return ACADOERROR( RET_NONPOSITIVE_WEIGHT );

    MatrixVariablesGrid tmpS(S);
    VariablesGrid       tmpR(r);

    return objective.addLSQ( &tmpS, h, &tmpR );
}
Exemple #4
0
void ScColorEngine::getShadeColorCMYK(const ScColor& color, const ScribusDoc* doc, 
										  CMYKColor& cmyk, double level)
{
	if (color.getColorModel() == colorModelRGB)
	{
		RGBColor rgb;
		getShadeColorRGB(color, doc, rgb, level);
		ScColor tmpR(rgb.r, rgb.g, rgb.b);
		getCMYKValues(tmpR, doc, cmyk);
	}
	else
	{
		cmyk.c = qRound(color.CR * level / 100.0);
		cmyk.m = qRound(color.MG * level / 100.0);
		cmyk.y = qRound(color.YB * level / 100.0);
		cmyk.k = qRound(color.K * level / 100.0);
	}
}
void ScColorEngine::getShadeColorRGB(const ScColor& color, const ScribusDoc* doc, RGBColor& rgb, double level)
{
	if (color.getColorModel() == colorModelCMYK)
	{
		CMYKColor cmyk;
		getShadeColorCMYK(color, doc, cmyk, level);
		ScColor tmpC(cmyk.c, cmyk.m, cmyk.y, cmyk.k);
		getRGBValues(tmpC, doc, rgb);
	}
	else if (color.getColorModel() == colorModelRGB)
	{
		int h, s, v, snew, vnew;
		QColor tmpR(color.CR, color.MG, color.YB);
		tmpR.getHsv(&h, &s, &v);
		snew = qRound(s * level / 100.0);
		vnew = 255 - qRound(((255 - v) * level / 100.0));
		tmpR.setHsv(h, snew, vnew);
		tmpR.getRgb(&rgb.r, &rgb.g, &rgb.b);
		//We could also compute rgb shade using rgb directly
		/*rgb.CR = 255 - ((255 - color.CR) * level / 100);
		rgb.MG = 255 - ((255 - color.MG) * level / 100);
		rgb.YB = 255 - ((255 - color.YB) * level / 100);*/
	}
	else if (color.getColorModel() == colorModelLab)
	{
		ScColorTransform trans = doc ? doc->stdLabToRGBTrans : ScCore->defaultLabToRGBTrans;
		double inC[3];
		inC[0] = color.L_val * (level / 100.0);
		inC[1] = color.a_val;
		inC[2] = color.b_val;
		quint16 outC[3];
		trans.apply(inC, outC, 1);
		rgb.r = outC[0] / 257;
		rgb.g = outC[1] / 257;
		rgb.b = outC[2] / 257;
	}
}
Exemple #6
0
void ScColorEngine::getShadeColorRGB(const ScColor& color, const ScribusDoc* doc, RGBColor& rgb, double level)
{
	if (color.getColorModel() == colorModelCMYK)
	{
		CMYKColor cmyk;
		getShadeColorCMYK(color, doc, cmyk, level);
		ScColor tmpC(cmyk.c, cmyk.m, cmyk.y, cmyk.k);
		getRGBValues(tmpC, doc, rgb);
	}
	else
	{
		int h, s, v, snew, vnew;
		QColor tmpR(color.CR, color.MG, color.YB);
		tmpR.getHsv(&h, &s, &v);
		snew = qRound(s * level / 100.0);
		vnew = 255 - qRound(((255 - v) * level / 100.0));
		tmpR.setHsv(h, snew, vnew);
		tmpR.getRgb(&rgb.r, &rgb.g, &rgb.b);
		//We could also compute rgb shade using rgb directly
		/*rgb.CR = 255 - ((255 - color.CR) * level / 100);
		rgb.MG = 255 - ((255 - color.MG) * level / 100);
		rgb.YB = 255 - ((255 - color.YB) * level / 100);*/
	}
}
RTC::ReturnCode_t ReferenceForceUpdater::onExecute(RTC::UniqueId ec_id)
{
  loop ++;

  // check dataport input
  for (unsigned int i=0; i<m_forceIn.size(); i++){
    if ( m_forceIn[i]->isNew() ) {
      m_forceIn[i]->read();
    }
    if ( m_ref_forceIn[i]->isNew() ) {
      m_ref_forceIn[i]->read();
    }
  }
  if (m_basePosIn.isNew()) {
    m_basePosIn.read();
  }
  if (m_baseRpyIn.isNew()) {
    m_baseRpyIn.read();
  }
  if (m_rpyIn.isNew()) {
    m_rpyIn.read();
  }
  if (m_qRefIn.isNew()) {
    m_qRefIn.read();
  }

  //syncronize m_robot to the real robot
  if ( m_qRef.data.length() ==  m_robot->numJoints() ) {
    Guard guard(m_mutex);

    // Interpolator
    for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
      std::string arm = itr->first;
      size_t arm_idx = ee_index_map[arm];
      bool transition_interpolator_isEmpty = transition_interpolator[arm]->isEmpty();
      if ( ! transition_interpolator_isEmpty ) {
        transition_interpolator[arm]->get(&transition_interpolator_ratio[arm_idx], true);
        if ( transition_interpolator[arm]->isEmpty() && m_RFUParam[arm].is_active && m_RFUParam[arm].is_stopping ) {
          std::cerr << "[" << m_profile.instance_name << "] ReferenceForceUpdater active => inactive." << std::endl;
          m_RFUParam[arm].is_active = false;
          m_RFUParam[arm].is_stopping = false;
        }
      }
    }
    // If RFU is not active
    {
      bool all_arm_is_not_active = true;
      for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
        std::string arm = itr->first;
        size_t arm_idx = ee_index_map[arm];
        if ( m_RFUParam[arm].is_active ) all_arm_is_not_active = false;
        else for (unsigned int j=0; j<3; j++ ) ref_force[arm_idx](j) = m_ref_force_in[arm_idx].data[j];
      }
      //determin ref_force_out from ref_force_in
      if ( all_arm_is_not_active ) {
        for (unsigned int i=0; i<m_ref_force_in.size(); i++ ){
          for (unsigned int j=0; j<6; j++ ) {
            m_ref_force_out[i].data[j] = m_ref_force_in[i].data[j];
          }
          m_ref_force_out[i].tm = m_ref_force_in[i].tm;
          m_ref_forceOut[i]->write();
        }
        return RTC::RTC_OK;
      }
    }

    // If RFU is active
    {
      hrp::dvector qorg(m_robot->numJoints());

      // reference model
      for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
        qorg[i] = m_robot->joint(i)->q;
        m_robot->joint(i)->q = m_qRef.data[i];
      }
      m_robot->rootLink()->p = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z);
      m_robot->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
      m_robot->calcForwardKinematics();
      if ( (ee_map.find("rleg") != ee_map.end() && ee_map.find("lleg") != ee_map.end()) // if legged robot
           && !use_sh_base_pos_rpy ) {
        // TODO
        //  Tempolarily modify root coords to fix foot pos rot
        //  This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272

        // get current foot mid pos + rot
        std::vector<hrp::Vector3> foot_pos;
        std::vector<hrp::Matrix33> foot_rot;
        std::vector<std::string> leg_names;
        leg_names.push_back("rleg");
        leg_names.push_back("lleg");
        for (size_t i = 0; i < leg_names.size(); i++) {
          hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name);
          foot_pos.push_back(target_link->p + target_link->R * ee_map[leg_names[i]].localPos);
          foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR);
        }
        hrp::Vector3 current_foot_mid_pos ((foot_pos[0]+foot_pos[1])/2.0);
        hrp::Matrix33 current_foot_mid_rot;
        rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]);
        // calculate fix pos + rot
        hrp::Vector3 new_foot_mid_pos(current_foot_mid_pos);
        hrp::Matrix33 new_foot_mid_rot;
        {
          hrp::Vector3 ex = hrp::Vector3::UnitX();
          hrp::Vector3 ez = hrp::Vector3::UnitZ();
          hrp::Vector3 xv1 (current_foot_mid_rot * ex);
          xv1(2) = 0.0;
          xv1.normalize();
          hrp::Vector3 yv1(ez.cross(xv1));
          new_foot_mid_rot(0,0) = xv1(0); new_foot_mid_rot(1,0) = xv1(1); new_foot_mid_rot(2,0) = xv1(2);
          new_foot_mid_rot(0,1) = yv1(0); new_foot_mid_rot(1,1) = yv1(1); new_foot_mid_rot(2,1) = yv1(2);
          new_foot_mid_rot(0,2) = ez(0); new_foot_mid_rot(1,2) = ez(1); new_foot_mid_rot(2,2) = ez(2);
        }
        // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
        hrp::Matrix33 tmpR (new_foot_mid_rot * current_foot_mid_rot.transpose());
        m_robot->rootLink()->p = new_foot_mid_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
        rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
        m_robot->calcForwardKinematics();
      }
    }

    for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
      // Update reference force
      hrp::Vector3 internal_force = hrp::Vector3::Zero();
      std::string arm = itr->first;
      size_t arm_idx = ee_index_map[arm];
      double interpolation_time = 0;
      bool is_active = m_RFUParam[arm].is_active;
      if ( is_active && loop % m_RFUParam[arm].update_count == 0 ) {
        hrp::Link* target_link = m_robot->link(ee_map[arm].target_name);
        hrp::Vector3 abs_motion_dir, tmp_act_force, df;
        hrp::Matrix33 ee_rot, sensor_rot;
        ee_rot = target_link->R * ee_map[arm].localR;
        if ( m_RFUParam[arm].frame=="local" )
            abs_motion_dir = ee_rot * m_RFUParam[arm].motion_dir;
        else {
            hrp::Matrix33 current_foot_mid_rot;
            std::vector<hrp::Matrix33> foot_rot;
            std::vector<std::string> leg_names;
            leg_names.push_back("rleg");
            leg_names.push_back("lleg");
            for (size_t i = 0; i < leg_names.size(); i++) {
                hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name);
                foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR);
            }
            rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]);
            abs_motion_dir = current_foot_mid_rot * m_RFUParam[arm].motion_dir;
        }
        for (size_t i = 0; i < 3; i++ ) tmp_act_force(i) = m_force[arm_idx].data[i];
        hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::FORCE, arm_idx);
        sensor_rot = sensor->link->R * sensor->localR;
        tmp_act_force = sensor_rot * tmp_act_force;
        // Calc abs force diff
        df = tmp_act_force - ref_force[arm_idx];
        double inner_product = 0;
        if ( ! std::fabs((abs_motion_dir.norm() - 0.0)) < 1e-5 ) {
          abs_motion_dir.normalize();
          inner_product = df.dot(abs_motion_dir);
          if ( ! (inner_product < 0 && ref_force[arm_idx].dot(abs_motion_dir) < 0.0) ) {
            hrp::Vector3 in_f = ee_rot * internal_force;
            ref_force[arm_idx] = ref_force[arm_idx].dot(abs_motion_dir) * abs_motion_dir + in_f + (m_RFUParam[arm].p_gain * inner_product * transition_interpolator_ratio[arm_idx]) * abs_motion_dir;
            interpolation_time = (1/m_RFUParam[arm].update_freq) * m_RFUParam[arm].update_time_ratio;
            if ( ref_force_interpolator[arm]->isEmpty() ) {
              ref_force_interpolator[arm]->setGoal(ref_force[arm_idx].data(), interpolation_time, true);
            }
          }
        }
        if ( DEBUGP ) {
          std::cerr << "[" << m_profile.instance_name << "] Updating reference force" << std::endl;
          std::cerr << "[" << m_profile.instance_name << "]   inner_product = " << inner_product << ", ref_force = " << ref_force[arm_idx].dot(abs_motion_dir) << ", interpolation_time = " << interpolation_time << "[s]" << std::endl;
          std::cerr << "[" << m_profile.instance_name << "]   new ref_force = " << ref_force[arm_idx].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
          std::cerr << "[" << m_profile.instance_name << "]   act_force = " << tmp_act_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
          std::cerr << "[" << m_profile.instance_name << "]   df = " << df.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
        }
      }
      if (!ref_force_interpolator[arm]->isEmpty()) {
        ref_force_interpolator[arm]->get(ref_force[arm_idx].data(), true);
      }
    }
  }

  //determin ref_force_out from ref_force_in
  for (unsigned int i=0; i<m_ref_force_in.size(); i++ ){
    for (unsigned int j=0; j<6; j++ ) {
      m_ref_force_out[i].data[j] = m_ref_force_in[i].data[j];
    }
    for (unsigned int j=0; j<3; j++ ) {
      m_ref_force_out[i].data[j] = ref_force[i](j) * transition_interpolator_ratio[i] + m_ref_force_in[i].data[j] * (1-transition_interpolator_ratio[i]);
    }
    m_ref_force_out[i].tm = m_ref_force_in[i].tm;
    m_ref_forceOut[i]->write();
  }

  return RTC::RTC_OK;
}