Iunctus.cpp

Go to the documentation of this file.

// Iunctus.cpp: implementation of the Iunctus class.
//

#include "Iunctus.h"
#include "Simulator.h"
#include "Circle.h"
#include "CappedRectangle.h"

// Construction/Destruction


Iunctus::Iunctus(real x, real y, std::string label):
   ArticulatedAgentQuasistatic(label),myBody(NULL),controller(NULL){
   eye=NULL;
   build(x, y);
   controller=new RandomController(24, 16, 56789);
}

Iunctus::~Iunctus(){
   delete eye;
   delete controller;
}

void Iunctus::build(real x, real y){
   ArticulatedLink* link;
   ArticulatedLink* hand;
   ArticulatedLimb* limb;
   const real kFactorFirstLink=20.0f;
   const real kFactorSecondLink=10.0f;
   const real kFactorThirdLink=5.0f;
   int sign;
   r.setXY(x,y);
   //addObject(eye,0.0f,0.0f,0.0f);
   //create body
   myBody=new Circle(0.2f, 0, 0, "body", 10);
   addObject(myBody,0.0f,0.0f,0.0f);
   computeMemberPositions();
   computeIStar0();
   //add eye
   eye=new VisualSensor(myBody, 0,0, M_PI/2, 0.2f, M_PI/3, 10);
   for(int i=0;i<2;i++){
      sign= i==0 ? 1 : -1;
      limb=addLimb(0.25f, i==0 ? 10.0f*degrees : 170.0f*degrees);
      //create link 0 - first arm segment
      link=limb->addOneObjectLink(0.40f,sign*10.0f*degrees,
                                    new CappedRectangle(0.20f, 0.05f, 0, 0, 0, "arm 1", 10, 5), 
                                    0.20f,0.0f, 0.0f, NULL);
      link->k=kFactorFirstLink*DEFAULT_K;
      //create link 1 - second arm segment
      link=limb->addOneObjectLink(0.40f,sign*40.0f*degrees,
                                    new CappedRectangle(0.15f,0.05f, 0, 0, 0, "arm 2", 10, 5), 
                                    0.25f,0.0f, 0.0f, link);
      link->k=kFactorSecondLink*DEFAULT_K;
      //link->setNChildLinks(1);
      //create link 2 - hand
      link=limb->addOneObjectLink(0.10f,sign*10.0f*degrees,
                                    new Circle(0.05f, 0, 0, "hand", 5), 
                                    0.10f,0.0f, 0.0f, link);
        link->theta0Min=-30.0f*degrees;
        link->theta0Max=30.0f*degrees;
      
      link->k=kFactorThirdLink*DEFAULT_K;
      hand=link;
      //link->setNChildLinks(2);
      //create link 3 - finger right 1
      link=limb->addOneObjectLink(0.17f,-sign*60.0f*degrees,
                                    new CappedRectangle(0.05f,0.02f, 0, 0, 0, "finger right 1", 10, 5), 
                                    0.12f,0.0f, 0.0f, hand);
      if(i==0){
         link->theta0Min=-90.0*degrees;
         link->theta0Max=-30.0*degrees;
      } else {
         link->theta0Min=30.0f*degrees;
         link->theta0Max=90.0f*degrees;
      }
      //link->setNChildLinks(1);
      //create link 4 - finger right 2
      link=limb->addOneObjectLink(0.14f,sign*60.0f*degrees,
                                    new CappedRectangle(0.05f,0.02f, 0, 0, 0, "finger right 2", 10, 5), 
                                    0.09f,0.0f, 0.0f, link);
      if(i==0){
         link->theta0Min=30.0*degrees;
         link->theta0Max=90.0*degrees;
      } else {
         link->theta0Min=-90.0f*degrees;
         link->theta0Max=-30.0f*degrees;
      };
      //end finger right
      //create link 5 - finger left 1
      link=limb->addOneObjectLink(0.17f,sign*60.0f*degrees,
                                    new CappedRectangle(0.05f, 0.02f, 0, 0, 0, "finger left 1", 10, 5), 
                                    0.12f,0.0f, 0.0f, hand);
      link->theta0=sign*60.0*degrees;
      if(i==0){
         link->theta0Min=30.0*degrees;
         link->theta0Max=90.0*degrees;
      } else {
         link->theta0Min=-90.0f*degrees;
         link->theta0Max=-30.0f*degrees;
      };
      //create link 6 - finger left 2
      link=limb->addOneObjectLink(0.14f,-sign*60.0f*degrees,
                                    new CappedRectangle(0.05f,0.02f, 0, 0, 0, "finger left 2", 10, 5), 
                                    0.09f,0.0f, 0.0f, link);
      if(i==0){
         link->theta0Min=-90.0*degrees;
         link->theta0Max=-30.0*degrees;
      } else {
         link->theta0Min=30.0f*degrees;
         link->theta0Max=90.0f*degrees;
      }
   }
   //we call forwardKinematics() for updating the absolute positions of the objects
   forwardKinematics();
   setFillColor(Color("wheat"));
   setOutlineColor(Color("slateGray"));
}


void Iunctus::proprioception(){
   unsigned int k;
   ArticulatedLink* link;
   ArticulatedLimb* limb;
   ArticulatedLimbPVector::iterator limbsI, limbsEnd;
   ArticulatedLinkPVector::iterator linksI, linksEnd; 

   k=0;
   for(limbsI=limbs.begin(), limbsEnd=limbs.end(); limbsI!=limbsEnd; limbsI++){
      limb=*limbsI;
      for (linksI=limb->links.begin(), linksEnd=limb->links.end(); linksI!=linksEnd; linksI++){
         link=*linksI;
         assert(link);
         controller->setInput(k, (link->theta-link->theta0Min)/(link->theta0Max-link->theta0Min) );
         k++;
      }
   }
}


void Iunctus::controll(){
   int i, k;
   ArticulatedLink* link;
   ArticulatedLimb* limb;
   ArticulatedLimbPVector::iterator limbsI, limbsEnd;
   ArticulatedLinkPVector::iterator linksI, linksEnd;
   
   const real rocketForceFactor=10.0; //was 2.0 before

   //***get the output of the controller

   //update motor commands issued by the controller, for link movement
   k=0;
   i=0;
   for(limbsI=limbs.begin(), limbsEnd=limbs.end(); limbsI!=limbsEnd; limbsI++){
      i++;
      limb=*limbsI;
      for (linksI=limb->links.begin(), linksEnd=limb->links.end(); linksI!=linksEnd; linksI++){
         link=*linksI;
         assert(link);
         /*
         link->theta0+=((real)rand()/RAND_MAX-0.5)/10.0;
         if(link->theta0<link->theta0Min) link->theta0=link->theta0Min;
         if(link->theta0>link->theta0Max) link->theta0=link->theta0Max;
         */
         if(i==1)
            link->theta0=link->theta0Min+(link->theta0Max-link->theta0Min)*controller->getOutput(k);
         else
            link->theta0=link->theta0Max-(link->theta0Max-link->theta0Min)*controller->getOutput(k);
         k++;
      }
   }
   //update motor commands for body movement
   //the torque
   betaExternal.x=(controller->getOutput(14)-0.5)*rocketForceFactor*myBody->R;
   betaExternal.y=0; //for resetting mouse force
   //the forward "rocket" force
   betaExternal.z=(controller->getOutput(15)-0.5)*rocketForceFactor;

   //***set the input of the controller
   //...
   
   //***advance controller time
   controller->advanceTime();
}



void Iunctus::draw(GUI *gui){
   ArticulatedAgentQuasistatic::draw(gui);
   /*
   //draw body axis
   p->dc->DrawLine(p->panX+p->zoomX*myBody->r.x,p->panY+p->zoomY*myBody->r.y, 
      p->panX+p->zoomX*(myBody->r.x+myBody->R*cos(myBody->alpha+M_PI/2)),
      p->panY+p->zoomY*(myBody->r.y+myBody->R*sin(myBody->alpha+M_PI/2)));
      */
   /*
   //draw body force
   Vector2 f;
   f.y=betaExternal.z;
   f.rotate(myBody->alpha);
   p->drawForce(myBody->r,f);
   */

   //draw body torque
   //p->drawTorque(myBody->r,myBody->alpha+M_PI/2,betaExternal.x);
   
   //draw body eye
   real eyeAngle=M_PI/6;
   real x, y, xOld, yOld;
   xOld = myBody->r.x;
   yOld = myBody->r.y;
   for(int i = 0; i <= 10; ++i) {
      x = myBody->r.x+myBody->R*0.75*cos(myBody->alpha+M_PI/2+eyeAngle*(2*i/10.0-1));
      y = myBody->r.y+myBody->R*0.75*sin(myBody->alpha+M_PI/2+eyeAngle*(2*i/10.0-1));
      gui->drawLine(xOld, yOld, x, y);
      xOld=x; yOld=y;
   }
   x = myBody->r.x;
   y = myBody->r.y;
   gui->drawLine(xOld, yOld, x, y);
   //if(eye) eye->draw(gui);
}

Thyrix homepage • Users' guide

(C) Arxia 2004-2005