// this method should only be entered when holding ref_impl's mutex
void TaskManager::set_max_threads_impl(unsigned int max, Mutex::TrackLock& lock) {
if (ref_impl->error || ref_impl->stop_status != StopStatus::running) return;
if (!max) max = 1;
if (max < ref_impl->min_threads) max = ref_impl->min_threads;
if (max > ref_impl->max_threads) {
ref_impl->max_threads = max;
const unsigned int new_used_threads = std::min(ref_impl->tasks, max);
unsigned int new_thread_count = 0;
if (new_used_threads > ref_impl->used_threads) {
new_thread_count = new_used_threads - ref_impl->used_threads;
ref_impl->used_threads = new_used_threads;
}
// we need to give the threads a reference before we release the
// mutex so we don't race with thread killing in stop_all()
for (unsigned int refs = 0; refs < new_thread_count; ++refs) ref_impl->ref();
lock.unlock();
for (; new_thread_count; --new_thread_count) {
std::unique_ptr<Thread> t;
try {
t = Thread::start(Callback::make(*ref_impl,
&TaskManager::RefImpl::do_tasks,
false),
false);
}
catch (...) { // try block might throw std::bad_alloc
// roll back for any unstarted threads
lock.lock();
ref_impl->error = true;
ref_impl->used_threads -= new_thread_count;
if (ref_impl->stop_status == StopStatus::stopped && ref_impl->blocking)
ref_impl->cond.broadcast();
lock.unlock();
for (unsigned int unrefs = 0; unrefs < new_thread_count; ++unrefs) ref_impl->unref();
throw;
}
if (!t.get()) {
// roll back for any unstarted threads
lock.lock();
ref_impl->error = true;
ref_impl->used_threads -= new_thread_count;
if (ref_impl->stop_status == StopStatus::stopped && ref_impl->blocking)
ref_impl->cond.broadcast();
lock.unlock();
for (unsigned int unrefs = 0; unrefs < new_thread_count; ++unrefs) ref_impl->unref();
throw TaskError();
}
}
}
else ref_impl->max_threads = max;
}
void TaskManager::stop_all() {
Mutex::TrackLock lock{ref_impl->mutex};
if (ref_impl->stop_status == StopStatus::stopped) throw TaskError();
{ // stop_guard scope block. stop_guard is only referenced in
// add_task() , and is included to enable
// StopMode::wait_for_running to be implemented scaleably
Mutex::Lock stop_lock{ref_impl->stop_guard};
ref_impl->stop_status = StopStatus::stop_requested;
// there is no race here once 'stop_status' is set to
// stop_requested
if (ref_impl->stop_mode == StopMode::wait_for_running)
while (!ref_impl->task_queue.empty()) ref_impl->task_queue.pop();
// we could be adding more KillThread callbacks than necessary
// here, because as we are doing this a timeout on a thread might
// expire leading to its demise in that way. However, that
// doesn't matter - we just get left with a redundant callback in
// 'task_queue' which never gets used
for (unsigned int count = ref_impl->used_threads; count; --count) {
try {
ref_impl->task_queue.emplace(
std::unique_ptr<const Callback::Callback>(Callback::make(&throw_kill_thread)),
std::unique_ptr<const Callback::Callback>()
);
}
catch (...) { // if the push throws, it is guaranteed that the
// item concerned is not inserted in the queue,
// because std::unique_ptr's move constructor and
// move assignment operator do not throw, so we
// can rethrow here without issues
ref_impl->error = true;
throw;
}
}
ref_impl->stop_status = StopStatus::stopped;
}
if (ref_impl->blocking) {
// in case the destructor is also blocking and unwaits first, we
// need to take a reference to the RefImpl object so the last test
// of the while variable is valid, and also take a pointer to its
// address so we can still address it
RefImpl* tmp = ref_impl;
tmp->ref();
while (tmp->used_threads) tmp->cond.wait(tmp->mutex);
lock.unlock(); // we cannot do a final unreference of RefImpl
// still holding its mutex
tmp->unref();
}
}
void QuickVerifyTask::Update(DCNotifier *notifier, DWORD event_code, void *param)
{
switch (event_code)
{
case DM::DM_Event::kNewDiskDetected:
NewDiskDetected( param );
break;
case DM::DM_Event::kTaskInProgress:
TaskInProgress( param );
break;
case DM::DM_Event::kBadBlock:
BadSector( param );
break;
case DM::DM_Event::kTaskComplete:
TaskComplete( param );
break;
case DM::DM_Event::kTaskBreak:
TaskBreak( param );
break;
case DM::DM_Event::kTaskError:
TaskError( param );
break;
case DM::DM_Event::kDetectError:
DetectError( param );
break;
case DM::DM_Event::kDiskRemoved:
DiskRemoved( param );
break;
}
}
void HumanoidController::HumanoidControl()
{
// Perform Optimization
{
UpdateConstraintMatrix();
int maxPriorityLevels = OptimizationSchedule.maxCoeff();
const int numTasks = OptimizationSchedule.size();
if (maxPriorityLevels > 0)
{
for (int level=1; level<=maxPriorityLevels; level++)
{
taskConstrActive.setZero(numTasks);
taskOptimActive.setZero(numTasks);
bool runOpt = false;
for (int i=0; i<numTasks;i ++)
{
if (OptimizationSchedule(i) == level)
{
taskOptimActive(i) = 1;
runOpt = true;
}
else if ((OptimizationSchedule(i) < level) && (OptimizationSchedule(i) > -1))
{
taskConstrActive(i) = 1;
}
}
if (runOpt)
{
UpdateObjective();
UpdateHPTConstraintBounds();
//cout << "Optimizing level " << level << endl;
Optimize();
for (int i=0; i<numTasks;i ++)
{
if (OptimizationSchedule(i) == level)
{
TaskBias(i) += TaskError(i);
//cout << "Optimization Level " << level << " task error " << i << " = " << TaskError(i) << endl;
}
}
}
}
}
/// Compute optimal quantities
// desired change of centroidal momentum
hDotOpt = CentMomMat*qdd + cmBias;
// Desired ZMP info
zmpWrenchOpt.setZero();
Vector6F icsForce, localForce;
Float * nICS = icsForce.data(); // ICS
Float * fICS = nICS+3;
Float * nLoc = localForce.data(); // Local
Float * fLoc = nLoc+3;
for (int k1 = 0; k1 < NS; k1++)
{
LinkInfoStruct * listruct = artic->m_link_list[SupportIndices[k1]];
CartesianVector tmp;
localForce = SupportXforms[k1].transpose()*fs.segment(6*k1,6);
// Apply Spatial Force Transform Efficiently
// Rotate Quantities
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,nLoc,nICS);
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,fLoc,fICS);
// Add the r cross f
CROSS_PRODUCT(listruct->link_val2.p_ICS,fICS,tmp);
ADD_CARTESIAN_VECTOR(nICS,tmp);
zmpWrenchOpt+=icsForce;
}
transformToZMP(zmpWrenchOpt,zmpPosOpt);
// Form Joint Input and simulate
VectorXF jointInput = VectorXF::Zero(STATE_SIZE);
int k = 7;
// Skip over floating base (i=1 initially)
for (int i=1; i<artic->m_link_list.size(); i++)
{
LinkInfoStruct * linki = artic->m_link_list[i];
if (linki->dof)
{
//cout << "Link " << i << " dof = " << linki->dof << endl;
//cout << "qd = " << qdDm.segment(k,linki->dof).transpose() << endl;
jointInput.segment(k,linki->dof) = tau.segment(linki->index_ext-6,linki->dof);
k+=linki->link->getNumDOFs();
}
}
//cout << "Tau = " << tau.transpose() << endl;
//cout << "Joint input = " << jointInput.transpose() << endl;
//exit(-1);
//jointInput.segment(7,NJ) = tau;
artic->setJointInput(jointInput.data());
/// verification
ComputeActualQdd(qddA);
}
#ifdef CONTROL_DEBUG
// Debug Code
{
}
#endif
}
void CUploadTask::Run()
{
m_Item.ullOffset=0;
if (m_Item.ullFilesize==0)
{
string content="";
CURLcode ret=COssApi::AddObject(m_Item.strHost,m_Item.strBucket,m_Item.strObject,m_Item.ullFilesize,this,NULL,(culfunc)curl_header,content);
int httpcode=ParesHeaderCode(m_strHeader);
if (httpcode==200)
{
Finish();
goto END;
}
if (httpcode>0)
TaskError(httpcode,_T("http code"));
else
TaskError(ret,_T("curl error"));
goto END;
}
m_UnFinish.InsertPair(0,m_Item.ullFilesize-1);
if (CheckMultipart())
{
if (m_Item.strUploadId==_T(""))
{
COssInitiateMultipartUploadRet ret;
if(COssApi::InitiateMultipartUploadObject(m_Item.strHost,m_Item.strBucket,m_Item.strObject,ret))
{
m_Item.strUploadId=ret.m_strUploadId;
GetNetwork()->m_TransDb.Update_UploadUploadId(m_Item.strPathhash,m_Item.strUploadId);
}
else
{
TaskError(TRANSERROR_OSSERROR,ret.m_strCode);
goto END;
}
}
if (!CreateMultipartFile())
{
TaskError(TRANSERROR_CREATEMULTIPARTERROR,wstring(GetSkinInfo()->GetString(_T("prompt"),_T("CreateFileInfoError"))));
goto END;
}
}
m_ullStarttime=CTime::GetCurrentTime().GetTime();
m_ullRuntime=m_ullStarttime;
while(true)
{
if (m_bStop)
goto END;
if (CheckFinish())
{
m_ullTranssize=0;
if (m_Item.strUploadId!=L"")
{
COssRet ret;
if(!COssApi::CompleteMultipartUpload(m_Item.strHost,m_Item.strBucket,m_Item.strObject,m_Item.strUploadId,m_PartList,ret))
{
TaskError(TRANSERROR_OSSERROR,ret.m_strCode);//zhengÓеĴíÎóÓ¦¸Ãɾ³ýid
goto END;
}
}
Finish();
goto END;
}
int nNum=GetPeerNum();
if (nNum>=GetNetwork()->m_UManager.m_nPeerMax)
{
Sleep(1);
continue;
}
ULONGLONG ullPos,ullSize;
ullPos=ullSize=0;
m_csFileLock.Lock();
int nIndex=GetUploadIndex(ullPos,ullSize);
m_csFileLock.Unlock();
if (nIndex<0)
{
Sleep(1000);
continue;
}
m_csLock.Lock();
nNum=m_listPeer.size();
m_csLock.Unlock();
if (nNum<GetNetwork()->m_UManager.m_nPeerMax)
{
CUploadPeer * peer =new CUploadPeer;
peer->Init(this,m_Item.strHost,m_Item.strBucket,m_Item.strObject);
if (peer->OpenFile(m_Item.strUploadId,m_Item.strFullpath))
{
m_csFileLock.Lock();
m_UnFinish.RemovePairs(ullPos,ullPos+ullSize-1);
m_csFileLock.Unlock();
m_csLock.Lock();
m_listPeer.push_back(peer);
m_csLock.Unlock();
peer->StartUpload(nIndex,ullPos,ullSize);
GetNetwork()->m_ThreadPool.AddTask(peer);
}
else
{
int a=GetLastError();
delete peer;
if (m_Item.strUploadId==_T(""))
{
TaskError(TRANSERROR_OPENFILE,wstring(slnhelper::GetLastErrorMessage(a)));
goto END;
}
}
}
else
{
bool bHave=false;
m_csLock.Lock();
for (int i=0;i<m_listPeer.size();i++)
{
CUploadPeer * peer =(CUploadPeer*)m_listPeer[i];
if (peer->IsIdle())
{
m_csFileLock.Lock();
m_UnFinish.RemovePairs(ullPos,ullPos+ullSize-1);
m_csFileLock.Unlock();
peer->StartUpload(nIndex,ullPos,ullSize);
GetNetwork()->m_ThreadPool.AddTask(peer);
bHave=true;
break;
}
}
m_csLock.Unlock();
if (!bHave)
Sleep(10);
}
}
END:
if (m_bDelete&&CheckMultipart())
{
if (m_Item.strUploadId!=L"")
{
COssRet ret;
COssApi::AbortMultipartUpload(m_Item.strHost,m_Item.strBucket,m_Item.strObject,m_Item.strUploadId,ret);
}
}
if (m_entOut)
m_entOut->SetEvent();
m_Item.nStatus=TRANSTASK_REMOVE;
}
void TaskManager::add_task(std::unique_ptr<const Callback::Callback> task,
std::unique_ptr<const Callback::Callback> fail) {
{ // scope block for mutex lock
Mutex::TrackLock lock{ref_impl->mutex};
if (ref_impl->error || ref_impl->stop_status != StopStatus::running)
throw TaskError();
// check if we need to start a new thread
if (ref_impl->tasks >= ref_impl->used_threads
&& ref_impl->used_threads < ref_impl->max_threads) {
// by incrementing 'tasks' and 'used_threads' here (and backing
// out later below if something goes wrong), there is no race
// from starting the thread before adding the task, even if the
// thread has a very short minimum idle time, and we can also
// release the mutex before calling Thread::start() or emplacing
// in the queue to reduce contention on the mutex: this is
// because, after coming out of a timeout, RefImpl::do_tasks()
// will test again whether tasks >= used_threads and if so the
// new thread will not exit. Doing it this way also means that
// we are guaranteed that if an exception is thrown no task has
// been added.
++ref_impl->tasks;
++ref_impl->used_threads;
ref_impl->ref(); // give each thread a reference
lock.unlock();
std::unique_ptr<Thread> t;
try {
t = Thread::start(Callback::make(*ref_impl,
&TaskManager::RefImpl::do_tasks,
false),
false);
}
catch (...) { // try block might throw std::bad_alloc
// roll back for the unstarted thread
lock.lock();
ref_impl->error = true;
--ref_impl->tasks;
--ref_impl->used_threads;
if (ref_impl->stop_status == StopStatus::stopped && ref_impl->blocking)
ref_impl->cond.broadcast();
lock.unlock();
ref_impl->unref();
throw;
}
if (!t.get()) {
// roll back for the unstarted thread
lock.lock();
ref_impl->error = true;
--ref_impl->tasks;
--ref_impl->used_threads;
if (ref_impl->stop_status == StopStatus::stopped && ref_impl->blocking)
ref_impl->cond.broadcast();
lock.unlock();
ref_impl->unref();
throw TaskError();
}
}
else {
// if we are in this block the mutex must still be locked
++ref_impl->tasks;
}
}
// we use stop_guard and stop_status to detect whether stop_all()
// has been called between us releasing the TaskManager mutex above
// and reaching here - stop_guard is included so that we can push
// onto the queue below without holding the TaskManager mutex. The
// only other place where stop_guard is locked is in stop_all():
// stop_all() is normally only called once. stop_guard will
// therefore not give rise to any significant additional contention,
// because AsyncQueueDispatch::emplace() is itself ordered (and only
// moves two std::unique_ptr objects onto the queue).
// ref_impl->stop_status is set in stop_all() holding both the
// TaskManager mutex and stop_guard, so we can lock either in order
// to make a safe read. stop_guard would be a candidate for being a
// read-write lock, except this is pointless because as mentioned
// AsyncQueueDispatch::emplace() is ordered.
Mutex::TrackLock stop_lock{ref_impl->stop_guard};
if (ref_impl->stop_status == StopStatus::running) {
try {
ref_impl->task_queue.emplace(std::move(task), std::move(fail));
}
catch (...) {
// roll back for the unadded task
// release 'stop_guard' - 'mutex' cannot be locked after
// 'stop_guard' is locked
stop_lock.unlock();
Mutex::Lock lock{ref_impl->mutex};
ref_impl->error = true;
--ref_impl->tasks;
throw;
}
}
else {
// roll back for the unadded task
// release 'stop_guard' - 'mutex' cannot be locked after
// 'stop_guard' is locked
stop_lock.unlock();
Mutex::Lock lock{ref_impl->mutex};
--ref_impl->tasks;
throw TaskError();
}
}
void TaskManager::set_blocking(bool block) {
Mutex::Lock l{ref_impl->mutex};
if (ref_impl->stop_status == StopStatus::stopped) throw TaskError();
ref_impl->blocking = block;
}
void HumanoidController::HumanoidControl(ControlInfo & ci) {
int taskRow = 0;
Float discountFactor = 1;
dmTimespec tv1, tv2, tv3, tv4;
//Update Graphics Variables
{
ComPos[0] = pCom(0);
ComPos[1] = pCom(1);
ComPos[2] = pCom(2);
ComDes[0] = pComDes(0);
ComDes[1] = pComDes(1);
ComDes[2] = pComDes(2);
}
// Perform Optimization
{
dmGetSysTime(&tv2);
UpdateConstraintMatrix();
int maxPriorityLevels = OptimizationSchedule.maxCoeff();
const int numTasks = OptimizationSchedule.size();
if (maxPriorityLevels > 0) {
for (int level=1; level<=maxPriorityLevels; level++) {
taskConstrActive.setZero(numTasks);
taskOptimActive.setZero(numTasks);
bool runOpt = false;
for (int i=0; i<numTasks;i ++) {
if (OptimizationSchedule(i) == level) {
taskOptimActive(i) = 1;
runOpt = true;
}
else if ((OptimizationSchedule(i) < level) && (OptimizationSchedule(i) > -1)) {
taskConstrActive(i) = 1;
}
}
if (runOpt) {
UpdateObjective();
UpdateHPTConstraintBounds();
dmGetSysTime(&tv3);
//cout << "Optimizing level " << level << endl;
Optimize();
for (int i=0; i<numTasks;i ++) {
if (OptimizationSchedule(i) == level) {
TaskBias(i) += TaskError(i);
//cout << "Optimization Level " << level << " task error " << i << " = " << TaskError(i) << endl;
}
}
}
}
}
else {
UpdateObjective();
UpdateHPTConstraintBounds();
dmGetSysTime(&tv3);
Optimize();
}
//exit(-1);
// Extract Results
hDotOpt = CentMomMat*qdd + cmBias;
// Form Joint Input and simulate
VectorXF jointInput = VectorXF::Zero(STATE_SIZE);
// Extact Desired ZMP info
zmpWrenchOpt.setZero();
Vector6F icsForce, localForce;
Float * nICS = icsForce.data(), * nLoc = localForce.data();
Float * fICS = nICS+3, * fLoc = nLoc+3;
for (int k1 = 0; k1 < NS; k1++) {
LinkInfoStruct * listruct = artic->m_link_list[SupportIndices[k1]];
CartesianVector tmp;
localForce = SupportXforms[k1].transpose()*fs.segment(6*k1,6);
// Apply Spatial Force Transform Efficiently
// Rotate Quantities
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,nLoc,nICS);
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,fLoc,fICS);
// Add the r cross f
CROSS_PRODUCT(listruct->link_val2.p_ICS,fICS,tmp);
ADD_CARTESIAN_VECTOR(nICS,tmp);
zmpWrenchOpt+=icsForce;
}
transformToZMP(zmpWrenchOpt,zmpPosOpt);
int k = 7;
// Skip over floating base (i=1 initially)
for (int i=1; i<artic->m_link_list.size(); i++) {
LinkInfoStruct * linki = artic->m_link_list[i];
if (linki->dof) {
//cout << "Link " << i << " dof = " << linki->dof << endl;
//cout << "qd = " << qdDm.segment(k,linki->dof).transpose() << endl;
jointInput.segment(k,linki->dof) = tau.segment(linki->index_ext-6,linki->dof);
k+=linki->link->getNumDOFs();
}
}
//cout << "Tau = " << tau.transpose() << endl;
//cout << "Joint input = " << jointInput.transpose() << endl;
//exit(-1);
//jointInput.segment(7,NJ) = tau;
artic->setJointInput(jointInput.data());
ComputeActualQdd(qddA);
dmGetSysTime(&tv4);
}
//Populate Control Info Struct
{
ci.calcTime = timeDiff(tv1,tv2);
ci.setupTime = timeDiff(tv2,tv3);
ci.optimTime = timeDiff(tv3,tv4);
ci.totalTime = timeDiff(tv1,tv4);
ci.iter = iter;
}
#ifdef CONTROL_DEBUG
// Debug Code
{
cout << "q " << q.transpose() << endl;
cout << "qd " << qdDm.transpose() << endl;
cout << "qd2 " << qd.transpose() << endl;
cout << "Task Bias " << TaskBias << endl;
//cout << "H = " << endl << artic->H << endl;
cout << "CandG = " << endl << artic->CandG.transpose() << endl;
if (simThread->sim_time > 0) {
cout << setprecision(5);
MSKboundkeye key;
double bl,bu;
for (int i=0; i<numCon; i++) {
MSK_getbound(task, MSK_ACC_VAR, i, &key, &bl, &bu);
cout << "i = " << i;
switch (key) {
case MSK_BK_FR:
cout << " Free " << endl;
break;
case MSK_BK_LO:
cout << " Lower Bound " << endl;
break;
case MSK_BK_UP:
cout << " Upper Bound " << endl;
break;
case MSK_BK_FX:
cout << " Fixed " << endl;
break;
case MSK_BK_RA:
cout << " Ranged " << endl;
break;
default:
cout << " Not sure(" << key << ")" << endl;
break;
}
cout << bl << " to " << bu << endl;
}
cout << "x(57) = " << xx(57) << endl;
cout << "tau = " << tau.transpose() << endl;
cout << "qdd = " << qdd.transpose() << endl;
cout << "fs = " << fs.transpose() << endl;
//cout << "lambda = " << lambda.transpose() << endl;
VectorXF a = TaskJacobian * qdd;
//cout << "a" << endl;
VectorXF e = TaskJacobian * qdd - TaskBias;
cout << "e = " << e.transpose() << endl;
MatrixXF H = artic->H;
VectorXF CandG = artic->CandG;
MatrixXF S = MatrixXF::Zero(NJ,NJ+6);
S.block(0,6,NJ,NJ) = MatrixXF::Identity(NJ,NJ);
VectorXF generalizedContactForce = VectorXF::Zero(NJ+6);
for (int i=0; i<NS; i++) {
generalizedContactForce += SupportJacobians[i].transpose()*fs.segment(6*i,6);
}
VectorXF qdd2 = H.inverse()*(S.transpose() * tau + generalizedContactForce- CandG);
//cout << "qdd2 = " << qdd2.transpose() << endl << endl << endl;
//cout << "CandG " << CandG.transpose() << endl;
//cout << "Gen Contact Force " << generalizedContactForce.transpose() << endl;
cout << "hdot " << (CentMomMat*qdd + cmBias).transpose() << endl;
cout << "cmBias " << cmBias.transpose() << endl;
cout << "qd " << qd.transpose() << endl;
//VectorXF qdd3 = H.inverse()*(S.transpose() * tau - CandG);
//FullPivHouseholderQR<MatrixXF> fact(H);
cout <<"fNet \t" << (fs.segment(3,3) + fs.segment(9,3)).transpose() << endl;
Vector3F g;
g << 0,0,-9.81;
cout <<"hdot - mg\t" << (CentMomMat*qdd + cmBias).segment(3,3).transpose() - totalMass * g.transpose()<< endl;
exit(-1);
}
// Old Debug Code
{
VectorXF generalizedContactForce = VectorXF::Zero(NJ+6);
Matrix6F X;
MatrixXF Jac;
X.setIdentity();
for (int i=0; i<NS; i++) {
int linkIndex = SupportIndices[i];
artic->computeJacobian(linkIndex,X,Jac);
dmRigidBody * link = (dmRigidBody *) artic->getLink(linkIndex);
for (int j=0; j< link->getNumForces(); j++) {
dmForce * f = link->getForce(j);
Vector6F fContact;
f->computeForce(artic->m_link_list[linkIndex]->link_val2,fContact.data());
generalizedContactForce += Jac.transpose()*fContact;
}
}
cout << "J' f = " << generalizedContactForce.transpose() << endl;
VectorXF qdd3 = H.inverse()*(S.transpose() * tau - CandG + generalizedContactForce);
cout << "qdd3 = " << qdd3.transpose() << endl;
VectorXF state = VectorXF::Zero(2*(NJ+7));
state.segment(0,NJ+7) = q;
state.segment(NJ+7,NJ+7) = qdDm;
VectorXF stateDot = VectorXF::Zero(2*(NJ+7));
//Process qdds
artic->dynamics(state.data(),stateDot.data());
//
VectorXF qdds = VectorXF::Zero(NJ+6);
qdds.segment(0,6) = stateDot.segment(NJ+7,6);
//cout << "w x v " << cr3(qd.segment(0,3))*qd.segment(3,3) << endl;
qdds.segment(3,3) -= cr3(qdDm.segment(0,3))*qdDm.segment(3,3);
qdds.segment(6,NJ) = stateDot.segment(NJ+7*2,NJ);
Matrix3F ics_R_fb;
copyRtoMat(artic->m_link_list[0]->link_val2.R_ICS,ics_R_fb);
qdds.head(3) = ics_R_fb.transpose() * qdds.head(3);
qdds.segment(3,3) = ics_R_fb.transpose() * qdds.segment(3,3);
cout << "qdds = " << qdds.transpose() << endl;
//cout << "CandG " << endl << CandG << endl;
//cout << setprecision(6);
//cout << "I_0^C = " << endl << artic->H.block(0,0,6,6) << endl;
//exit(-1);
}
}
#endif
/*{
MatrixXF H = artic->H;
VectorXF CandG = artic->CandG;
MatrixXF S = MatrixXF::Zero(NJ,NJ+6);
S.block(0,6,NJ,NJ) = MatrixXF::Identity(NJ,NJ);
VectorXF generalizedContactForce = VectorXF::Zero(NJ+6);
for (int i=0; i<NS; i++) {
generalizedContactForce += SupportJacobians[i].transpose()*fs.segment(6*i,6);
}
qdd = H.inverse()*(S.transpose() * tau + generalizedContactForce- CandG);
cout << setprecision(8);
cout << "fs " << endl << fs << endl;
cout << "qdd " << endl << qdd << endl;
}
exit(-1);*/
static int numTimes = 0;
numTimes++;
//Float dummy;
//cin >> dummy;
//if (numTimes == 2) {
// exit(-1);
//}
//exit(-1);
}
