void
McGrip::getJointDisplacementMatrix(Matrix **J)
{
*J = new Matrix(Matrix::ZEROES<Matrix>(6, 6));
assert(numChains == 2);
for (int c = 0; c < 2; c++) {
assert(getChain(c)->getNumJoints() == 3);
for (int j = 0; j < 3; j++) {
(*J)->elem(3 * c + j, 3 * c + j) = getChain(c)->getJoint(j)->getDisplacement();
}
}
}
bool Residue::hasTorsionOmega() const
{
// instance must have a parent chain
if (getChain() == 0)
{
return false;
}
// at least 2 residues are needed to create an angle
if (getChain()->countResidues() < 2)
{
return false;
}
// the torsion angle phi is not defined for
// the N-terminus
return !isCTerminal() && hasProperty(PROPERTY__AMINO_ACID);
}
ListObject *HashTable::find(ListObject *item) const
{
ListObject *p;
ListObject *s;
int ii = 0;
s = *getChain(item);
for (p = *getChain(item); p && item->cmp(p); p = p->getNext()) {
if (p==s)
if (ii > 0)
throw "Stuck in loop.";
else
ii++;
}
return p;
}
Eigen::Matrix4d urdf_traverser::getTransformMatrix(const LinkConstPtr& from_link, const LinkConstPtr& to_link)
{
assert(from_link);
assert(to_link);
if (from_link->name == to_link->name) return Eigen::Matrix4d::Identity();
std::vector<JointPtr> pjoints = getChain(from_link, to_link);
if (pjoints.empty())
{
ROS_ERROR("could not get chain from %s to %s", from_link->name.c_str(), to_link->name.c_str());
return Eigen::Matrix4d::Identity();
}
// ROS_INFO("Chain from %s to %s",from_link->name.c_str(),to_link->name.c_str());
Eigen::Matrix4d ret = Eigen::Matrix4d::Identity();
for (std::vector<JointPtr>::iterator it = pjoints.begin(); it != pjoints.end(); ++it)
{
// ROS_INFO("Chain joint %s",(*it)->name.c_str());
Eigen::Matrix4d mat = getTransform(*it).matrix();
ret *= mat;
}
return ret;
}
/** 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 DspNode::setNumberOfInlets(const ulong nins) throw(DspError&)
{
bool state = false;
if(m_running)
{
sDspChain chain = getChain();
if(chain)
{
try
{
state = chain->suspend();
}
catch(DspError& e)
{
throw e;
}
}
else
{
stop();
}
}
m_nins = nins;
if(m_sample_ins)
{
delete [] m_sample_ins;
}
m_sample_ins = new sample*[m_nins];
sDspChain chain = getChain();
if(chain)
{
try
{
chain->resume(state);
}
catch(DspError& e)
{
throw e;
}
}
}
sDspContext DspNode::getContext() const noexcept
{
sDspChain chain = getChain();
if(chain)
{
return chain->getContext();
}
else
{
return nullptr;
}
}
ovrTextureSwapChain1_3 renderChain(ovrSession1_3 session, ovrSwapTextureSet* ts)
{
ovrTextureSwapChainWrapper* chainwrapper = getChain(session, ts);
int currentIndex = 0;
ovr_GetTextureSwapChainCurrentIndex1_3(session, chainwrapper->swapChain, ¤tIndex);
CopyTexture(chainwrapper->pContext, chainwrapper->textures[currentIndex], &ts->Textures[ts->CurrentIndex]);
ovr_CommitTextureSwapChain1_3(session, chainwrapper->swapChain);
return chainwrapper->swapChain;
}
void HashTable::remove(ListObject *item)
{
ListObject **p,*q;
p = getChain(item);
q = item->next;
if (item) {
if (find(item)) {
item->removeFromList();
--numObjects;
if (*p==item) {
*p = q;
}
}
}
}
bool Residue::isCTerminal() const
{
if (isAminoAcid() == true)
{
const Chain* chain = getChain();
if (chain != 0)
{
ResidueConstIterator res_it(chain->endResidue());
for (--res_it; +res_it && &(*res_it) != this && !res_it->isAminoAcid(); --res_it) {};
return (&(*res_it) == this);
}
}
return false;
}
void DspNode::start() throw(DspError&)
{
sDspChain chain = getChain();
if(chain)
{
if(m_running)
{
stop();
}
m_samplerate = chain->getSampleRate();
m_vectorsize = chain->getVectorSize();
prepare();
if(m_running)
{
for(ulong i = 0; i < getNumberOfInputs(); i++)
{
try
{
m_inputs[i]->start(shared_from_this());
}
catch(DspError& e)
{
m_running = false;
throw e;
}
m_sample_ins[i] = m_inputs[i]->getVector();
}
for(ulong i = 0; i < getNumberOfOutputs(); i++)
{
try
{
m_outputs[i]->start(shared_from_this());
}
catch(DspError& e)
{
m_running = false;
throw e;
}
m_sample_outs[i] = m_outputs[i]->getVector();
}
}
}
}
bool Transformation::getChain(const base::Time& atTime, std::vector< Eigen::Affine3d >& result, bool interpolate) const
{
std::vector< TransformationType > intern;
bool ret = getChain(atTime, intern, interpolate);
if(!ret)
return false;
result.resize(intern.size());
std::vector<Eigen::Affine3d >::iterator it_out = result.begin();
for(std::vector<TransformationType >::const_iterator it = intern.begin(); it != intern.end(); it++)
{
*it_out = *it;
it_out++;
}
return true;
}
// insert always inserts at the head of the chain
ListObject *HashTable::insert(ListObject *item)
{
ListObject *pp;
if (item)
{
if (pp=find(item)) {
if (pp==item)
return item;
}
ListObject **p = getChain(item);
item->insertBefore(*p);
*p = item;
numObjects++;
}
return item;
}
void processOneLine(char *line)
/* process one line */
{
int i;
char *chp;
char ch;
chp = line;
for (i=0; i<7; i++)
{
word[i] = chp;
chp = strstr(chp, "\t");
if (chp != NULL)
{
*chp = '\0';
chp++;
}
}
for (i=0; i<7; i++)
{
if (sameWord(word[i],""))
{
/* for fields that are empty, use previous ones */
word[i] = previousWord[i];
}
previousWord[i] = strdup(word[i]);
}
ch = getChain(word[0]);
if (ch == '-')
{
printf("%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n", proteinId, word[0], "", word[1], word[2], word[3],
word[4], word[5], word[6]);
}
else
{
printf("%s\t%s\t%c\t%s\t%s\t%s\t%s\t%s\t%s\n", proteinId, word[0], ch, word[1], word[2], word[3],
word[4], word[5], word[6]);
}
}
int
McGrip::jointTorqueEquilibrium()
{
Matrix *a, *B;
getRoutingMatrices(&B, &a);
Matrix p(8, 1);
//tendon insertion points
p.elem(0, 0) = 5;
p.elem(1, 0) = 5;
p.elem(2, 0) = 1.65;
//--------------
p.elem(3, 0) = 5;
p.elem(4, 0) = 5;
p.elem(5, 0) = 1.65;
//link length and joint radius
p.elem(6, 0) = getJointRadius();
p.elem(7, 0) = getLinkLength();
//compute joint torques
Matrix tau(6, 1);
matrixMultiply(*B, p, tau);
matrixAdd(tau, *a, tau);
//multiply by tendon force
assert(mTendonVec.size() == 2);
assert(mTendonVec[0]->getName() == "Finger 0");
assert(mTendonVec[1]->getName() == "Finger 1");
double f = mTendonVec[0]->getActiveForce();
for (int j = 0; j < 3; j++) {
tau.elem(j, 0) *= f;
}
f = mTendonVec[1]->getActiveForce();
for (int j = 0; j < 3; j++) {
tau.elem(3 + j, 0) *= f;
}
DBGA("Recovered joint forces:\n" << tau);
//compute joint spring values
assert(numChains == 2);
Matrix k(6, 1);
for (int c = 0; c < 2; c++) {
assert(getChain(c)->getNumJoints() == 3);
for (int j = 0; j < 3; j++) {
k.elem(3 * c + j, 0) = getChain(c)->getJoint(j)->getSpringForce();
}
}
DBGA("Recovered spring forces:\n" << k);
//compute the difference
Matrix delta(6, 1);
k.multiply(-1.0);
matrixAdd(tau, k, delta);
f = delta.fnorm();
int result;
if (f >= 1.0e3) {
DBGA("McGrip joint equilibrium failed; error norm: " << f);
result = 1;
} else {
DBGA("McGrip joint equilibrium success");
result = 0;
}
return result;
}
OVR_PUBLIC_FUNCTION(void) ovr_DestroySwapTextureSet(ovrSession session, ovrSwapTextureSet* textureSet) {
ovr_DestroyTextureSwapChain1_3((ovrSession1_3)session, getChain((ovrSession1_3)session, textureSet)->swapChain);
removeChain((ovrSession1_3)session, textureSet);
}
bool
RobotIQ::autoGrasp(bool renderIt, double speedFactor, bool stopAtContact)
{
//not implemented for the RobotIQ yet
if (myWorld->dynamicsAreOn()) return Hand::autoGrasp(renderIt, speedFactor, stopAtContact);
if (numDOF != 11) {DBGA("Hard-coded autograsp does not match RobotIQ hand"); return false;}
std::vector<double> desiredVals(11, 0.0);
if (speedFactor < 0)
{
DBGA("Hand opening not yet implemented for RobotIQ hand; forcing it to open pose");
desiredVals[3] = getDOF(3)->getVal();
desiredVals[7] = getDOF(7)->getVal();
forceDOFVals(&desiredVals[0]);
return false;
}
std::vector<double> desiredSteps(11, 0.0);
desiredVals[0] = desiredVals[4] = desiredVals[8] = dofVec[0]->getMax();
desiredVals[2] = desiredVals[6] = desiredVals[10] = dofVec[2]->getMin();
desiredSteps[0] = desiredSteps[4] = desiredSteps[8] = speedFactor*AUTO_GRASP_TIME_STEP;
desiredSteps[2] = desiredSteps[6] = desiredSteps[10] = -speedFactor*AUTO_GRASP_TIME_STEP;
int steps=0;
while(1)
{
bool moved = moveDOFToContacts(&desiredVals[0], &desiredSteps[0], true, renderIt);
std::vector<double> ref;
ref.push_back(0); ref.push_back(4); ref.push_back(8);
std::vector<double> links;
links.push_back(0); links.push_back(1); links.push_back(1);
for(size_t i=0; i<ref.size(); i++)
{
if (getChain(i)->getLink(links[i]+0)->getNumContacts() ||
getChain(i)->getLink(links[i]+1)->getNumContacts() ||
getDOF(ref[i]+0)->getVal() == getDOF(ref[i]+0)->getMax() )
{
desiredVals[ref[i]+0] = getDOF(ref[i]+0)->getVal();
desiredSteps[ref[i]+0] = 0;
if (!getChain(i)->getLink(links[i]+1)->getNumContacts())
{
desiredVals[ref[i]+1] = getDOF(ref[i]+1)->getMax();
desiredSteps[ref[i]+1] = speedFactor*AUTO_GRASP_TIME_STEP;
}
else
{
desiredVals[ref[i]+1] = getDOF(ref[i]+1)->getVal();
desiredSteps[ref[i]+1] = 0;
}
desiredVals[ref[i]+2] = getDOF(ref[i]+2)->getMax();
desiredSteps[ref[i]+2] = speedFactor*AUTO_GRASP_TIME_STEP;
}
}
if (!moved) break;
if (++steps > 1000)
{
DBGA("RobotIQ hard-coded autograsp seems to be looping forever; escaping...");
break;
}
}
return true;
}
//these two functions below are just for debugging purposes
OVR_PUBLIC_FUNCTION(ovrResult) ovr_GetTextureSwapChainCurrentIndex(ovrSession session, ovrSwapTextureSet* textureSet, int* currentIndex) {
ovrTextureSwapChain1_3 chain = getChain((ovrSession1_3)session, textureSet)->swapChain;
return ovr_GetTextureSwapChainCurrentIndex1_3((ovrSession1_3)session, chain, currentIndex);
}
OVR_PUBLIC_FUNCTION(ovrResult) ovr_CommitTextureSwapChain(ovrSession session, ovrSwapTextureSet* textureSet) {
return ovr_CommitTextureSwapChain1_3((ovrSession1_3)session, getChain((ovrSession1_3)session, textureSet)->swapChain);
}
/*! The overall matrices B and a relate hand construction parameters and
tendon force to joint torques:
(B [l r d]^t + a) * f = tau
By definition, the matrix of unknowns is thus
[l0 l1 l2 l3 l4 l5 r d]^T
This takes into account the influence of all insertion points on *all*
joints that come before them (like a Jacobian for example).
The contribution of each individual insertion point on all joints is
computed in the reference system of the link that the insertion point
is on, and considered on all proximal joints. The contributions are
computed separately, in the \a assembleTorqueMatrices(...) function.
With the current assumptions, it turns out that only l_i influences
the torque at joint i, not any l_j with j > i. Therefore, the part of
the B matrix that corresponds to the l entries is diagonal.
*/
void
McGrip::getRoutingMatrices(Matrix **B, Matrix **a)
{
*B = new Matrix(Matrix::ZEROES<Matrix>(6, 8));
*a = new Matrix(Matrix::ZEROES<Matrix>(6, 1));
//compute the world locations of all the joints of the robot
//this is overkill, as we might not need all of them
std::vector< std::vector<transf> > jointTransf(getNumChains());
for (int c = 0; c < getNumChains(); c++) {
jointTransf[c].resize(getChain(c)->getNumJoints(), transf::IDENTITY);
getChain(c)->getJointLocations(NULL, jointTransf[c]);
}
assert(numChains == 2);
for (int c = 0; c < 2; c++) {
assert(getChain(c)->getNumJoints() == 3);
for (int j = 0; j < 3; j++) {
int refJointNum = c * 3 + j;
transf refTran = jointTransf.at(c).at(j);
for (int k = j; k < 3; k++) {
int thisJointNum = c * 3 + k;
int nextJointNum = -1;
if (k != 2) {
nextJointNum = c * 3 + k + 1;
}
//compute the transform from one joint to the other
transf thisTran = jointTransf.at(c).at(k);
//relative transform in thisJoint's coordinate system
transf relTran = thisTran.inverse() % refTran;
vec3 translation = relTran.translation();
//for this hand, the z component should always be 0
if (translation.z() > 1.0e-3) {
DBGA("Z translation in McGrip routing matrix");
}
double tx, ty;
//in the math, I have used a different convention than link
//axes in hand geometry.
tx = translation.y();
ty = translation.x();
//this obviously means we will need to also flip the sign of the result
//since we changed the "handed-ness" of the coordinate system
DBGP("Joint translation: " << tx << " " << ty);
//get the joint angles
double theta = getChain(c)->getJoint(k)->getVal() +
getChain(c)->getJoint(k)->getOffset();
double theta_n = 0.0;
if (k != 2) {
theta_n = getChain(c)->getJoint(k + 1)->getVal() +
getChain(c)->getJoint(k + 1)->getOffset();
}
//compute the contributions
//negate the translation, as we want the vector from refJoint to thisJoint
//but still in thisJoint's system
assembleTorqueMatrices(refJointNum, thisJointNum, nextJointNum,
-tx, -ty,
theta, theta_n,
**B, **a);
}
}
}
//flip the signs; in the math a positive torque is an extension
//in the model, a positive torque is flexion
(*B)->multiply(-1.0);
(*a)->multiply(-1.0);
DBGP("B matrix:\n" << **B);
DBGP("a vector:\n" << **a);
}
