/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/dof.cpp

Go to the documentation of this file.

//######################################################################
//
// GraspIt!
// Copyright (C) 2002-2009  Columbia University in the City of New York.
// All rights reserved.
//
// GraspIt! is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// GraspIt! is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
//
// Author(s): Andrew T. Miller and Matei T. Ciocarlie
//
// $Id: dof.cpp,v 1.24 2009/07/27 17:00:17 cmatei Exp $
//
//######################################################################

#include "dof.h"

#include <QTextStream>

#include "robot.h"
#include "joint.h"
#include "dynJoint.h"
#include "body.h"
#include "world.h"

//#define GRASPITDBG
#include "debug.h"

DOF::DOF() : owner(NULL), q(0.0),desiredq(0.0), desiredVelocity(0.0),defaultVelocity(0.0),
        actualVelocity(0.0), maxAccel(10.0), force(0.0),maxForce(0.0),
        extForce(0.0), Kv(0.0),Kp(0.0),mHistoryMaxSize(10), mDynStartTime(0.0), currTrajPt(0), 
        draggerScale(20.0)
{
}

DOF::DOF(const DOF *original)
{
  owner = NULL;
  dofNum = original->dofNum;
  maxq = original->maxq;
  minq = original->minq;
  q = 0.0;
  desiredq = 0.0;

  defaultVelocity = original->defaultVelocity;
  maxAccel = original->maxAccel;
  desiredVelocity = 0.0;
  actualVelocity = 0.0;

  maxForce = original->maxForce;
  force = 0.0;
  extForce = 0.0;

  Kv = original->Kv;
  Kp = original->Kp;

  mHistoryMaxSize = original->mHistoryMaxSize;
  draggerScale = original->draggerScale;
  currTrajPt = 0;
}

void
DOF::initDOF(Robot *myRobot,const std::list<Joint *>& jList)
{
  owner = myRobot;  
  jointList = jList; // copy jList
  updateMinMax();
}

void DOF::updateMinMax()
{
  maxq = (*jointList.begin())->getMax()/getStaticRatio(*jointList.begin());
  minq = (*jointList.begin())->getMin()/getStaticRatio(*jointList.begin());
  if (maxq < minq) std::swap(maxq, minq);  
  std::list<Joint *>::iterator j;
  double testMin, testMax;
  int num = 0;
  for(j=++jointList.begin();j!=jointList.end();j++) {
    testMax = (*j)->getMax()/getStaticRatio(*j);
    testMin = (*j)->getMin()/getStaticRatio(*j);
    //can happen if ratio is negative
    if (testMax < testMin) std::swap(testMin, testMax);
    DBGP("Joint " << num++ << " min "  << testMin << " max " << testMax);
    maxq = ( testMax < maxq ? testMax : maxq);
    minq = ( testMin > minq ? testMin : minq);
    DBGP("maxq " << maxq << " minq " << minq);
  }
}

void
DOF::updateSetPoint() {
        if (!trajectory.empty()) {    
                setPoint = trajectory[currTrajPt++];
                if (trajectory.begin()+currTrajPt==trajectory.end()) {
                        trajectory.clear();
                }
        }else if (setPoint != desiredq) {
                setPoint=desiredq;
        }
        DBGP("Update set point: " << setPoint);
}

void
DOF::setTrajectory(double *traj,int numPts)
{
        trajectory.clear();
        currTrajPt = 0;
        trajectory.reserve(numPts);
        for (int i=0;i<numPts;i++) {
                trajectory.push_back(traj[i]);
        }
        desiredq = traj[numPts-1];
        mErrorHistory.clear();
        mPositionHistory.clear();
        mForceHistory.clear();
        //reset the dynamics start time
        mDynStartTime = owner->getWorld()->getWorldTime();
}

void
DOF::addToTrajectory(double *traj,int numPts)
{
        for (int i=0;i<numPts;i++){
                trajectory.push_back(traj[i]);
        }
        desiredq = trajectory.back();
}

void
DOF::callController(double timeStep)
{
        //we need to update the current position based on current joint values 
        //we rely on those having already been set from dynamics
        updateFromJointValues();

        //bookkeeping
        mErrorHistory.push_front(setPoint - q);
        while ((int)mErrorHistory.size() > mHistoryMaxSize) {
                mErrorHistory.pop_back();
        }
        //the error wrt the last desired position in the trajectory
        /*
        if (!trajectory.empty()) {
                mPositionHistory.push_front(trajectory.back() - q);
        } else {
                mPositionHistory.push_front(setPoint - q);
        }
        */
        if (!mPositionHistory.empty()) {
                mVelocityHistory.push_front( (q - mPositionHistory.front() ) / timeStep );
                while ((int)mVelocityHistory.size() > mHistoryMaxSize) {
                        mVelocityHistory.pop_back();
                }
        }

        mPositionHistory.push_front(q);
        while ((int)mPositionHistory.size() > mHistoryMaxSize) {
                mPositionHistory.pop_back();
        }
        
        //call the appropriate controller
        double newForce = PDPositionController(timeStep);
        //actually sets the force
        setForce(newForce);

        mForceHistory.push_front(newForce);
        while ((int)mForceHistory.size() > mHistoryMaxSize) {
                mForceHistory.pop_back();
        }
}

double
DOF::PDPositionController(double timeStep)
{
        double error = mErrorHistory.front(), lastError;
        if (mErrorHistory.size() >= 2) {
                lastError = *(++mErrorHistory.begin());
        } else {
                lastError = error;
        }

        double newForce;

        newForce = Kp * error + Kv * (error-lastError)/timeStep;

        DBGP("DOF " << getDOFNum() );
        DBGP("setPoint ="<<setPoint<<" error ="<<error<<" edot = "<<(error-lastError));
        DBGP("proportional: "<<error<<"*"<<Kp<<"="<<error*Kp);
        DBGP("derivative: "<<Kv<<"*"<<(error-lastError)/timeStep);
        DBGP("cap: "<<getMaxForce()*0.8 << " Force: "<<newForce);
        DBGP("e " << error << " de " << error-lastError << " ts " << timeStep << " f " << newForce);
        return newForce;
}

bool
DOF::dynamicsProgress()
{
        return true;
}

bool
DOF::readParametersFromXml(const TiXmlElement* root)
{
        if(!getDouble(root,"defaultVelocity", defaultVelocity))
                return false;
        if(!getDouble(root,"maxEffort", maxForce))
                return false;
        if(!getDouble(root,"Kp", Kp))
                return false;
        if(!getDouble(root,"Kd", Kv))
                return false;
        if(!getDouble(root,"draggerScale", draggerScale))
                return false;
        return true;
}

bool
DOF::writeToStream(QTextStream &stream)
{
        stream << q;
        return true;
}

bool
DOF::readFromStream(QTextStream &stream)
{
        if (stream.atEnd()) return false;
        stream >> q;
        return true;
}

void
RigidDOF::setForce(double f)
{
        Joint *activeJoint = *(jointList.begin());
        if (f > maxForce) force = maxForce;
        else if (f < -maxForce) force = -maxForce;
        else force = f;

        //set force for the first joint only, passive joint constraints will take care of the others
        activeJoint->applyInternalWrench(force);
        DBGP("Applied force = " << force);
}

double
RigidDOF::getClosestJointLimit(int *direction)
{
        bool firstTime = true;
        double closestLimit = 0.0;
        *direction = 0.0;
        std::list<Joint*>::iterator j;
        for (j=jointList.begin(); j!=jointList.end(); j++) {
                double val = (*j)->getVal();
                double maxError = val - (*j)->getMax();
                double minError = (*j)->getMin() - val;
                double error; int jdir;
                if (maxError > minError) {
                        error = maxError;
                        jdir = -1;
                } else {
                        error = minError;
                        jdir = +1;
                }
                error /= getStaticRatio(*j);
                if (firstTime || fabs(error) > fabs(closestLimit)) {
                        closestLimit = error;
                        *direction = jdir;
                        firstTime = false;
                }
        }
        return closestLimit;
}

int
RigidDOF::getNumLimitConstraints() 
{
        int d;
        if (getClosestJointLimit(&d) < -0.01) return 0;
        return 1;
}

void 
RigidDOF::buildDynamicLimitConstraints(std::map<Body*,int> &islandIndices, int numBodies, 
                                                                  double* H, double *g, int &hcn)
{       
        int dir;
        double error = getClosestJointLimit(&dir);
        if ( error < -0.01) return;

        g[hcn] = MIN(0.0, -error - 0.005);
        DBGP("adding dof limit constraint for DOF " << dofNum << " in dir " << dir << "; error: "<<g[hcn]);

        //the first joint in the list is considered the master joint
        Joint *currentJoint = jointList.front();
        DynamicBody *prevLink = currentJoint->dynJoint->getPrevLink();
        DynamicBody *nextLink = currentJoint->dynJoint->getNextLink();
   
        assert( islandIndices[prevLink]>=0 );
        int row = 6*islandIndices[prevLink];
        H[(hcn)*6*numBodies + row+3] -= dir * currentJoint->getWorldAxis()[0];
        H[(hcn)*6*numBodies + row+4] -= dir * currentJoint->getWorldAxis()[1];
        H[(hcn)*6*numBodies + row+5] -= dir * currentJoint->getWorldAxis()[2];

    assert( islandIndices[nextLink]>=0 );
        row = 6*islandIndices[nextLink];
        H[(hcn)*6*numBodies + row+3] +=  dir * currentJoint->getWorldAxis()[0];
        H[(hcn)*6*numBodies + row+4] +=  dir * currentJoint->getWorldAxis()[1];
        H[(hcn)*6*numBodies + row+5] +=  dir * currentJoint->getWorldAxis()[2];

        hcn++;
}

void 
RigidDOF::buildDynamicCouplingConstraints(std::map<Body*,int> &islandIndices, int numBodies, 
                                                                         double* Nu, double*, int &ncn)
{
        //again we consider the first joint as the master joint 
        Joint *masterJoint = jointList.front();
        
        DynamicBody *masterPrevLink = masterJoint->dynJoint->getPrevLink();
        DynamicBody *masterNextLink = masterJoint->dynJoint->getNextLink();
        double masterRatio = getStaticRatio(masterJoint);
        vec3 masterFreeRotAxis = masterJoint->getWorldAxis();

        std::list<Joint*>::iterator it = jointList.begin();
        while(++it != jointList.end()) {
                Joint *currentJoint = (*it);
                //for now this only works on revolute dynamic joints
                //which means made up of a single revolute static joint
                //(unlike universal dynamic joints which are made up of 2
                //revolute static joints)

                if (currentJoint->getType() != REVOLUTE) continue;
                
                DynamicBody *prevLink = currentJoint->dynJoint->getPrevLink();
                DynamicBody *nextLink = currentJoint->dynJoint->getNextLink();
                vec3 freeRotAxis = currentJoint->getWorldAxis();

                //the *rotation* between these joint links...
                double ratio = 1.0/getStaticRatio(currentJoint);
                assert( islandIndices[prevLink] >= 0 );
                int row = 6*islandIndices[prevLink];
                Nu[(ncn)*6*numBodies + row+3] -= ratio * freeRotAxis[0];
                Nu[(ncn)*6*numBodies + row+4] -= ratio * freeRotAxis[1];
                Nu[(ncn)*6*numBodies + row+5] -= ratio * freeRotAxis[2];
                assert( islandIndices[nextLink] >= 0 );
                row = 6*islandIndices[nextLink];
                Nu[(ncn)*6*numBodies + row+3] +=  ratio * freeRotAxis[0];
                Nu[(ncn)*6*numBodies + row+4] +=  ratio * freeRotAxis[1];
                Nu[(ncn)*6*numBodies + row+5] +=  ratio * freeRotAxis[2];

                //... must be the same as the movement between the master joint links
                ratio = 1.0/masterRatio;
                assert( islandIndices[masterPrevLink] >= 0 );
                row = 6*islandIndices[masterPrevLink];
                Nu[(ncn)*6*numBodies + row+3] += ratio * masterFreeRotAxis[0];
                Nu[(ncn)*6*numBodies + row+4] += ratio * masterFreeRotAxis[1];
                Nu[(ncn)*6*numBodies + row+5] += ratio * masterFreeRotAxis[2];
                assert( islandIndices[masterNextLink] >= 0 );
                row = 6*islandIndices[masterNextLink];
                Nu[(ncn)*6*numBodies + row+3] += - ratio * masterFreeRotAxis[0];
                Nu[(ncn)*6*numBodies + row+4] += - ratio * masterFreeRotAxis[1];
                Nu[(ncn)*6*numBodies + row+5] += - ratio * masterFreeRotAxis[2];

                ncn++;          

                //there should be a corrective term added here!!!
        }
}

double 
RigidDOF::getStaticRatio(Joint *j) const 
{
        return j->getCouplingRatio();
}

void
RigidDOF::getJointValues(double *jointVals) const
{
        std::list<Joint*>::const_iterator j;
        for(j=jointList.begin();j!=jointList.end();j++) {
                jointVals[ (*j)->getNum() ] = q * getStaticRatio(*j);
        }
}

bool
RigidDOF::accumulateMove(double q1, double *jointVals, int *stoppedJoints)
{
        if ( fabs(q-q1) < 1.0e-5 ) return false;
        std::list<Joint*>::iterator j;
        if (stoppedJoints) {
                for(j=jointList.begin();j!=jointList.end();j++) {
                        if ( stoppedJoints[ (*j)->getNum()] ) return false;
                }
        }
        for(j=jointList.begin();j!=jointList.end();j++) {
                jointVals[ (*j)->getNum() ] = q1 * getStaticRatio(*j);
        }
        return true;
}

void
RigidDOF::updateFromJointValues(const double *jointVals)
{
        assert(!jointList.empty());
        double val;
        if (jointVals) {
                val = jointVals[ jointList.front()->getNum() ];
        }
        else {
                val = jointList.front()->getVal();
        }
        q = val / getStaticRatio(jointList.front());
        DBGP("DOF " << getDOFNum() << ": set value " << q);
}

BreakAwayDOF::~BreakAwayDOF()
{
        if (mInBreakAway) delete [] mInBreakAway;
        if (mBreakAwayValues) delete [] mBreakAwayValues;
}

void BreakAwayDOF::initDOF(Robot *myRobot, const std::list<Joint*> &jList)
{
        DOF::initDOF(myRobot, jList);
        int size = (int)jList.size();
        assert(size>0);
        mInBreakAway = new int[size];
        mBreakAwayValues = new double[size];
        for (int i=0; i<size; i++) {
                mInBreakAway[i] = 0;
                //maybe useful for debugging purposes; should not matter what we put here
                mBreakAwayValues[i] = -10;
        }
}

void
BreakAwayDOF::getJointValues(double *jointVals) const
{
        int index = -1;
        std::list<Joint*>::const_iterator j;
        for (j=jointList.begin(); j!=jointList.end(); j++) {    
                index++;
                if (mInBreakAway[index] && q > mBreakAwayValues[index]) {
                        jointVals[ (*j)->getNum() ] = mBreakAwayValues[index] * getStaticRatio(*j);
                } else {
                        jointVals[ (*j)->getNum() ] = q * getStaticRatio(*j);
                }
        }
}

bool
BreakAwayDOF::accumulateMove(double q1, double *jointVals, int *stoppedJoints)
{
        if ( fabs(q-q1) < 1.0e-5 ) {
                DBGP("DOF new value same as current value");
                return false;
        }
        std::list<Joint*>::iterator j;
        //check if we are moving in the negative direction of the DOF and some joint is stopped
        for (j=jointList.begin(); j!=jointList.end(); j++) {    
                if (stoppedJoints && stoppedJoints[ (*j)->getNum()] ) {
                        if (q1 < q){
                                DBGP("Joint stopped in the negative direction; stopping all movement");
                                return false;
                        }
                }
        }

        //we are moving in the positive direction of the DOF
        bool movement = false;
        int index = -1;
        for (j=jointList.begin(); j!=jointList.end(); j++) {    
                index++;
                if (mInBreakAway[index] && q1 > mBreakAwayValues[index]) {
                        DBGP("Joint " << index << " is in break away");
                        continue;
                }
                double newVal = q1 * getStaticRatio(*j);
                DBGP("q1 " << q1 << " * ratio " << getStaticRatio(*j) << " = " << newVal);
                double oldVal = (*j)->getVal();
                if (stoppedJoints) {
                        if (newVal > oldVal && (stoppedJoints[ (*j)->getNum()] & 1) ) {
                                DBGP("Joint " << index << " is stopped in the positive direction");
                                continue;
                        }
                        if (newVal < oldVal && (stoppedJoints[ (*j)->getNum()] & 2) ) {
                                DBGP("Joint " << index << " is stopped in the negative direction");
                                continue;
                        }
                }
                jointVals[ (*j)->getNum() ] = newVal;
                movement = true;
        }
        return movement;
}

void
BreakAwayDOF::updateVal(double q1)
{
        std::list<Joint*>::iterator j;
        int index;
        for(j=jointList.begin(), index=0; j!=jointList.end(); j++,index++) {
                double jVal = (*j)->getVal() / getStaticRatio(*j);
                if (mInBreakAway[index]) {
                        if (q1 < mBreakAwayValues[index] - 1.0e-5) {
                                mInBreakAway[index] = false;
                                DBGP("DOF " << dofNum << " joint " << index << " re-engaged");
                        }
                } else {
                        if (jVal < q1 - 1.0e-5) {
                                DBGP("DOF " << dofNum << " joint " << index << " disengaged at " << jVal);
                                mInBreakAway[index] = true;
                                mBreakAwayValues[index] = jVal;
                        }
                }
        }
        q = q1;
}

void
BreakAwayDOF::updateFromJointValues(const double *jointVals)
{
        std::list<Joint*>::iterator j;
        int index;
        double val;
        for(j=jointList.begin(),index=0; j!=jointList.end(); j++,index++) {
                if (mInBreakAway[index]) continue;
                if (!jointVals) {
                        val = (*j)->getVal() / getStaticRatio(*j);
                } else {
                        val = jointVals[ (*j)->getNum() ] / getStaticRatio(*j);
                }
                break;
        }
        //all joints in breakaway?
        assert( j!= jointList.end() );
        q = val;
        DBGP("DOF " << getDOFNum() << ": set value " << q);
}

void
BreakAwayDOF::reset()
{
        if (!mInBreakAway) return;
        for (int j=0; j<(int)jointList.size(); j++) {
                mInBreakAway[j] = false;
        }
}

bool
BreakAwayDOF::writeToStream(QTextStream &stream)
{
        stream << q;
        for (int j=0; j<(int)jointList.size(); j++) {
                if (!mInBreakAway || !mInBreakAway[j]) {
                        stream << " " << 0;
                } else {
                        stream << " " << 1 << " " << mBreakAwayValues[j];
                }
        }
        return true;
}

double
BreakAwayDOF::getSaveVal() const
{
        if (!mInBreakAway) return q;
        double val = q;
        for (int j=0; j<(int)jointList.size(); j++) {
                if ( mInBreakAway[j] ) {
                        if ( mBreakAwayValues[j] < val ) {
                                val = mBreakAwayValues[j];
                        }
                }
        }
        return val;
}

bool
BreakAwayDOF::readFromStream(QTextStream &stream)
{
        if (stream.atEnd()) return false;
        stream >> q;
        DBGP("Value: " << q);
        for (int j=0; j<(int)jointList.size(); j++) {
                assert(mInBreakAway);
                stream >> mInBreakAway[j];
                DBGP("  bway: " << mInBreakAway[j]);
                if (mInBreakAway[j] == 1) {
                        stream >> mBreakAwayValues[j];
                } else if (mInBreakAway[j]==0) {
                        mBreakAwayValues[j] = -10;
                } else {
                        return false;
                }
        }
        return true;
}

bool
BreakAwayDOF::readParametersFromXml(const TiXmlElement* root)
{
        if (!DOF::readParametersFromXml(root)) return false;
        if(!getDouble(root,"breakAwayTorque", mBreakAwayTorque)||mBreakAwayTorque < 0){
                //the break away torque is missing; warn and use default value
                DBGA("BreakAway torque missing or negative, using default value 0.0");
                mBreakAwayTorque = 0.0f;
        } else {
                //convert to N mm
                mBreakAwayTorque *= 1.0e3;
        }
        return true;
}

bool
BreakAwayDOF::computeStaticJointTorques(double *jointTorques, double)
{
        std::list<Joint*>::iterator j;
        int index;
        for(j=jointList.begin(), index=0; j!=jointList.end(); j++,index++) {
                if (mInBreakAway[index]) {
                        jointTorques[ (*j)->getNum() ] += mBreakAwayTorque;
                }
        }
        return true;
}

void
CompliantDOF::initDOF(Robot *myRobot, const std::list<Joint*> &jList)
{
        DOF::initDOF(myRobot, jList);
        std::list<Joint*>::iterator j;
        for(j=jointList.begin(); j!=jointList.end(); j++) {
                if ( (*j)->getSpringStiffness() == 0.0 ) {
                        DBGA("ERROR: Compliant joint has no stiffness! DEFAULT VALUE will be used!");
                }
        }
}

double 
CompliantDOF::getStaticRatio(Joint *j) const {
        double ref = jointList.front()->getSpringStiffness();
        assert(ref!=0.0);
        double k = j->getSpringStiffness();
        if (k == 0.0) k = ref;
        /* this is actually incorrect. I will hard-code in the right relationship
                for the HH, but in the future, todo: generalize this */
        //return j->getCouplingRatio() * ( ref / k );
        int jn = j->getNum();
        if (jn==0 || jn==2 || jn== 4 || jn==6) {
                return j->getCouplingRatio();
        } else {
                double tau2 = j->getCouplingRatio();
                double tau1 = jointList.front()->getCouplingRatio();
                return (ref / k) * ( tau1 * tau2) / (tau1 + tau2);
        } 
}

void 
CompliantDOF::getJointValues(double* jointVals) const 
{
        std::list<Joint*>::const_iterator j;
        for (j=jointList.begin(); j!=jointList.end(); j++) {    
                jointVals[ (*j)->getNum() ] = q * getStaticRatio(*j);
        }
}

void 
CompliantDOF::updateFromJointValues(const double* jointVals)
{
        double max = -1.0e5;
        std::list<Joint*>::const_iterator j;
        for (j=jointList.begin(); j!=jointList.end(); j++) {
                double v;
                if (jointVals) {
                        v = jointVals[ (*j)->getNum() ] / getStaticRatio(*j);
                } else {
                        v = (*j)->getVal() / getStaticRatio(*j);
                }
                max = std::max(max,v);
        }
        q = max;
}

bool 
CompliantDOF::accumulateMove(double q1, double *jointVals, int *stoppedJoints)
{
        if ( fabs(q-q1) < 1.0e-5 ) {
                DBGP("DOF is already at desired value");
                return false;
        }

        bool movement = false;
        std::list<Joint*>::const_iterator j; int index;
        for (j=jointList.begin(), index=0; j!=jointList.end(); j++, index++) {  
                double newVal = q1 * getStaticRatio(*j);
                double oldVal = (*j)->getVal();
                if (stoppedJoints) {
                        if (newVal > oldVal && (stoppedJoints[ (*j)->getNum()] & 1) ) {
                                DBGP("Joint " << index << " is stopped in the positive direction");
                                continue;
                        }
                        if (newVal < oldVal && (stoppedJoints[ (*j)->getNum()] & 2) ) {
                                DBGP("Joint " << index << " is stopped in the negative direction");
                                continue;
                        }
                }
                jointVals[ (*j)->getNum() ] = newVal;
                movement = true;
        }
        return movement;
}


bool 
CompliantDOF::computeStaticJointTorques(double *jointTorques, double dofForce)
{
        //this version assumes that dof force is applied as pure joint
        //torque. This is probably not physically accurate for most actual hands
        //dof force ends up as a force on the link, and then joint forces will have
        //to be computed as JT * link_forces

        bool retVal = true;
        //add the spring forces for all joints
        double maxDofTorque = 0.0;
        std::list<Joint*>::iterator j;
        Joint *pj = NULL;
        int count = 0;
        for(j=jointList.begin(); j!=jointList.end(); j++) {
                double springTorque = (*j)->getSpringForce();
                DBGP("Inner spring: " << springTorque);
                jointTorques[ (*j)->getNum() ] -= springTorque;
                //also propagate this to previous joint
                if (count==1 || count==3 || count==5 || count==7) {
                        assert(pj);
                        vec3 axis1 = (*j)->dynJoint->getPrevLink()->getTran().affine().row(2);
                        vec3 axis2 =   pj->dynJoint->getPrevLink()->getTran().affine().row(2);
                        double t = fabs(axis1 % axis2);
                        //todo what about non-revolute joints, complex kinematic chains, etc...
                        jointTorques[pj->getNum()] += springTorque * t;
                }
                pj = (*j);
                count++;
        }
        /*
        std::cerr << "before max torque:\n";
        for(j=jointList.begin(); j!=jointList.end(); j++) {
          std::cerr << jointTorques[ (*j)->getNum() ] << " ";
        }
        std::cerr << "\n";
        */
        //what is the max torque that the dof must balance at any joint
        for(j=jointList.begin(); j!=jointList.end(); j++) {
                double springTorque = - jointTorques[(*j)->getNum()];
                double dofTorque = springTorque / (*j)->getCouplingRatio();
                if (fabs(dofTorque) > fabs(maxDofTorque)) {
                        maxDofTorque = dofTorque;
                }
        }
        DBGP("Max dof torque: " << maxDofTorque);

        double dofTorque;
        if (dofForce < 0) {
                //dof force has not been passed in. We thus compute the case where all we
                //want is to keep the joints in their current position. Therefore, the dof
                //force has to balance the max spring force found at any joint
                dofTorque = maxDofTorque;
        } else {
                //we have some level of dof force applied. Use that one.
                //hard-coded 4cm moment arm, same as in setForce. At some point we'll have
                //to use actual insertion points
                dofTorque = force * 40.0;
                if (dofTorque < maxDofTorque) {
                        DBGA("For now, dof torque must at least balance spring forces!");
                        //give it a little bit of extra woomph
                        dofTorque = maxDofTorque + 3.0e7;
                }
        }

        //now apply the dof torque to all joints
        for(j=jointList.begin(); j!=jointList.end(); j++) {
                double jointTorque = dofTorque * (*j)->getCouplingRatio();
                jointTorques[ (*j)->getNum() ] += jointTorque;
                if ( jointTorques[ (*j)->getNum() ] < -1.0e-5) {
                        DBGP("Error: negative torque on joint " << (*j)->getNum());
                        //now that spring torques are also propagated back, this is no longer an error
                        //retVal = false;
                }
                //try to fix numerical errors at least for 0, since it is a special case
                if ( fabs(jointTorques[(*j)->getNum()]) < 1.0e-5) {
                  jointTorques[ (*j)->getNum() ] = 0.0;
                }
                DBGP(jointTorques[ (*j)->getNum() ]);
        }
        /*
        std::cerr << "after max torque:\n";
        for(j=jointList.begin(); j!=jointList.end(); j++) {
          std::cerr << jointTorques[ (*j)->getNum() ] << " ";
        }
        std::cerr << "\n";
        */
        return retVal;
}

int 
CompliantDOF::getNumLimitConstraints()
{
        int numCon = 0;
        std::list<Joint*>::iterator j;
        for(j=jointList.begin(); j!=jointList.end(); j++) {
                if ( (*j)->getVal() >= (*j)->getMax() - 0.01) numCon ++;
                else if ( (*j)->getVal() <= (*j)->getMin() + 0.01) numCon ++;
        }
        return numCon;
}

void 
CompliantDOF::buildDynamicLimitConstraints(std::map<Body*,int> &islandIndices, int numBodies, 
                                                                                   double* H, double *g, int &hcn)
{
        std::list<Joint*>::iterator j;
        int count = 0;
        for(j=jointList.begin(); j!=jointList.end(); j++) {
                Joint *currentJoint = *j;
                DynamicBody *prevLink = currentJoint->dynJoint->getPrevLink();
                DynamicBody *nextLink = currentJoint->dynJoint->getNextLink();
                int row, dir;
                double error;
                if (currentJoint->getVal() >= currentJoint->getMax()-0.01) {
                        error = currentJoint->getMax() - currentJoint->getVal();
                        dir = -1;
                } else if (currentJoint->getVal() <= currentJoint->getMin()+0.01) {
                        error = currentJoint->getVal() - currentJoint->getMin();
                        dir = +1;
                } else continue;
                    
                g[hcn] = MIN(0.0, error - 0.005);
                DBGP("adding dof limit constraint for DOF " << dofNum << "  error: "<<g[hcn]);

                assert( islandIndices[prevLink]>=0 );
                row = 6*islandIndices[prevLink];
                H[(hcn)*6*numBodies + row+3] -= dir * currentJoint->getWorldAxis()[0];
                H[(hcn)*6*numBodies + row+4] -= dir * currentJoint->getWorldAxis()[1];
                H[(hcn)*6*numBodies + row+5] -= dir * currentJoint->getWorldAxis()[2];

        assert( islandIndices[nextLink]>=0 );
                row = 6*islandIndices[nextLink];
                H[(hcn)*6*numBodies + row+3] +=  dir * currentJoint->getWorldAxis()[0];
                H[(hcn)*6*numBodies + row+4] +=  dir * currentJoint->getWorldAxis()[1];
                H[(hcn)*6*numBodies + row+5] +=  dir * currentJoint->getWorldAxis()[2];

                hcn++;
                count++;
        }
}

void 
CompliantDOF::setForce(double f)
{
        //cap the force
        if (f > maxForce) force = maxForce;
        if (force < 0) force = 0;
        else if (f < -maxForce) force = -maxForce;
        else force = f;

        //apply it to links. compute moment arms in order to obtain required torque ratios
        //reference is a moment arm of 40mm
        std::list<Joint*>::iterator j;
        for(j=jointList.begin(); j!=jointList.end(); j++) {
                double torqueRatio = (*j)->getCouplingRatio();
                DynamicBody* body = (*j)->dynJoint->getNextLink();
                //assume a pure torque is applied to the joint
                //easier to match in the statics computation
                //assume a moment arm of 40mm for the conversion
                //could keep dof effort as torques in the files, but this is how we have it now
                double jointTorque = force * 40.0 * torqueRatio;
                //since a pure torque is applied, maybe we just need to apply to the next link
                //and let joint constraints take care of the rest?
                body->addTorque(jointTorque * (*j)->getWorldAxis());
                
                /*
                //assume force is applied to the link - probably more correct
                //joint location in body coordinates
                vec3 bodyJoint = (*j)->dynJoint->getNextTrans().translation();
                //vector from joint location to body cog in body coordinates
                vec3 cog = (body->getCoG()  - position::ORIGIN) - bodyJoint;
                //joint axis in body coordinates
                vec3 axis = (*j)->getWorldAxis() * body->getTran().inverse();
                //force is cross product of the two, scaled to desired force magnitude
                vec3 forceVector = force * normalise( axis * cog );
                //scale the arm based on how much torque we want around the joint
                //40mm arm is reference
                vec3 arm = 40.0 * torqueRatio * normalise(cog);
                //position on body
                vec3 bodyPosition = bodyJoint + arm;
                position bodyPos(bodyPosition.x(), bodyPosition.y(), bodyPosition.z());
                body->addForceAtRelPos(forceVector, bodyPos);
                */
        }
}

double
CompliantDOF::PDPositionController(double timeStep)
{
        //todo find a better home for the PD controller (at the DOF level)
        return smoothProfileController(timeStep);
        //return DOF::PDPositionController(timeStep);
        //return getMaxForce();
}

bool
CompliantDOF::dynamicsProgress()
{
        double eps = 1.0e-5;
        if ((int)mPositionHistory.size() < mHistoryMaxSize) return true;
        std::list<double>::iterator it = mPositionHistory.begin();
        double ref = *it; it++;
        //fprintf(stderr,"%f: ", ref);
        while (it!=mPositionHistory.end()) {
                //fprintf(stderr,"%f ", *it);
                if ( fabs(*it-ref) > eps) {
                        //fprintf(stderr,"move.\n");
                        return true;
                }
                it++;
        }
        //fprintf(stderr,"no move.\n");
        return false;
}

double 
CompliantDOF::smoothProfileController(double)
{
        double time = owner->getWorld()->getWorldTime() - mDynStartTime;
        time = std::min(time, 1.0);
        if (time < 0) {
                DBGA("Zero elapsed time in CD controller");
        }
        time = std::max(time, 0.0);
        return time * getMaxForce();
}

---

graspit
Author(s):
autogenerated on Wed Jan 25 10:58:52 2012