Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members  

SIG_DynaMechsCommandInterface.cpp

00001 #include "SIGEL_Simulation/SIG_DynaMechsCommandInterface.h"
00002 #include "SIGEL_Tools/SIG_IO.h"
00003 
00004 #include <pointvector.h>
00005 #include <cmath>
00006 
00007 SIGEL_Simulation::SIG_DynaMechsCommandInterface::SIG_DynaMechsCommandInterface(SIG_DynaMechsSimulationData& theSimulationData)
00008   : simulationData(theSimulationData)
00009 { };
00010 
00011 void SIGEL_Simulation::SIG_DynaMechsCommandInterface::moveDrive(int driveNo,
00012                                                                 QVector<SIG_Register> const& registers)
00013 {
00014 #ifdef SIG_DEBUG
00015   SIGEL_Tools::SIG_IO::cerr << "entering method moveDrive\n";
00016 #endif
00017   SIGEL_Robot::SIG_Drive *drive = 0;
00018 
00019   int driveIndex = 0;
00020 
00021   if (simulationData.drives.size()>0)
00022     {
00023       long double minRegisterValue = - std::pow( static_cast<long double>(2),
00024                                                  static_cast<long double>(registers[0]->getSize() - 1) );
00025 
00026       int absoluteDriveNo = driveNo - static_cast< int >(minRegisterValue);
00027 
00028       driveIndex = absoluteDriveNo % simulationData.drives.size();
00029 
00030 #ifdef SIG_DEBUG
00031       SIGEL_Tools::SIG_IO::cerr << "Drive index: "
00032                                 << driveIndex
00033                                 << "\n";
00034 #endif
00035 
00036       drive = simulationData.drives[ driveIndex ];
00037     };
00038 
00039   if (drive)
00040     {
00041 #ifdef SIG_DEBUG
00042       SIGEL_Tools::SIG_IO::cerr << "Drive exists!\n";
00043 #endif
00044       long double minRegisterValue = - ( std::pow( static_cast<long double>(2),
00045                                                    static_cast<long double>(registers[0]->getSize() - 1) )
00046                                          - 1 );
00047 
00048       long double registerValueRange = - minRegisterValue * 2;
00049 
00050       int registerValue = registers[0]->getValue();
00051 
00052       long double absoluteRegisterValue = registerValue - minRegisterValue;
00053 
00054       if (absoluteRegisterValue<0)
00055         absoluteRegisterValue = 0;
00056 
00057       long double force = (   ( absoluteRegisterValue / registerValueRange )
00058                             * ( 2 * drive->getMaxForce() ) )
00059                           - drive->getMaxForce();
00060 
00061 #ifdef SIG_DEBUG
00062       SIGEL_Tools::SIG_IO::cerr << "minRegisterValue: "
00063                                 << static_cast< double >(minRegisterValue)
00064                                 << "\n"
00065                                 << "registerValueRange: "
00066                                 << static_cast< double >(registerValueRange)
00067                                 << "\n"
00068                                 << "registerValue: "
00069                                 << registerValue
00070                                 << "\n"
00071                                 << "maxForce: "
00072                                 << drive->getMaxForce()
00073                                 << "\n";
00074 #endif
00075 
00076       if (std::abs(force) >= drive->getMinForce())
00077         {
00078           double jointInput = static_cast< double >(force);
00079 
00080 #ifdef SIG_DEBUG
00081           SIGEL_Tools::SIG_IO::cerr << "Applying force/torque "
00082                                     << jointInput
00083                                     << " at drive "
00084                                     << drive->getName()
00085                                     << ".\n";
00086 #endif
00087 
00088           int linkNumber = simulationData.jointIndices[ drive->getJoint()->getNumber() ];
00089 
00090           SIG_DynaMechsLink *dynaMechsLink = simulationData.dynaMechsLinks[ linkNumber ];
00091 
00092           dynaMechsLink->dynaMechsLink->setJointInput( &jointInput );
00093 
00094           double forceDuration = simulationData.robot.getLangParam()->getCommand( "MOVE" )->getDuration();
00095 
00096           simulationData.driveForcesTimeAccounts[ driveIndex ] = forceDuration;
00097         };
00098     };
00099 };

Generated at Mon Sep 3 01:32:19 2001 for PG 368 - SIGEL by doxygen1.2.3 written by Dimitri van Heesch, © 1997-2000