Exemplo n.º 1
0
int
main(int argc, char** argv)
{
	if (argc < 2)
	{
		std::cout << "Usage: rlDynamics1Demo MODELFILE Q1 ... Qn QD1 ... QDn QDD1 ... QDDn" << std::endl;
		return 1;
	}
	
	try
	{
		rl::mdl::XmlFactory factory;
		boost::shared_ptr< rl::mdl::Model > model(factory.create(argv[1]));
		
		rl::mdl::Dynamic* dynamic = dynamic_cast< rl::mdl::Dynamic* >(model.get());
		
		rl::math::Vector q(model->getDof());
		rl::math::Vector qd(model->getDof());
		rl::math::Vector qdd(model->getDof());
		rl::math::Vector tau(model->getDof());
		
		for (std::size_t i = 0; i < model->getDof(); ++i)
		{
			q(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2]);
			qd(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2 + model->getDof()]);
			qdd(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2 + 2 * model->getDof()]);
		}
		
		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->setAcceleration(qdd);
		
		dynamic->inverseDynamics();
		dynamic->getTorque(tau);
		std::cout << "tau = " << tau.transpose() << std::endl;
		
		dynamic->forwardDynamics();
		dynamic->getAcceleration(qdd);
		std::cout << "qdd = " << qdd.transpose() << std::endl;
		
		dynamic->rungeKuttaNystrom(1);
		dynamic->getPosition(q);
		dynamic->getVelocity(qd);
		std::cout << "q = " << q.transpose() << std::endl;
		std::cout << "qd = " << qd.transpose() << std::endl;
	}
	catch (const std::exception& e)
	{
		std::cout << e.what() << std::endl;
		return 1;
	}
	
	return 0;
}
Exemplo n.º 2
0
void CUIQuestBook::AddQuestListToMessageBoxL(const int& iMessageBoxType)
{
	CUIManager* pUIManager = CUIManager::getSingleton();
	Quest* pQuest = GAMEDATAMGR()->GetQuest();

	if (pQuest == NULL)
		return;

	int iQuestIndex = -1;
	CTString strQuestTitle;
	int count;

	count = pQuest->GetCompleteQuestCount();

	for( int iComplete = 0; iComplete < count; ++iComplete )
	{
		iQuestIndex = pQuest->GetCompleteQuestIndex(iComplete);

		CQuestDynamicData qdd(CQuestSystem::Instance().GetStaticData(iQuestIndex));				
		strQuestTitle = qdd.GetName();

		if( 0 == strQuestTitle.Length() )
		{
			if (_pNetwork->m_ubGMLevel > 1)
				strQuestTitle.PrintF("Index : %d", iQuestIndex);
			else
				continue;
		}

		pUIManager->AddMessageBoxLString( iMessageBoxType, FALSE, strQuestTitle, ciQuestClassifier + iQuestIndex, 0xF2F200FF, CTString("A") );
	}

	count = pQuest->GetProceedQuestCount();

	for( int iNew = 0; iNew < count; ++iNew )
	{
		iQuestIndex = pQuest->GetProceedQuestIndex(iNew);

		CQuestDynamicData qdd(CQuestSystem::Instance().GetStaticData(iQuestIndex));				
		strQuestTitle = qdd.GetName();

		if( 0 == strQuestTitle.Length() )
		{
			if (_pNetwork->m_ubGMLevel > 1)
				strQuestTitle.PrintF("Index : %d", iQuestIndex);
			else
				continue;
		}

		pUIManager->AddMessageBoxLString( iMessageBoxType, FALSE, strQuestTitle, ciQuestClassifier + iQuestIndex, 0xF2F200FF, CTString("Q") );
	}
}
Exemplo n.º 3
0
/* Given a state vector of length 2*N corresponding to N joint angles
 * and N joint speeds and torque vector of length N, 
 * this fills in the 2*N length vector sd with
 * the corresponding joint speeds and joint accelerations.
 */
std::vector<double>
ckbot::chain_rate::calc_rate(std::vector<double> s, std::vector<double> T)
{
    int N = c.num_links();

    std::vector<double> qdd(N);
    std::vector<double> q(N);
    std::vector<double> qd(N);

    for(int i=0; i<N; ++i)
    {
        q[i] = s[i];
        qd[i] = s[N+i];
    }

    /* Update the chain's state to match the requested initial
     * conditions (s)
     */
    c.propogate_angles_and_rates(q,qd);

    tip_base_step(s, T);
    qdd = base_tip_step(s, T);

    for (int i=0; i < N; ++i)
    {
        /* Rate of change of positions are already in our state */
        sd[i] = s[N+i];

        /* The second half of the state vector is rate of change
         * of velocities, or the calculated accelerations.
         */
        sd[N+i] = qdd[i];
    }
    return sd;
}
Exemplo n.º 4
0
void robManipulatorTest::TestInverseDynamics(){
    cmnPath path;
    path.AddRelativeToCisstShare("/models/WAM");
    std::string fname = path.Find("wam7.rob", cmnPath::READ);

    robManipulator WAM7( fname );

  vctDynamicVector<double> q(7, 0.0), qd(7, 1.0), qdd(7, 1.0);
  vctFixedSizeVector<double, 6> ft(0.0);
  vctDynamicVector<double> tau = WAM7.RNE( q, qd, qdd, ft );

}
Exemplo n.º 5
0
void CUIQuestBook::UpdateQuestContent( int nQuestIdx )
{
	CUITooltipMgr::getSingleton()->setData(NULL);

	m_nSelectQuestIdx = nQuestIdx;

	std::map<int, CUIImage*>::iterator iter = m_mapQuestSelect.begin();
	std::map<int, CUIImage*>::iterator iter_end = m_mapQuestSelect.end();

	for (; iter != iter_end; ++iter)
	{
		(*iter).second->Hide(TRUE);
	}

	if (m_nSelectQuestIdx <= 0)
	{
		initContent();
		return;
	}

	std::map<int, CUIImage*>::iterator it = m_mapQuestSelect.find(m_nSelectQuestIdx);

	if (it != iter_end)
	{
		(*it).second->Hide(FALSE);
	}

	CQuestDynamicData* pQuestDD = CQuestSystem::Instance().GetDynamicDataByQuestIndex( m_nSelectQuestIdx );
	CQuestDynamicData qdd(CQuestSystem::Instance().GetStaticData(m_nSelectQuestIdx));

	if (GAMEDATAMGR()->GetQuest()->isRaidMessage(m_nSelectQuestIdx) == true)
	{		
		pQuestDD = &qdd;
	}

	// 타입 아이콘
	int nType[eICON_MAX];
	int nToolTipIdx[eICON_MAX];
	nType[eQUEST_TYPE1] = pQuestDD->GetQuestType1();
	nType[eQUEST_TYPE2] = pQuestDD->GetQuestType2();
	nType[eQUEST_SCALE] = pQuestDD->GetQuestPartyScale();

	nToolTipIdx[eQUEST_TYPE1] = nType1StringIdx[nType[eQUEST_TYPE1]];
	nToolTipIdx[eQUEST_TYPE2] = nType2StringIdx[nType[eQUEST_TYPE2]];
	nToolTipIdx[eQUEST_SCALE] = nScaleStringIdx[nType[eQUEST_SCALE]];

	CTString strTooltip;
	COLOR	tempColor;
	int		i;

	for (i = 0; i < eICON_MAX; i++)
	{		
		if(m_pImageArr[i] != NULL)
		{
			m_pImageArr[i]->SetRenderIdx(nType[i]);
			strTooltip.PrintF(_S(nToolTipIdx[i], "타입 툴팁"));
			m_pImageArr[i]->setTooltip(strTooltip.str_String);
			m_pImageArr[i]->Hide(FALSE);
		}
	}	

	// 퀘스트 이름
	if (m_ptbName != NULL)
	{
		tempColor = m_ptbName->GetTextColor();
		m_ptbName->SetAlignTextV(eALIGN_V_CENTER);
		m_ptbName->SetText( CTString(pQuestDD->GetName()), tempColor );
		m_ptbName->StringEllipsis(2);

		if (m_ptbName->IsEllipsis() == true)
		{
			m_ptbName->setTooltip(CTString(pQuestDD->GetName()));
		}
		else
		{
			m_ptbName->setTooltip(CTString(""));
		}
		m_ptbName->Hide(FALSE);
	}

	// List 내용
	if (m_plistContent == NULL)
		return;

	m_plistContent->Hide(FALSE);
	m_plistContent->DeleteAllListItem();
	m_vecOptionPrize.clear();

	if (m_plistScroll)
		m_plistScroll->SetScrollPos(0);

	CUITextBox* pTextBox = NULL;
	CUITextBox* pTempTb = NULL;
	CUIText* pText = NULL;
	CUIImage* pImg = NULL;
	CUIIcon* pIcon = NULL;
	CUIListItem* plistItem = NULL;
	CTString strTemp;

	// 1. Summury
	pTextBox = (CUITextBox*)m_pDesign->CloneSummury();

	if (pTextBox != NULL)
	{		
		pTextBox->SetText(CTString(pQuestDD->GetDesc3()));
		int nCnt = pTextBox->GetLineCount();

		for (i = 0; i < nCnt; ++i)
		{			
			pText = (CUIText*)m_pDesign->CloneSummuryText();

			if(pText != NULL)
			{
				strTemp = pTextBox->GetLineString(i);
				pText->SetText(strTemp);
				m_plistContent->AddListItem(pText);
			}
		}

		SAFE_DELETE(pTextBox);
		pText = (CUIText*)m_pDesign->CloneSpace();
		if (pText != NULL)
		{
			m_plistContent->AddListItem(pText);
		}
	}

	// 2. 진행 상황
	plistItem = (CUIListItem*)m_pDesign->CloneTitle();

	if (plistItem != NULL)
	{
		strTemp.PrintF(_S(1703, "진행상황"));
		pText = (CUIText*)plistItem->findUI("str_copytitle");

		if (pText != NULL)
			pText->SetText(strTemp);

		m_plistContent->AddListItem(plistItem);
	}

	// 진행 상황 내용
	int count = pQuestDD->GetCountStatusDesc();

	for (int index = 0; index < count; index++)
	{
		pTextBox = (CUITextBox*)m_pDesign->CloneDesc();

		if (pTextBox != NULL)
		{
			if (index != 0 && index % 2 == 1)
				tempColor = 0xFF0000FF;
			else
				tempColor = pTextBox->GetTextColor();

			pTextBox->SetText(CTString(pQuestDD->GetStatusDesc(index)), tempColor, TRUE);
			int nCnt = pTextBox->GetLineCount();

			for (i = 0; i < nCnt; ++i)
			{			
				pTempTb = (CUITextBox*)m_pDesign->CloneDesc();
				if(pTempTb != NULL)
				{
					strTemp = pTextBox->GetLineString(i);
					pTempTb->SetText(strTemp, tempColor, TRUE);
					m_plistContent->AddListItem(pTempTb);
				}
			}
			SAFE_DELETE(pTextBox);
		}
	}

	pText = (CUIText*)m_pDesign->CloneSpace();
	if (pText != NULL)
	{
		m_plistContent->AddListItem(pText);
	}

	// 3. Npc
	plistItem = (CUIListItem*)m_pDesign->CloneTitle();

	if (plistItem != NULL)
	{
		int NpcIdx = pQuestDD->GetPrizeNPCIndex();
		// Name
		if (NpcIdx > 0)
		{
			CMobData* pNpc = CMobData::getData(NpcIdx);
			strTemp.PrintF("%s", pNpc->GetName());
		}
		else
		{
			strTemp.PrintF(_S(457, "시스템"));
		}		
			
		pText = (CUIText*)plistItem->findUI("str_copytitle");

		if (pText != NULL)
			pText->SetText(strTemp);

		m_plistContent->AddListItem(plistItem);
	}

	// 4. StartDesc
	pTextBox = (CUITextBox*)m_pDesign->CloneDesc();

	if (pTextBox != NULL)
	{
		pTextBox->SetText(CTString(pQuestDD->GetDesc()));

		int nCnt = pTextBox->GetLineCount();

		for (i = 0; i < nCnt; ++i)
		{			
			pText = (CUIText*)m_pDesign->CloneDescText();

			if(pText != NULL)
			{
				strTemp = pTextBox->GetLineString(i);
				pText->SetText(strTemp);
				m_plistContent->AddListItem(pText);
			}
		}
		
		SAFE_DELETE(pTextBox);
		pText = (CUIText*)m_pDesign->CloneSpace();
		if (pText != NULL)
		{
			m_plistContent->AddListItem(pText);
		}
	}

	// 5. 수행가능 레벨
	plistItem = (CUIListItem*)m_pDesign->CloneTitle();

	if (plistItem != NULL)
	{
		strTemp.PrintF(_S( 1704, "수행가능 레벨" ));

		pText = (CUIText*)plistItem->findUI("str_copytitle");

		if (pText != NULL)
			pText->SetText(strTemp);

		m_plistContent->AddListItem(plistItem);
	}

	// 6. 레벨 값	
	pTextBox = (CUITextBox*)m_pDesign->CloneDesc();

	if (pTextBox != NULL)
	{
		if(pQuestDD->GetNeedMinLevel() == pQuestDD->GetNeedMaxLevel())
		{
			strTemp.PrintF(_S( 1705, "레벨 %d" ), pQuestDD->GetNeedMinLevel());
		}
		else if(pQuestDD->GetNeedMaxLevel()==999)
		{
			strTemp.PrintF(_S( 5667, "레벨 : %d ~ MAX" ), pQuestDD->GetNeedMinLevel());
		}
		else
		{
			strTemp.PrintF(_S( 1706, "레벨 %d ~ 레벨 %d" ), pQuestDD->GetNeedMinLevel(), pQuestDD->GetNeedMaxLevel());
		}

		pTextBox->SetText(strTemp);
		int nCnt = pTextBox->GetLineCount();

		for (i = 0; i < nCnt; ++i)
		{			
			pText = (CUIText*)m_pDesign->CloneDescText();
			if(pText != NULL)
			{
				strTemp = pTextBox->GetLineString(i);
				pText->SetText(strTemp);
				m_plistContent->AddListItem(pText);
			}
		}
		SAFE_DELETE(pTextBox);
		pText = (CUIText*)m_pDesign->CloneSpace();
		if (pText != NULL)
		{
			m_plistContent->AddListItem(pText);
		}
	}

	// 7. 퀘스트 보상	
	// 고정보상
	if(pQuestDD->IsPrizeExist())
	{
		// 보상 타이틀
		plistItem = (CUIListItem*)m_pDesign->CloneTitle();

		if (plistItem != NULL)
		{
			pText = (CUIText*)plistItem->findUI("str_copytitle");

			if (pText != NULL)
				pText->SetText(pQuestDD->GetPrizeDesc( 0 ));

			m_plistContent->AddListItem(plistItem);
		}

		// 보상 아이템		
		CUIImageBox* pImgBox = new CUIImageBox;
		int Count = pQuestDD->GetCountPrizeDesc();

		for( INDEX i = 1; i < Count; ++i )
		{
			plistItem = (CUIListItem*)m_pDesign->CloneListItem();

			if (plistItem == NULL)
				continue;

			// 아이템 설명
			CTString strPrize = pQuestDD->GetPrizeDesc( i );
			bool bIcon = true;
			UIBtnExType eBtnType = UBET_ITEM;

			pTextBox = (CUITextBox*)plistItem->findUI("tb_copyComplete");

			if (pTextBox != NULL)
			{
				tempColor = pTextBox->GetTextColor();
				pTextBox->SetText(strPrize, tempColor, TRUE);
			}

			// 아이콘인지, 이미지 인지 결정
			switch (pQuestDD->GetPrizeType(i-1) )
			{
			case QPRIZE_ITEM:
				eBtnType = UBET_ITEM;
				break;
			case QPRIZE_SKILL:
			case QPRIZE_SSKILL:
				eBtnType = UBET_SKILL;
				break;
			default:
				bIcon = false;
				break;
			}

			pIcon = (CUIIcon*)plistItem->findUI("icon_copyItem");
			pImg = (CUIImage*)plistItem->findUI("img_copyitem");

			// 아이템
			if (bIcon == true)
			{			
				if (pIcon != NULL)
				{
					pIcon->setData(eBtnType, pQuestDD->GetPrizeIndex(i-1));
					pIcon->Hide(FALSE);
				}
			}
			else
			{
				if (pImg != NULL)
				{					
					pImgBox->SetImageByType(CUIImageBox::eImageType(pQuestDD->GetPrizeType(i-1)), pQuestDD->GetPrizeIndex(i-1), pQuestDD->GetSyndicateType());
					UIRectUV pUv = pImgBox->GetUV();
					pImg->setTexData( _pUIBtnTexMgr->GetTex( UIBtnExType(pImgBox->GetBtnType()), pImgBox->GetTextureID() ));					
					pImg->SetUV(pUv.U0, pUv.V0, pUv.U1, pUv.V1);
					pImg->Hide(FALSE);
				}
			}

			m_plistContent->AddListItem(plistItem);
		}

		if (pImgBox)
		{
			delete pImgBox;
			pImgBox = NULL;
		}
	}

	//선택 보상
	if(pQuestDD->IsOptionPrizeExist())
	{
		// 보상 타이틀
		plistItem = (CUIListItem*)m_pDesign->CloneTitle();

		if (plistItem != NULL)
		{
			pText = (CUIText*)plistItem->findUI("str_copytitle");

			if (pText != NULL)
				pText->SetText(pQuestDD->GetOptionPrizeDesc(0));

			m_plistContent->AddListItem(plistItem);
		}

		// 보상 아이템		
		CUIImageBox* pImgBox = new CUIImageBox;
		int Count = pQuestDD->GetCountOptionPrizeDesc();

		for( INDEX i = 1; i < Count; ++i )
		{
			plistItem = (CUIListItem*)m_pDesign->CloneListItem();

			if (plistItem == NULL)
				continue;

			// 아이템 설명
			CTString strPrize = pQuestDD->GetOptionPrizeDesc( i );
			bool bIcon = true;
			UIBtnExType eBtnType = UBET_ITEM;

			pTextBox = (CUITextBox*)plistItem->findUI("tb_copyComplete");

			if (pTextBox != NULL)
			{
				tempColor = pTextBox->GetTextColor();
				pTextBox->SetText(strPrize, tempColor, TRUE);
			}

			// 아이콘인지, 이미지 인지 결정
			switch (pQuestDD->GetOptionPrizeType(i-1) )
			{
			case QPRIZE_ITEM:
				eBtnType = UBET_ITEM;
				break;
			case QPRIZE_SKILL:
			case QPRIZE_SSKILL:
				eBtnType = UBET_SKILL;
				break;
			default:
				bIcon = false;
				break;
			}

			pIcon = (CUIIcon*)plistItem->findUI("icon_copyItem");
			pImg = (CUIImage*)plistItem->findUI("img_copyitem");

			// 아이템
			if (bIcon == true)
			{			
				if (pIcon != NULL)
				{
					pIcon->setData(eBtnType, pQuestDD->GetOptionPrizeIndex(i-1));
					pIcon->Hide(FALSE);
				}
			}
			else
			{
				if (pImg != NULL)
				{					
					pImgBox->SetImageByType(CUIImageBox::eImageType(pQuestDD->GetOptionPrizeType(i-1)), pQuestDD->GetOptionPrizeIndex(i-1), pQuestDD->GetSyndicateType());
					UIRectUV pUv = pImgBox->GetUV();
					pImg->setTexData( _pUIBtnTexMgr->GetTex( UIBtnExType(pImgBox->GetBtnType()), pImgBox->GetTextureID() ));					
					pImg->SetUV(pUv.U0, pUv.V0, pUv.U1, pUv.V1);
					pImg->Hide(FALSE);
				}
			}

			pImg = (CUIImage*)plistItem->findUI("img_selectItem");

			if (pImg != NULL)
				m_vecOptionPrize.push_back(pImg);

			CmdQuestBookPrizeItemSet* pCmd = new CmdQuestBookPrizeItemSet;
			pCmd->setData(this, i - 1);
			plistItem->SetCommandUp(pCmd);

			m_plistContent->AddListItem(plistItem);
		}

		if (pImgBox)
		{
			delete pImgBox;
			pImgBox = NULL;
		}
	}

	int nMaxItem = m_plistContent->getListItemCount();
	m_plistContent->UpdateScroll(nMaxItem);
	m_plistContent->UpdateList();

	int nShowCnt = m_plistContent->GetItemShowNum();

	if (m_plistScroll != NULL)
		m_plistScroll->SetItemsPerPage(nShowCnt);

	if (m_pbtnGiveUp != NULL)
		m_pbtnGiveUp->SetEnable(TRUE);

	if (pQuestDD->GetPrizeNPCIndex() == 0 && GAMEDATAMGR()->GetQuest()->IsCompleteQuest(pQuestDD->GetQuestIndex()))
	{
		if (m_pbtnOk != NULL)
			m_pbtnOk->SetEnable(TRUE);
	}
}
void JointTrajectoryActionController::update()
{
  ros::Time time = ros::Time::now();

  std::vector<double> q(joints_.size()), qd(joints_.size()), qdd(joints_.size());

  boost::shared_ptr<const SpecifiedTrajectory> traj_ptr = current_trajectory_;
  if (!traj_ptr)
    ROS_FATAL("The current trajectory can never be null");

  // Only because this is what the code originally looked like.
  const SpecifiedTrajectory &traj = *traj_ptr;

  if (traj.size() == 0)
  {
    ROS_ERROR("No segments in the trajectory");
    return;
  }

  // ------ Finds the current segment

  // Determines which segment of the trajectory to use.
  int seg = -1;
  while (seg + 1 < (int)traj.size() && traj[seg + 1].start_time <= time.toSec())
  {
    ++seg;
  }

  if (seg == -1)
  {
    // ROS_ERROR("No earlier segments.  First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
    // return;
    seg = 0;
  }

  // ------ Trajectory Sampling

  for (size_t i = 0; i < q.size(); ++i)
  {
    sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration, time.toSec() - traj[seg].start_time,
                               q[i], qd[i], qdd[i]);
  }

  // ------ Calculate error

  std::vector<double> error(joints_.size());
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    error[i] = katana_->getMotorAngles()[i] - q[i];
  }

  // ------ State publishing

  pr2_controllers_msgs::JointTrajectoryControllerState msg;

  // Message containing current state for all controlled joints
  for (size_t j = 0; j < joints_.size(); ++j)
    msg.joint_names.push_back(joints_[j]);
  msg.desired.positions.resize(joints_.size());
  msg.desired.velocities.resize(joints_.size());
  msg.desired.accelerations.resize(joints_.size());
  msg.actual.positions.resize(joints_.size());
  msg.actual.velocities.resize(joints_.size());
  // ignoring accelerations
  msg.error.positions.resize(joints_.size());
  msg.error.velocities.resize(joints_.size());
  // ignoring accelerations

  msg.header.stamp = time;
  for (size_t j = 0; j < joints_.size(); ++j)
  {
    msg.desired.positions[j] = q[j];
    msg.desired.velocities[j] = qd[j];
    msg.desired.accelerations[j] = qdd[j];
    msg.actual.positions[j] = katana_->getMotorAngles()[j];
    msg.actual.velocities[j] = katana_->getMotorVelocities()[j];
    // ignoring accelerations
    msg.error.positions[j] = error[j];
    msg.error.velocities[j] = katana_->getMotorVelocities()[j] - qd[j];
    // ignoring accelerations
  }

  controller_state_publisher_.publish(msg);
  // TODO: here we could publish feedback for the FollowJointTrajectory action; however,
  // this seems to be optional (the PR2's joint_trajectory_action_controller doesn't do it either)
}
Exemplo n.º 7
0
int
main(int argc, char** argv)
{
	if (argc < 2)
	{
		std::cout << "Usage: rlDynamics2Demo MODELFILE Q1 ... Qn QD1 ... QDn QDD1 ... QDDn" << std::endl;
		return 1;
	}
	
	try
	{
		rl::mdl::XmlFactory factory;
		boost::shared_ptr< rl::mdl::Model > model(factory.create(argv[1]));
		
		rl::mdl::Dynamic* dynamic = dynamic_cast< rl::mdl::Dynamic* >(model.get());
		
		rl::math::Vector q(dynamic->getDof());
		rl::math::Vector qd(dynamic->getDof());
		rl::math::Vector qdd(dynamic->getDof());
		rl::math::Vector tau(dynamic->getDof());
		
		for (std::size_t i = 0; i < dynamic->getDof(); ++i)
		{
			q(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2]);
			qd(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2 + dynamic->getDof()]);
			qdd(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2 + 2 * dynamic->getDof()]);
		}
		
		rl::math::Vector g(3);
		dynamic->getWorldGravity(g);
		
		rl::math::Vector tmp(dynamic->getDof());
		rl::math::Vector tmp2(6 * dynamic->getOperationalDof());
		
		std::cout << "===============================================================================" << std::endl;
		
		// FP
		
		dynamic->setPosition(q);
		dynamic->forwardPosition();
		const rl::math::Transform::ConstTranslationPart& position = dynamic->getOperationalPosition(0).translation();
		rl::math::Vector3 orientation = dynamic->getOperationalPosition(0).rotation().eulerAngles(2, 1, 0).reverse();
		std::cout << "x = " << position.x() << " m, y = " << position.y() << " m, z = " << position.z() << " m, a = " << orientation.x() * rl::math::RAD2DEG << " deg, b = " << orientation.y() * rl::math::RAD2DEG << " deg, c = " << orientation.z() * rl::math::RAD2DEG << " deg" << std::endl;
		
		std::cout << "===============================================================================" << std::endl;
		
		// FV
		
		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->forwardVelocity();
		std::cout << "xd = " << dynamic->getOperationalVelocity(0).linear().transpose() << " " << dynamic->getOperationalVelocity(0).angular().transpose() << std::endl;
		
		std::cout << "-------------------------------------------------------------------------------" << std::endl;
		
		// J
		
		rl::math::Matrix J(6 * dynamic->getOperationalDof(), dynamic->getDof());
		dynamic->calculateJacobian(J);
		std::cout << "J = " << std::endl << J << std::endl;
		
		// J * qd
		
		tmp2 = J * qd;
		std::cout << "xd = " << tmp2.transpose() << std::endl;
		
		// invJ
		
		rl::math::Matrix invJ(dynamic->getDof(), 6 * dynamic->getOperationalDof());
		dynamic->calculateJacobianInverse(J, invJ);
		std::cout << "J^{-1} = " << std::endl << invJ << std::endl;
		
		std::cout << "===============================================================================" << std::endl;
		
		// FA
		
		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->setAcceleration(qdd);
		dynamic->setWorldGravity(g);
		dynamic->forwardVelocity();
		dynamic->forwardAcceleration();
		std::cout << "xdd = " << dynamic->getOperationalAcceleration(0).linear().transpose() << " " << dynamic->getOperationalAcceleration(0).angular().transpose() << std::endl;
		
		std::cout << "-------------------------------------------------------------------------------" << std::endl;
		
		// Jd * qd
		
		rl::math::Vector Jdqd(6 * dynamic->getOperationalDof());
		dynamic->calculateJacobianDerivative(Jdqd);
		std::cout << "Jd*qd = " << Jdqd.transpose() << std::endl;
		
		// J * qdd + Jd * qd 
		
		tmp2 = J * qdd + Jdqd;
		std::cout << "xdd = " << tmp2.transpose() << std::endl;
		
		std::cout << "===============================================================================" << std::endl;
		
		// RNE
		
		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->setAcceleration(qdd);
		dynamic->inverseDynamics();
		dynamic->getTorque(tau);
		std::cout << "tau = " << tau.transpose() << std::endl;
		
		std::cout << "-------------------------------------------------------------------------------" << std::endl;
		
		// M
		
		rl::math::Matrix M(dynamic->getDof(), dynamic->getDof());
		dynamic->setPosition(q);
		dynamic->calculateMassMatrix(M);
		std::cout << "M = " << std::endl << M << std::endl;
		
		// V
		
		rl::math::Vector V(dynamic->getDof());
		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->calculateCentrifugalCoriolis(V);
		std::cout << "V = " << V.transpose() << std::endl;
		
		// G
		
		rl::math::Vector G(dynamic->getDof());
		dynamic->setPosition(q);
		dynamic->setWorldGravity(g);
		dynamic->calculateGravity(G);
		std::cout << "G = " << G.transpose() << std::endl;
		
		// M * qdd + V + G
		
		tmp = M * qdd + V + G;
		std::cout << "tau = " << tmp.transpose() << std::endl;
		
		std::cout << "===============================================================================" << std::endl;
		
		// FD
		
		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->setTorque(tau);
		dynamic->forwardDynamics();
		dynamic->getAcceleration(tmp);
		std::cout << "qdd = " << tmp.transpose() << std::endl;
		
		std::cout << "-------------------------------------------------------------------------------" << std::endl;
		
		// M^{-1}
		
		rl::math::Matrix invM(dynamic->getDof(), dynamic->getDof());
		dynamic->setPosition(q);
		dynamic->calculateMassMatrixInverse(invM);
		std::cout << "M^{-1} = " << std::endl << invM << std::endl;
		
		// V
		
		std::cout << "V = " << V.transpose() << std::endl;
		
		// G
		
		std::cout << "G = " << G.transpose() << std::endl;
		
		// M^{-1} * ( tau - V - G )
		
		tmp = invM * (tau - V - G);
		std::cout << "qdd = " << tmp.transpose() << std::endl;
		
		std::cout << "===============================================================================" << std::endl;
	}
	catch (const std::exception& e)
	{
		std::cout << e.what() << std::endl;
		return 1;
	}
	
	return 0;
}
Exemplo n.º 8
0
/* Returns a vector of length N corresponding to each link's qdd */
std::vector<double>
ckbot::chain_rate::base_tip_step(std::vector<double> s, std::vector<double> T)
{
    int N = c.num_links();

    std::vector<double> q(N);
    std::vector<double> qd(N);
    std::vector<double> qdd(N);

    for(int i=0; i<N; ++i)
    {
        q[i] = s[i];
        qd[i] = s[N+i];
    }

    Eigen::VectorXd grav(6);
    grav << 0,0,0,0,0,9.81;

    Eigen::Matrix3d R_cur;
    Eigen::Vector3d r_i_ip;
    Eigen::MatrixXd phi(6,6);
    Eigen::VectorXd alpha(6);
    alpha = Eigen::VectorXd::Zero(6);

    Eigen::VectorXd alpha_p(6);
    Eigen::VectorXd G(6);
    Eigen::VectorXd a(6);

    Eigen::VectorXd H_b_frame_star(6);
    Eigen::VectorXd H_w_frame_star(6);
    Eigen::RowVectorXd H(6);

    for (int i=0; i<N; ++i)
    {
        module_link& cur = c.get_link(i);

        /* 3x3 Rotation matrix from this link's coordinate
         * frame to the world frame
         */
        R_cur = c.get_current_R(i);

        /* Vector, in world frame, from this link's inbound joint to its
         * outbound joint
         */
        r_i_ip = R_cur*(cur.get_r_ip1() - cur.get_r_im1());

        /* 6x6 Spatial body operator matrix that transforms spatial
         * vectors from the outbound joint to the inbound joint in
         * the world frame
         */
        phi = get_body_trans(r_i_ip);

        /* Transform the spatial accel vector from the inbound
         * link's inbound joint to this link's inbound joint
         */
        alpha_p = phi.transpose()*alpha;

        /* These are calculated in tip_base_step */
        int cur_index = 0;
        for (int k = (6*i); k <= (6*(i+1)-1); ++k, ++cur_index)
        {
            G[cur_index] = G_all[k];
            a[cur_index] = a_all[k];
        }

        /* The angular acceleration of this link's joint */
        /* TODO: 6DOF changes here...This gets a whoooole lotta broken */
        qdd[i] = mu_all[i] - G.transpose()*alpha_p;

        /* Joint matrix in body frame.  Check tip_base_step for the definition */
        /* TODO: 6DOF changes here a plenty */
        H_b_frame_star = cur.get_joint_matrix().transpose();

        Eigen::MatrixXd tmp_6x6(6,6);
        tmp_6x6 << R_cur, Eigen::Matrix3d::Zero(),
                   Eigen::Matrix3d::Zero(), R_cur;

        /* Joint matrix in the world frame */
        H_w_frame_star = tmp_6x6*H_b_frame_star;
        /* As a row vector */
        H = H_w_frame_star.transpose();

        /* Spatial acceleration of this link at its inbound joint in
         * its frame (ie: including its joint accel)
         */
        alpha = alpha_p + H.transpose()*qdd[i] + a;
    }
    return qdd;
}