Beispiel #1
0
BitcoinBlock::BitcoinBlock(const QByteArray &data, quint32 _height) {
	is_valid = true;
	height = _height;
	hash = BitcoinCrypto::doubleSha256(data.left(80));
	raw = data.left(80);
	size = data.length();
	if (data.length() > 80) {
		txns = data.mid(80);
	}
	QDataStream data_r(raw);
	data_r.setByteOrder(QDataStream::LittleEndian);
	data_r >> version;
	parent = BitcoinStream::readData(data_r, 32);
	merkle_root = BitcoinStream::readData(data_r, 32);
	data_r >> timestamp >> bits >> nonce;
}
RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id)
{
    //std::cout << "ImpedanceController::onExecute(" << ec_id << ")" << std::endl;
    loop ++;

    // check dataport input
    bool update_rpy = false;
    for (unsigned int i=0; i<m_forceIn.size(); i++){
        if ( m_forceIn[i]->isNew() ) {
            m_forceIn[i]->read();
        }
    }
    if (m_rpyIn.isNew()) {
      m_rpyIn.read();
      update_rpy = true;
    }
    if (m_qCurrentIn.isNew()) {
      m_qCurrentIn.read();
      //
      if (update_rpy) updateRootLinkPosRot(m_rpy);
      for (unsigned int i=0; i<m_forceIn.size(); i++){
        if ( m_force[i].data.length()==6 ) {
          std::string sensor_name = m_forceIn[i]->name();
          hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_name);
          hrp::Vector3 data_p(m_force[i].data[0], m_force[i].data[1], m_force[i].data[2]);
          hrp::Vector3 data_r(m_force[i].data[3], m_force[i].data[4], m_force[i].data[5]);
          if ( DEBUGP ) {
            std::cerr << "raw force : " << data_p[0] << " " << data_p[1] << " " << data_p[2] << std::endl;
            std::cerr << "raw moment : " << data_r[0] << " " << data_r[1] << " " << data_r[2] << std::endl;
          }
          hrp::Matrix33 sensorR;
          if ( sensor ) {
            // real force sensore
            sensorR = sensor->link->R * sensor->localR;
          } else if ( m_sensors.find(sensor_name) !=  m_sensors.end()) {
            // virtual force sensor
            if ( DEBUGP ) {
              //std::cerr << " targetR: " << target->R << std::endl;
              std::cerr << " sensorR: " << m_sensors[sensor_name].R << std::endl;
            }
            sensorR = m_robot->link(m_sensors[sensor_name].parent_link_name)->R * m_sensors[sensor_name].R;
          } else {
            std::cerr << "unknwon force param" << std::endl;
          }
          abs_forces[sensor_name] = sensorR * data_p;
          abs_moments[sensor_name] = sensorR * data_r;
          if ( DEBUGP ) {
            hrp::Vector3& tmpf = abs_forces[sensor_name];
            hrp::Vector3& tmpm = abs_moments[sensor_name];
            std::cerr << "world force : " << tmpf[0] << " " << tmpf[1] << " " << tmpf[2] << std::endl;
            std::cerr << "world moment : " << tmpm[0] << " " << tmpm[1] << " " << tmpm[2] << std::endl;
          }
        }
      }
    }
    if (m_qRefIn.isNew()) {
        m_qRefIn.read();
        m_q.tm = m_qRef.tm;
    }
    if ( m_qRef.data.length() ==  m_robot->numJoints() &&
         m_qCurrent.data.length() ==  m_robot->numJoints() ) {

        if ( DEBUGP ) {
            std::cerr << "qRef  : ";
            for ( int i = 0; i <  m_qRef.data.length(); i++ ){
                std::cerr << " " << m_qRef.data[i];
            }
            std::cerr << std::endl;
        }

        if ( m_impedance_param.size() == 0 ) {
          for ( int i = 0; i < m_qRef.data.length(); i++ ){
            m_q.data[i] = m_qRef.data[i];
          }
          m_qOut.write();
          return RTC_OK;
        }

        Guard guard(m_mutex);

	{
	  hrp::dvector qorg(m_robot->numJoints());
	  
	  // reference model
	  for ( int i = 0; i < m_robot->numJoints(); i++ ){
	    qorg[i] = m_robot->joint(i)->q;
            m_robot->joint(i)->q = m_qRef.data[i];
            qrefv[i] = m_qRef.data[i];
	  }
          if (m_rpyRefIn.isNew()) {
            m_rpyRefIn.read();
            //updateRootLinkPosRot(m_rpyRef);
          }
	  m_robot->calcForwardKinematics();

	  // set sequencer position to target_p0
	  for ( std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) {
            ImpedanceParam& param = it->second;
            param.target_p0 = m_robot->link(param.target_name)->p;
            param.target_r0 = m_robot->link(param.target_name)->R;
          }
          // back to impedance robot model (only for controlled joint)
	  for ( std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) {
            ImpedanceParam& param = it->second;
            for ( int j = 0; j < param.manip->numJoints(); j++ ){
              int i = param.manip->joint(j)->jointId;
              m_robot->joint(i)->q = qorg[i];
            }
	  }
          if (update_rpy) updateRootLinkPosRot(m_rpy);
	  m_robot->calcForwardKinematics();

	}

	// set m_robot to qRef when deleting status
    std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin();
	while(it != m_impedance_param.end()){
	  std::string sensor_name = it->first;
	  ImpedanceParam& param = it->second;

	  if ( param.transition_count > 0 ) {
	    hrp::JointPathExPtr manip = param.manip;
	    for ( int j = 0; j < manip->numJoints(); j++ ) {
              int i = manip->joint(j)->jointId; // index in robot model
	      hrp::Link* joint =  m_robot->joint(i);
              // transition_smooth_gain moves from 0 to 1
              // (/ (log (/ (- 1 0.99) 0.99)) 0.5)
              double transition_smooth_gain = 1/(1+exp(-9.19*(((MAX_TRANSITION_COUNT - param.transition_count) / MAX_TRANSITION_COUNT) - 0.5)));
              joint->q = ( m_qRef.data[i] - param.transition_joint_q[i] ) * transition_smooth_gain + param.transition_joint_q[i];
	    }
        param.transition_count--;
	    if(param.transition_count <= 0){ // erase impedance param
          std::cerr << "Finished cleanup and erase impedance param " << sensor_name << std::endl;
          m_impedance_param.erase(it++);
        }else{
          it++;
        }
	  } else {
	    // use impedance model

            hrp::Link* base = m_robot->link(param.base_name);
            hrp::Link* target = m_robot->link(param.target_name);
            assert(target);
            assert(base);

            param.current_p0 = target->p;
            param.current_r0 = target->R;

            hrp::JointPathExPtr manip = param.manip;
            assert(manip);
            //const int n = manip->numJoints();

            hrp::Vector3 dif_pos = hrp::Vector3(0,0,0);
            hrp::Vector3 vel_pos0 = hrp::Vector3(0,0,0);
            hrp::Vector3 vel_pos1 = hrp::Vector3(0,0,0);
            hrp::Vector3 dif_target_pos = hrp::Vector3(0,0,0);
            hrp::Vector3 dif_rot = hrp::Vector3(0,0,0);
            hrp::Vector3 vel_rot0 = hrp::Vector3(0,0,0);
            hrp::Vector3 vel_rot1 = hrp::Vector3(0,0,0);
            hrp::Vector3 dif_target_rot = hrp::Vector3(0,0,0);

            // rats/plugins/impedancecontrol.cpp
            //double M = 5, D = 100, K = 200;
            // dif_pos  = target_p0 (target_coords0) - current_p0(move_coords)
            // vel_pos0 = current_p0(move_coors) - current_p1(prev_coords0)
            // vel_pos1 = current_p1(prev_coords0) - current_p2(prev_coords1)
            // dif_target  = target_p0(target_coords0) - target_p1(target_coords1)
            //
            // current_p2(prev_coords1) = current_p1(prev_coords0)
            // currnet_p1(prev_coords0) = current_p0(move_coords) + vel_p
            // target_p1(target_coords1) = target_p0(target_coords0)

            if ( DEBUGP ) {
                std::cerr << "cur0  : " << param.current_p0[0] << " " << param.current_p0[1] << " " << param.current_p0[2] << std::endl;
                std::cerr << "cur1  : " << param.current_p1[0] << " " << param.current_p1[1] << " " << param.current_p1[2] << std::endl;
                std::cerr << "cur2  : " << param.current_p2[0] << " " << param.current_p2[1] << " " << param.current_p2[2] << std::endl;
                std::cerr << "tgt0  : " << param.target_p0[0] << " " << param.target_p0[1] << " " << param.target_p0[2] << std::endl;
                std::cerr << "tgt1  : " << param.target_p1[0] << " " << param.target_p1[1] << " " << param.target_p1[2] << std::endl;
            }
            // if ( DEBUGP ) {
            //     std::cerr << "cur0  : " << param.current_r0[0] << " " << param.current_r0[1] << " " << param.current_r0[2] << std::endl;
            //     std::cerr << "cur1  : " << param.current_r1[0] << " " << param.current_r1[1] << " " << param.current_r1[2] << std::endl;
            //     std::cerr << "cur2  : " << param.current_r2[0] << " " << param.current_r2[1] << " " << param.current_r2[2] << std::endl;
            //     std::cerr << "tgt0  : " << param.target_r0[0] << " " << param.target_r0[1] << " " << param.target_r0[2] << std::endl;
            //     std::cerr << "tgt1  : " << param.target_r1[0] << " " << param.target_r1[1] << " " << param.target_r1[2] << std::endl;
            // }

            dif_pos  = param.target_p0 - param.current_p0;
            vel_pos0 = param.current_p0 - param.current_p1;
            vel_pos1 = param.current_p1 - param.current_p2;
            dif_target_pos = param.target_p0 - param.target_p1;

            rats::difference_rotation(dif_rot, param.current_r0, param.target_r0);
            rats::difference_rotation(vel_rot0, param.current_r1, param.current_r0);
            rats::difference_rotation(vel_rot1, param.current_r2, param.current_r1);
            rats::difference_rotation(dif_target_rot, param.target_r1, param.target_r0);

            if ( DEBUGP ) {
                std::cerr << "dif_p : " << dif_pos[0] << " " << dif_pos[1] << " " << dif_pos[2] << std::endl;
                std::cerr << "vel_p0: " << vel_pos0[0] << " " << vel_pos0[1] << " " << vel_pos0[2] << std::endl;
                std::cerr << "vel_p1: " << vel_pos1[0] << " " << vel_pos1[1] << " " << vel_pos1[2] << std::endl;
                std::cerr << "dif_t : " << dif_target_pos[0] << " " << dif_target_pos[1] << " " << dif_target_pos[2] << std::endl;
            }
            if ( DEBUGP ) {
                std::cerr << "dif_r : " << dif_rot[0] << " " << dif_rot[1] << " " << dif_rot[2] << std::endl;
                std::cerr << "vel_r0: " << vel_rot0[0] << " " << vel_rot0[1] << " " << vel_rot0[2] << std::endl;
                std::cerr << "vel_r1: " << vel_rot1[0] << " " << vel_rot1[1] << " " << vel_rot1[2] << std::endl;
                std::cerr << "dif_t : " << dif_target_rot[0] << " " << dif_target_rot[1] << " " << dif_target_rot[2] << std::endl;
            }
            hrp::Vector3 vel_p, vel_r;
            //std::cerr << "MDK = " << param.M_p << " " << param.D_p << " " << param.K_p << std::endl;
            //std::cerr << "MDK = " << param.M_r << " " << param.D_r << " " << param.K_r << std::endl;
            // std::cerr << "ref_force = " << param.ref_force[0] << " " << param.ref_force[1] << " " << param.ref_force[2] << std::endl;
            // std::cerr << "ref_moment = " << param.ref_moment[0] << " " << param.ref_moment[1] << " " << param.ref_moment[2] << std::endl;

            // ref_force/ref_moment and force_gain/moment_gain are expressed in global coordinates. 
            vel_p =  ( param.force_gain * (abs_forces[it->first] - param.ref_force) * m_dt * m_dt
                       + param.M_p * ( vel_pos1 - vel_pos0 )
                       + param.D_p * ( dif_target_pos - vel_pos0 ) * m_dt
                       + param.K_p * ( dif_pos * m_dt * m_dt ) ) /
                     (param.M_p + (param.D_p * m_dt) + (param.K_p * m_dt * m_dt));
            vel_r =  ( param.moment_gain * (abs_moments[it->first] - param.ref_moment) * m_dt * m_dt
                       + param.M_r * ( vel_rot1 - vel_rot0 )
                       + param.D_r * ( dif_target_rot - vel_rot0 ) * m_dt
                       + param.K_r * ( dif_rot * m_dt * m_dt  ) ) /
                     (param.M_r + (param.D_r * m_dt) + (param.K_r * m_dt * m_dt));

            // generate smooth motion just after impedance started
            if ( DEBUGP ) {
                std::cerr << "vel_p : " << vel_p[0] << " " << vel_p[1] << " " << vel_p[2] << std::endl;
                std::cerr << "vel_r : " << vel_r[0] << " " << vel_r[1] << " " << vel_r[2] << std::endl;
            }
            manip->solveLimbIK(vel_p, vel_r, param.transition_count, param.avoid_gain, param.reference_gain, MAX_TRANSITION_COUNT, qrefv, DEBUGP);

	    param.current_p2 = param.current_p1;
	    param.current_p1 = param.current_p0 + vel_p;
	    param.target_p1 = param.target_p0;

	    param.current_r2 = param.current_r1;
            // if ( std::fabs(vel_r.norm() - 0.0) < ::std::numeric_limits<double>::epsilon() ) {
            if ( vel_r.norm() != 0.0 ) {
              hrp::Matrix33 tmpm;
              rats::rotm3times(tmpm, rats::rotation_matrix(vel_r.norm(), vel_r.normalized()), param.current_r0);
              param.current_r1 = tmpm;
            } else {
              param.current_r1 = param.current_r0;
            }
            param.target_r1 = param.target_r0;

            if ( param.transition_count < 0 ) {
              param.transition_count++;
            }
        it++;
	  } // else
    } // while

        if ( m_q.data.length() != 0 ) { // initialized
            for ( int i = 0; i < m_robot->numJoints(); i++ ){
                m_q.data[i] = m_robot->joint(i)->q;
            }
            m_qOut.write();
            if ( DEBUGP ) {
                std::cerr << "q     : ";
                for ( int i = 0; i < m_q.data.length(); i++ ){
                    std::cerr << " " << m_q.data[i];
                }
                std::cerr << std::endl;
            }
        }
        for (size_t i = 0; i < m_ref_forceOut.size(); i++) {
          m_ref_forceOut[i]->write();
        }
    } else {
        if ( DEBUGP || loop % 100 == 0 ) {
            std::cerr << "ImpedanceController is not working..." << std::endl;
            std::cerr << "         m_qRef " << m_qRef.data.length() << std::endl;
            std::cerr << "     m_qCurrent " << m_qCurrent.data.length() << std::endl;
        }
    }
    return RTC::RTC_OK;
}
RTC::ReturnCode_t RemoveForceSensorLinkOffset::onExecute(RTC::UniqueId ec_id)
{
  //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
  static int loop = 0;
  loop ++;
  for (unsigned int i=0; i<m_forceIn.size(); i++){
    if ( m_forceIn[i]->isNew() ) {
      m_forceIn[i]->read();
    }
  }
  hrp::Vector3 rpy;
  if (m_rpyIn.isNew()) {
    m_rpyIn.read();
    rpy = hrp::Vector3(m_rpy.data.r, m_rpy.data.p, m_rpy.data.y);
  } else {
    rpy = hrp::Vector3::Zero();
  }
  if (m_qCurrentIn.isNew()) {
    m_qCurrentIn.read();
    for ( int i = 0; i < m_robot->numJoints(); i++ ){
      m_robot->joint(i)->q = m_qCurrent.data[i];
    }
    //
    updateRootLinkPosRot(rpy);
    m_robot->calcForwardKinematics();
    for (unsigned int i=0; i<m_forceIn.size(); i++){
      if ( m_force[i].data.length()==6 ) {
        std::string sensor_name = m_forceIn[i]->name();
        hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_name);
        hrp::Vector3 data_p(m_force[i].data[0], m_force[i].data[1], m_force[i].data[2]);
        hrp::Vector3 data_r(m_force[i].data[3], m_force[i].data[4], m_force[i].data[5]);
        if ( DEBUGP ) {
          std::cerr << "forces[" << m_forceIn[i]->name() << "]" << std::endl;;
          std::cerr << "raw force : " << data_p[0] << " " << data_p[1] << " " << data_p[2] << std::endl;
          std::cerr << "raw moment : " << data_r[0] << " " << data_r[1] << " " << data_r[2] << std::endl;
        }
        if ( sensor ) {
          // real force sensor
          hrp::Matrix33 sensorR = sensor->link->R * sensor->localR;
          hrp::Vector3 mg = hrp::Vector3(0,0, m_forcemoment_offset_param[sensor_name].link_offset_mass * grav * -1);
          // force and moments which do not include offsets
          hrp::Vector3 off_force = sensorR * (data_p - m_forcemoment_offset_param[sensor_name].force_offset) - mg;
          hrp::Vector3 off_moment = sensorR * (data_r - m_forcemoment_offset_param[sensor_name].moment_offset) - hrp::Vector3(sensorR * m_forcemoment_offset_param[sensor->name].link_offset_centroid).cross(mg);
          // convert absolute force -> sensor local force
          off_force = hrp::Vector3(sensorR.transpose() * off_force);
          off_moment = hrp::Vector3(sensorR.transpose() * off_moment);
          for (size_t j = 0; j < 3; j++) {
            m_force[i].data[j] = off_force(j);
            m_force[i].data[3+j] = off_moment(j);
          }
          if ( DEBUGP ) {
            std::cerr << "off force : " << off_force[0] << " " << off_force[1] << " " << off_force[2] << std::endl;
            std::cerr << "off moment : " << off_moment[0] << " " << off_moment[1] << " " << off_moment[2] << std::endl;
          }
        } else {
          std::cerr << "unknwon force param" << std::endl;
        }
      }
    }
  }
  for (unsigned int i=0; i<m_forceOut.size(); i++){
    m_forceOut[i]->write();
  }
  return RTC::RTC_OK;
}