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 };