C++ Chain类代码示例

Chain ModelGetter::GetModel()
{
  const int MAX_LEN = 1024 ;
  char cur_line[1024] ;
  
  Chain chain ;
  while (!infile_.eof())
  {
    double model[6] ;
    infile_.getline(cur_line, MAX_LEN) ;
    int num_read = sscanf(cur_line, "%lf %lf %lf %lf %lf %lf", &model[0], &model[1], &model[2], &model[3], &model[4], &model[5]) ;
    if (num_read < 6)
      break ;
    
    // Now add the data to the chain
    Joint J(Joint::RotZ) ;
    Vector rot_axis(model[3], model[4], model[5]) ;           // KDL doesn't need vector to be normalized. It even behaves nicely with vector=[0 0 0]
    double rot_ang = rot_axis.Norm() ;
    Rotation R(Rotation::Rot(rot_axis, rot_ang)) ;
    Vector trans(model[0], model[1], model[2]) ;
    chain.addSegment(Segment(J, Frame(R, trans))) ;
  }
  
  return chain ;
}

/** get all chains from the matching */
const RamAutoIndex::ChainOrderMap RamAutoIndex::getChainsFromMatching(const RamMaxMatching::Matchings& match, const SearchSet& nodes) {
    ASSERT(nodes.size() > 0);

    // Get all unmatched nodes from A
    const SearchSet& umKeys = getUnmatchedKeys(match, nodes);

    // Case: if no unmatched nodes then we have an anti-chain
    if (umKeys.size() == 0){ 
        for (SearchSet::const_iterator nit = nodes.begin(); nit != nodes.end(); ++nit) {
            std::set<SearchColumns> a;
            a.insert(*nit);
            chainToOrder.push_back(a);
            return chainToOrder;
        }
    }

    ASSERT(umKeys.size() > 0);

    // A worklist of used nodes
    SearchSet usedKeys;

    // Case: nodes < umKeys or if nodes == umKeys then anti chain - this is handled by this loop
    for (SearchSet::iterator it = umKeys.begin(); it != umKeys.end(); ++it) {
        Chain c = getChain(*it, match);
        ASSERT(c.size() > 0);
        chainToOrder.push_back(c);
    }

    ASSERT(chainToOrder.size() > 0);

    return chainToOrder;
}

void SolverTest::FkPosAndIkPosLocal(Chain& chain,ChainFkSolverPos& fksolverpos, ChainIkSolverPos& iksolverpos)
{
    JntArray q(chain.getNrOfJoints());
    for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
    {
        random(q(i));
    }
    JntArray q_init(chain.getNrOfJoints());
    double tmp;
    for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
    {
        random(tmp);
        q_init(i)=q(i)+0.1*tmp;
    }
    JntArray q_solved(q);

    Frame F1,F2;

    CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q,F1));
    CPPUNIT_ASSERT(0 <= iksolverpos.CartToJnt(q_init,F1,q_solved));
    CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q_solved,F2));

    CPPUNIT_ASSERT_EQUAL(F1,F2);
    //CPPUNIT_ASSERT_EQUAL(q,q_solved);

}

void SolverTest::FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver)
{
    double deltaq = 1E-4;

    Frame F1,F2;

    JntArray q(chain.getNrOfJoints());
    Jacobian jac(chain.getNrOfJoints());

    for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
    {
        random(q(i));
    }

    jacsolver.JntToJac(q,jac);

    for (unsigned int i=0; i< q.rows() ; i++)
    {
        // test the derivative of J towards qi
        double oldqi = q(i);
        q(i) = oldqi+deltaq;
        CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q,F2));
        q(i) = oldqi-deltaq;
        CPPUNIT_ASSERT(0==fksolverpos.JntToCart(q,F1));
        q(i) = oldqi;
        // check Jacobian :
        Twist Jcol1 = diff(F1,F2,2*deltaq);
        Twist Jcol2(Vector(jac(0,i),jac(1,i),jac(2,i)),
                    Vector(jac(3,i),jac(4,i),jac(5,i)));

        //CPPUNIT_ASSERT_EQUAL(true,Equal(Jcol1,Jcol2,epsJ));
        CPPUNIT_ASSERT_EQUAL(Jcol1,Jcol2);
    }
}

void SolverTest::FkVelAndIkVelLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainIkSolverVel& iksolvervel)
{

    JntArray q(chain.getNrOfJoints());
    JntArray qdot(chain.getNrOfJoints());

    for(unsigned int i=0; i<chain.getNrOfJoints(); i++)
    {
        random(q(i));
        random(qdot(i));
    }
    JntArrayVel qvel(q,qdot);
    JntArray qdot_solved(chain.getNrOfJoints());

    FrameVel cart;

    CPPUNIT_ASSERT(0==fksolvervel.JntToCart(qvel,cart));

    int ret = iksolvervel.CartToJnt(qvel.q,cart.deriv(),qdot_solved);
    CPPUNIT_ASSERT(0<=ret);

    qvel.deriv()=qdot_solved;

    if(chain.getNrOfJoints()<=6)
        CPPUNIT_ASSERT(Equal(qvel.qdot,qdot_solved,1e-5));
    else
    {
        FrameVel cart_solved;
        CPPUNIT_ASSERT(0==fksolvervel.JntToCart(qvel,cart_solved));
        CPPUNIT_ASSERT(Equal(cart.deriv(),cart_solved.deriv(),1e-5));
    }
}

bool KDLColladaLibraryJointsExporter::doExport(vector <Chain>& kdlChains)
{
    if (kdlChains.empty())
    {
        LOG(DEBUG) << "KDL Chains array is empty, has nothing to export";
        return false;
    }


    LOG(WARNING) << "Taking first element from the KDL chain array";

    Chain kdlChain = kdlChains[0];
    openLibrary();

    unsigned int jointNr = kdlChain.getNrOfSegments();

    for (unsigned int i = 0; i < jointNr; i++)
    {
        Segment segment = kdlChain.getSegment(i);
        Joint kdlJoint = segment.getJoint();
        string uniqueId = defaultJointIdPrefix + toString(i);
        COLLADASW::Joint colladaJoint = makeColladaSWJoint(COLLADASW::LibraryJoints::mSW, kdlJoint, uniqueId);
        LOG(INFO) << "Joint id: " << colladaJoint.getJointId();
        LOG(INFO) << "Joint name: " << colladaJoint.getJointName();
        addJoint(colladaJoint);
        joints.push_back(colladaJoint);
    }

    closeLibrary();

    return true;
}

		void score_res(ScoreBase * scoring, std::string filename, std::string score_type = "pairing") {
			Chain chain;
			int i, j, l;

			chain_read_model(chain, filename);
			l = chain.size();
			//JN_JN_OUT << l << std::endl;
			for (i = 0; i < l; i++) {
				for (j = 0; j < l; j++) {
					if (i == j) JN_OUT << 0 << "\t";
					else {
						scoring->en_bp(chain[i], chain[j]);
						if (score_type == "pairing") JN_OUT << scoring->m_en_pairing << "\t";
						else if (score_type == "stacking") JN_OUT << scoring->m_en_stacking << "\t";
						else if (score_type == "wc") JN_OUT << scoring->m_en_wc << "\t";
						else if (score_type == "nwc") JN_OUT << scoring->m_en_nwc << "\t";
						else {
							LOG << "Illegal score type: " << score_type << std::endl;
							throw "error!";
						}
					}
				}
				JN_OUT << std::endl;
			}
		}

int main ()
{
	Chain ch;
	ch.start.init();
	ch.chainBuilder (&ch.start, 5);
	ch.chainWalker (&ch.start);
}

// Merges two chains
void Board::mergeChains(Chain *node, int otherID, Move m) {
    // Find the chain to merge into the first chain
    Chain *temp = nullptr;
    int tempIndex = searchChainsByID(temp, otherID);

    // Update the chain id array when merging
    for (int i = 0; i < temp->size; i++) {
        Move idChange = temp->squares[i];
        chainID[index(getX(idChange), getY(idChange))] = node->id;

        node->add(idChange);
    }

    // Remove the move played from the other list of liberties
    temp->removeLiberty(temp->findLiberty(m));
    // And then merge the two lists
    for (int i = 0; i < temp->liberties; i++) {
        // If the liberty is not a repeat
        if (node->findLiberty(temp->libertyList[i]) == -1)
            node->addLiberty(temp->libertyList[i]);
    }

    // Delete the chain "temp" now since it has been fully merged in
    delete temp;
    chainList.removeFast(tempIndex);
}

void TestPart2(){
	
	Chain<string> a, b;
	
	a.ReadChain(); // User provides input for Chain a.
	cout <<"A: "<< a <<endl;
	cout <<"A.size: "<< a.Size()<<endl;
	
	b.ReadChain(); // User provides input for Chain b. 
	cout <<"B: "<< b << endl;
	cout <<"B.size: "<< b.Size()<<endl;
	
	cout <<"A+B: "<<a + b << endl; // Concatenates the two Chains.
	Chain<string> d = a + b; 
	cout <<"D: "<< d << endl;
	cout <<"D.size: "<< d.Size()<<endl;

	cout <<"D+: "<< d + "hi_there" <<endl; // Adds an element to the end.
	cout <<"D+.size: "<< d.Size()<<endl;
	cout <<"A[2]: "<< a[2] << endl; 	// Should print the 3rd element.
							// Throw an exception (or even abort()) if is out of
							// range.
	b[1] = "b_string"; // Should change the 2nd element to “b_string”
	cout <<"B: "<< b << endl;
	b[0] = "a_string"; 
	cout <<"B: "<< b <<endl;
	cout <<"B.size: "<< b.Size()<<endl;

} // End of TestPart2

size_t Board::removeCapturedStones (StoneColor colorToCapture)
{
    LOG_FUNCTION(cout, "Board::removeCapturedStones");

    size_t captureCount = 0;

    // A set of points that we've already examined. This helps prevents some
    // bad recursion and keeps a chain from being "found" once for each stone
    // in the chain.
    //
    ConstPointSet alreadyVisited;

    for (size_t row = 0; row < m_points.size(); ++row)
    {
        for (size_t column = 0; column < m_points[row].size(); ++column)
        {
            const Point & thePoint = m_points.at(row).at(column);

            // If the point we are considering has already been visited,
            // then Chain's ctor will throw a PointVisitedAlreadyException
            // object. We can safely swallow that exception and move on
            // to the next point.
            //
            try
            {
                Chain currentChain {thePoint, *this, &alreadyVisited};

                gLogger.log(LogLevel::kMedium, cout, "Discovered chain"); // " : ", chain);

                // If the chain we are looking at is the color we should consider capturing and
                // the chain has no liberties, then capture it
                //
                if (currentChain.color() == colorToCapture && currentChain.libertyCount() == 0)
                {
                    captureCount += currentChain.size();

                    // For each point in the chain, remove the stone from the board
                    // at those coordniates
                    //
                    currentChain.forEachPoint([this](const Point & point)
                    {
                        LOG_BUSY_FUNCTION(cout, "Chain::removeCapturedStones::lambda_doStoneRemoval");
                        m_points[point.coordinates.row][point.coordinates.column].removeStone();
                    });
                }
            }
            catch (const Chain::PointVisitedAlreadyException & ex)
            {
                gLogger.log(LogLevel::kFirehose, cout, "Skipping ", thePoint);
            }
        }
    }

    // If we ended up only capturing one stone, the 'Ko' rule might affect
    // the next move, so remember that fact!
    //
    m_koState.first = (captureCount == 1);

    return captureCount;
}

int VerifyJacobian::ComputeMaxError(const std::string& model_file, const std::string& joint_params_file, const std::string& jacobians_file, double& max_error)
{
  int result ;
  
  result = model_getter_.OpenFile(model_file) ;
  if (result < 0)
    return -1 ;

  result = joint_params_getter_.OpenFile(joint_params_file) ;
  if (result < 0)
    return -1 ;

  result = jacobians_getter_.OpenFile(jacobians_file) ;
  if (result < 0)
    return -1 ;
  
  Chain chain = model_getter_.GetModel() ;

  const unsigned int J = chain.getNrOfJoints() ;
  JntArray joint_array(J) ;
  result = joint_params_getter_.GetNextJointArray(joint_array) ;
  if (result < 0)
    return -1 ;
  
  LinkParamJacobian jac_actual ;
  jac_actual.twists_.resize(J) ;
  result = jacobians_getter_.GetNextJacobian(jac_actual) ;

  // Compute the jacobian using KDL functions
  
  LinkParamJacobian jac_computed ;
  jac_computed.twists_.resize(J) ;
  
  LinkParamJacobianSolver jac_solver ;
  jac_solver.JointsToCartesian(chain, joint_array, jac_computed) ;
  
  max_error = 0.0 ;
  for (unsigned int i=0; i < jac_computed.twists_.size(); i++)
  {
    for (unsigned int j=0; j<3; j++)
    {
      for (unsigned int k=0; k<3; k++)
      {
        double cur_error ;
        cur_error = fabs( jac_computed.twists_[i].trans_[j].vel(k) - jac_actual.twists_[i].trans_[j].vel(k) ) ;
        if (cur_error > max_error)
          max_error = cur_error ;
        
        cur_error = fabs( jac_computed.twists_[i].rot_[j].vel(k) - jac_actual.twists_[i].rot_[j].vel(k) ) ;
        if (cur_error > max_error)
          max_error = cur_error ;
      }
    }
  }    
  
  return 0 ;
}

bool Tree::addChain(const Chain& chain, const std::string& hook_name) {
    string parent_name = hook_name;
    for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) {
        if (this->addSegment(chain.getSegment(i), parent_name))
          parent_name = chain.getSegment(i).getName();
        else
            return false;
    }
    return true;
}

void
Module::set ( Log_Entry &e )
{
    for ( int i = 0; i < e.size(); ++i )
    {
        const char *s, *v;

        e.get( i, &s, &v );

        if ( ! strcmp( s, ":chain" ) )
        {
            /* This trickiness is because we may need to know the name of
               our chain before we actually get added to it. */
            int i;
            sscanf( v, "%X", &i );
            Chain *t = (Chain*)Loggable::find( i );

            assert( t );

            chain( t );
        }
    }

    for ( int i = 0; i < e.size(); ++i )
    {
        const char *s, *v;

        e.get( i, &s, &v );

/*         if ( ! strcmp( s, ":name" ) ) */
/*             label( v ); */
        if ( ! strcmp( s, ":parameter_values" ) )
        {
            set_parameters( v );
        }
        else if ( ! ( strcmp( s, ":is_default" ) ) )
        {
            is_default( atoi( v ) );
        }
        else if ( ! ( strcmp( s, ":active" ) ) )
        {
            bypass( ! atoi( v ) );
        }
        else if ( ! strcmp( s, ":chain" ) )
        {
            int i;
            sscanf( v, "%X", &i );
            Chain *t = (Chain*)Loggable::find( i );

            assert( t );

            t->add( this );
        }
    }
}

// method calculates error between two chains, used in finduniquecombination method
//
// Chain upper: the first chain, uses i values
// Chain lower: the second chain, uses i-1 values
//
// returns: double of error amount
double SearchStrategy::assignmentError(Chain upper, Chain lower)
{
	double AiM1 = (lower.first()).getCAIM()[0];
	double BiM1 = (lower.first()).getCBIM()[0];
	double Ai = (upper.last()).getCAI()[0];
	double Bi = (upper.last()).getCBI()[0];

	double squashedError = abs((AiM1 - Ai) / Ai) + abs((BiM1 - Bi)/ Bi);

	return squashedError;
}