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;
}
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") );
}
}
/* 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;
}
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 );
}
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)
}
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;
}
/* 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;
}
