Kasutaja tarvikud

Lehe tööriistad


juhendid:msrs_simulatsioon

Microsoft Robotics Studio simulatsiooni koostamine

Sissejuhatus

Paljud, kes tegelenud robootika ja eelkõige algoritmidega, on ilmselt tundnud, et roboti pidev testimine võib olla tüütu ja aeganõudev tegevus. Kui robot on suure liikuvusega ja programmi pealelaadimiseks peab seda igakord arvuti juurde tooma ning oma positsioonile tagasi asetama jääb sisuliste probleemidega tegelemiseks vähe aega. Samuti on roboti algoritmist vigade otsimine raske kui robot tegutseb kiiresti ja tagasiside edastamine komplitseeritud. Ning robot pole mitte alati valmis testimiseks, sest sellel tuleb vahepeal ka akusid laadida, seda tuleb mehhaanikutel modifitseerida või pole seda parasjagu üldse kohal.

Kõik need ja paljud teised valupunktid on pannud aluse robotite simuleerimisele. Sarnaselt muude valdkondadegi on simulatsiooni eesmärk kiirendada ja lihtsustada arendustööd, hoida kokku ressursse reaalsete katseobjektide loomiselt ja vähendada riske vigastuste tekkimiseks. Kui robot või selle osa on suhteliselt lihtsa ja ennustatava käitumisega, saab simulatsioone teha kasvõi pliiatsi ja paberiga erinevaid variante läbi proovides. Kui tegu on aga muutlikus keskkonnas toimiva või paljude liikumiskombinatsioonidega robotiga jääb inimese mõttetegevusest väheks ning simuleerimine tuleb arvuti hoolde anda.

Robotite simuleerimine

Üks kindel asi robotite puhul, nii nagu me neid üldjuhul mõistame, on nende allumine klassikalise füüsika seadustele. Kõik nad toimivad kolmemõõtmelises ruumis ja pidevas ajas. Erinevusi võib olla nanorobotitega, kuid need jätaks sellest teemast välja. Klassikalise füüsika seadused on ammu kirja pandud ja nende abil on koolipingist teadaolevalt võimalik arvutada kukkuvate kehade kiirusi, põrkuvate kehade impulsse ja kehadevahelisi höördejõude. Võttes appi geomeetria ning lineaaralgebra on arvutuslikult võimalik leida samade kehade liikumisi ka kolmemõõtmelises ruumis. Kasutades arvutite abi ja viies arvutusi läbi jadamisi on võimalik liikumisi arvutada juba kasvõi tuhandete kehade kohta.

Neid teadmisi ära kasutades luuaksegi simulatsioone - virtuaalseid keskkondi - mille lihtsaimaks näiteks on arvutimängud. Viimased töötavad näiliselt küll realistlikult, kuid tegelikult kasutatakse reaalajas töötamise saavutamiseks paljusid arvutit vähemkoormavaid lihtsustusi. Näiteks on peaaegu kõik objektid (kehad) arvutimängudes mitte-deformeeruvad ehk jäigad ja paljud objektid on staatilised - neil pole impulssi ega momenti. Arvutimängu eesmärk on niisiis luua tõetruu, mitte ülearu täpne, efekt ja seetõttu on arvutimängudes tegelikult niiöelda kaks maailma - visuaalne ja füüsiline. Visuaalne maailm koosneb suurest hulgast detailsetest objektidest, mis loovad silmailu, füüsiline maailm on aga palju detailidevaesem, kus visuaalselt kirju objekt on asendatud vaid selle „piirjoonega“ - näiteks risttahuka, keraga või nende kombinatsiooni(de)ga. Ka aeg pole pole arvutimängus pidev vaid diskreetne.

Simuleerimise täpsus sõltub muidugi vajadusest - on olemas molekuli täpsusega väga kaua töötavaid simulatsioone kui oluliselt kiirendatud kosmoselaevade simulatsioone. Robotite puhul tuleb järgnevalt juttu aga reaalajas toimivast ja visuaalsest simulatsioonist - ehk siis sellisest nagu on tüüpilises arvutimängus. Üks põhjus miks arvutimängude juurest otsida simuleerimise vahendeid on nende lai levik ja kättesaadavus. Saada on nii avatud lähtekoodiga, vabavaraliste kinnise lähtekoodiga kui tasulisi füüsikamootoreid. Nimekirja tuntumatest füüsikamootoritest leiab siit. Üks neist füüsikamootoreist osutub teistest juba eos märksa ülekaalukamaks kui teised ja sellest järgnevalt ka lähemalt:

PhysX

PhysX on Ageia loodud füüsikamootor mille eripäraks on selle võime teostada arvutusi spetsiaalse riistvara abil. See riistvara oli üks PCI siini kaart mis arvutisse tuli pista, kuid peale seda kui graafikakaartide tootja NVidia Ageia ära ostis, toimuvad füüsikaarvutused graafikakaardi protsessoris. Kui arvutis uuemat NVidia graafikakaarti pole toimetab PhysX tarkvaraliste arvutustega mis paraku tähendab suuremat koormust arvuti protsessorile.

PhysX-il on SDK (tarkvaraarendus komplekt) nii Windowsi kui Linuxi jaoks. SDK lähtekood on kinnine, kuid selles on olemas kõik päisefailid teekide sidumiseks oma programmi või mänguga. Olemas on õpetused, juhendid ja näited. PhysX SDK jaoks on loodud mitmeid vaheteeke selle sidumiseks teiste mängumootoritega ja teiste programmeerimiskeeltega. Üks tarkvaralahendus mis samuti PhysX kasutab on MSRS.

MSRS

MSRS on lühend nimetusest Microsoft Robotics Studio. Tegu on robotiplatvormide ja robotisülemite programmeerimise ja arendamise tarkvarapaketiga. MSRS tuumaks on nõrgalt seotud teenus-põhine arhitektuur mis võimaldab abstraktselt kirjeldada robotiteid, nende andureid ja täitureid. Teenused omakorda võimaldavad lihtsat visuaalset programmeerimist. Teenuseid saab hajutada erinevate arvutite vahel, ehk moodustada võrkraale. MSRS sisaldab loomulikult ka 3D füüsika simulaatorit, mis nagu öeldud, põhineb PhysX-il.

MSRS on suhteliselt uus tarkvara, mõned aastad vana. Microsoft on seda luues raske ülesande võtnud, sest ühiseid seoseid on loodud paljude erinevate robotiplatvormide vahel. Kuigi MSRS teeb suhtluse erinevate platvormide vahel ühtseks ja sellest tulenevalt ka lihtsamaks, on see siiski suhteliselt omapärane ja harjumatu. Seepärast autor hoiatab kohe ette, et kõik mis järgnevalt juhendis kirjas on ei pruugi 100% õige olla kuna teema on ka autorile üsna uus.

Arhitektuur

MSRS teenuste arhitektuuri moodustavad CCR (Concurrency and Coordination Runtime) ja DSS (Decentralized Software Services). Üldsõnaliselt on DSS tarkvaraline teenuste keskkond ja CCR nende teenuste omavahelise suhtluse koordinaator. Et lihtsamalt asja selgitada, peaks selgitama milleks need tehtud on.

Kui tegu on robotiga, on sel tõenäoliselt mitmekesine riistvara. Erinev riistvara tähendab erinevat kasutusmeetodit ja sellest tulenevalt ka erinevat riistvara poole pöördumise aega. Erinev pöördusaeg tähendab jällegi keerukat programmi kui kasutaja ei soovi iga riistvaralise pöördumise lõppu oodata. Seega tuleb kasutada asünkroonset programmeerimist. Kes sellega kursis on teab, et tegu on oluliselt keerukama programmeerimismeetodiga kui jadamisi täidetavaga. Tavaliselt kasutatakse sealjuures callback-e, puhvreid, lipukesi ja muid nippe, kuid kõik nad teevad programmi segasemaks ja selle täitmisjärjekorra ennustamise peaaegu võimatuks.

Ka CCR põhineb nendelsamadel nippidel, kuid seda ei jäeta programmeerijale nähtavaks. C# keele spetsiaalseid konstruktsioonielemente ja võtmesõnu ära kasutades on programmeerijal võimalik asünkroonset programmeerimist teostada sünkroonsel kirjaviisil. Ülesannetest (tasks) tehakse loendid (enumerator) millest samm-haaval täidetakse käske. Kui käsk on seotud asünkroonsete andmetega, ei peatu programmi töö nende andmete ootamisel vaid jätkub. Ja mis eriti oluline - kõik see toimub ühe lõime sees. Asünkroonsete protsesside lisandumisel ei looda operatsioonisüsteemis uut lõime. Samas, CCR ja DSS siseselt pole välistatud mitme lõime kasutamine.

Asünkroonsed andmete edastamiseks jaoks on spetsiaalsed liidesed (port). Nende liideste abil on seotud ka teenused. Liideste kaudu päritud ja saabunud andmetega tegeleb Arbiter või mitu. Sisuliselt tegeleb Arbiter andmetega seotud teenusesiseste funktsioonide väljakutsumisega. Kuna funktsioonidele saabuvad andmed võivad rikkuda süsteemi järjepidavust on võimalik määrata erinevat liiki lubasid funktsioonide kutsumiseks.

See on üldine info mida võiks teada teenuste puhul, kuid see on tõesti väga üldine, sest kogu arhitektuur on oluliselt mitmekesisem ja põhjalikum - teema on peaaegu sama mahukas kui MSRS simulaator. Võibolla isegi mahukam, sest simulaator on ise vaid üks teenus kogu paketist. Täpsema ja parema informatsiooni saamiseks teenuste kohta võib vaadata näiteks videotutvustusi. Kuna CCR ja DSS moodustavad selgepiirilise ja võimeka tarkvaralahenduse mis sobib ka paljude muude süsteemide programmeerimiseks peale robotite, siis on Microsoft eraldi välja andnud CCR & DSS Toolkit-i.

Simuleerimine

Simulaator on MSRS-i teenus, mille liideste kaudu lisatakse simulatsioonikeskkonda objekte. Nii nagu sissejuhatuses selgitatud ja nii nagu see ka MSRS-is käib, siis ühe simuleeritava objektiga seotakse nii visuaalne kui füüsiline mudel. Mõlemaid mudeleid saab importida failidest või luua programeerimise teel.

Visuaalseid mudeleid oskab MSRS lugeda Wavefront OBJ formaadis ja Microsofti oma .BOS formaadis. Nimetatud .BOS formaat on tegelikult binaarkujul (serialized) visuaalse mudeli andmetüüp millese mudelid .OBJ failidest lugedes efektiivsuse nimel automaatselt teisendatakse, nii et ei maksa üllatuda kui simulatsiooni käivitamisel .BOS faile tekib.

Füüsilise mudeli importimisega on lugu keerulisem. Kui visuaalseid mudeliformaate on kümneid või sadu, siis standardseid füüsilise mudeli failiformaate on autori teada vaid üks - COLLADA. Seda ainukest formaati tunnistab ka MSRS. COLLADA on standardiorganisatsioon kes 2006 aastal loos andmeformaadi füüsiliste mudelite kirjeldamiseks. Tegu on üsna mahuka formaadiga ja sellest lähtuvalt pole eriti ka programme mis laseksid mudeli kõiki füüsilise parameetreid määrata. Üks neist, Solidworks 2009, on profitasemel inseneri projekteerimistarkvara mis on aga tasuline ja väga kallis. NVidia on 3DS Max ja Maya jaoks loonud ka lisamooduli füüsilise mudeli loomiseks ning eksportimiseks, kuid ega needki odavad pole.

Teine modelleerimise meetod on mudeli manuaalne, ehk programmeerimise teel loomine. See tähendab, et simulatsiooniprogrammi (teenusesse) tuleb kirjutada käsud kuubikute, kerade ja muude kujundite loomiseks. Seejuures tuleb kõik mõõdud, koordinaadid, massid, höördetegurid, jms. parameetritena ära määrata. Sedasi võib isegi ühe lihtsa kuubiku loomiseks kümmekond rida programmikoodi kuluda. Visuaalse mudeli loomisel programmeerimise teel saab kasutada Microsofti XNA graafikamootorit, kuid visuaalset mudelit on lihtsam siiski importida. Muide, kui visuaalset mudelit mitte kasutada, siis jääb nähtavaks monokromaatiline füüsiline mudel, mis iseenesest on mugav variant roboti loomisel.

Füüsilise mudeli loomisel, õigemini selle muutmisel, peab teadma, et füüsilist mudelit ei saa muuta simulatsiooni töö ajal ega isegi mitte vahetult pärast loomist. Põhjus peitub PhysX füüsikamootoris mida MSRS kasutab. Seega pole võimalik manipuleerida mudeli staatiliste parameetritega. Mudeli dünaamilisi parameetreid maailma suhtes (asukoht, asend, kiirus jms) aga saab muuta.

MSRS koordinaatteljestik on paremakäeline - see tähendab, et vaataja suhtes X telg suureneb paremale poole, Y telg suureneb ülespoole ja Z telg suureneb lähenedes. Seda on väga oluline teada, sest peale paremakäelise koordinaatteljestiku on kasutusel ka vasakukäeline, kus Z telg suureneb hoopis kaugenedes, mis tundub justkui loogilisem - seepärast võib natuke raskusi tekkida MSRS-iga. Iga telg on MSRS-is ka oma värvi: X on punane, Y roheline ja Z sinine. Teljed visuaalselt kujutatuna:

MSRS koordinaatteljestik

Vasaku- ja paremakäelist koordinaatteljestikku hästi illustreeriv pilt on siin.

Koordinaatteljestiku mõõdud on SI ühikutes - meetrites. Samuti on ka kõik muud ühikud SI süsteemis: mass kilogrammides ja aeg sekundites.

Näide

Ball picker

Asjast saab kõige paremini aimu ikka läbi praktiliste näidete. MSRS-ga tuleb küll kaasa hulk näiteid erinevatest robotitest, kuid nende lähtekood pole avalik. Lähtekoodiga näidete häda on jällegi see, et füüsiliselt on nende mudelid liiga lihtsad ja enamus programmikoodi moodustavad teenused mis teevad asjast arusaamise keerukaks. Kuigi teenustest ei saa üle ega ümber, sest kõik „programmid“ mis MRSR-is töötavad ongi teenused, jätaks nende lähema käsitlemise pärastiseks ja läheks kohe asja tuuma - virtuaalse roboti koostamise juurde.

1. Vajalik tarkvara

Näide on loodud C# keeles Visual Studio 2008 ja Robotics Studio 2008 kasutades Windows Vista operatsioonisüsteemil. Nii Windows XP-l kui Vistal töötavad tasuta variandid mõlemast tarkvaraarendus paketist saab alla laadida siit:

2. Visuaalse mudeli loomine

Olgu tegu juba olemasoleva robotiga või loodava robotiga, alati peavad korrektse projekti puhul roboti kohta joonised olema. Ei ole näidetki mõtet tegema hakata enne kui pole ettekujutust robotist mida luua. Autor otsustas teha väikese roboti mis on mobiilne ja kannab manipulaatorit. Roboti ülesandeks on pallide korjamine.

Ideest lähtuvalt sai SolidWorks 2007 3D joonestusprogrammis koostatud roboti mudel. Kuna antud juhend keskendub MSRS-ile, ei hakka siinkohal joonestamisel peatuma. Küll aga on oluline teada, et SolidWorksi vaatepunktide loogika erineb MSRS-i omast. SolidWorksis on vaatepunkt objekti ees, õigemini objekt on vaataja ees, nii et vaataja ja objekti esikülg on vastamisi. MSRS puhul on vaatepunkti objekti peal (või sees) ja vaatesuund ühtib objekti asetusega. Selle erinevuse tõttu tuleb SolidWorksi mudelit MSRS-iga ühildamiseks ümber Y telje (üles-alla telg) 180 kraadi keerata. Seda võib teha juba modelleerimisel või siis mudeli importimisel MSRS-i. Autorile tundus loogilisem kui Solidworksi eesvaate puhul on näha roboti esikülg ja otsustas keeramise importimisel teha.

Solidworksi teljed

Eespool sai öeldud, et SolidWorks 2009-st on võimalik füüsilist mudelit MSRS-i viia, kuid selleks autor SolidWorks mudelit siiski ei koostanud. Füüsiline mudel on näites koostatud ikkagi kõigi võimalusi silmas pidades programmeerimise teel ja SolidWorksi mudel on vaid visualiseerimiseks. Ühest küljest saab SolidWorksi mudelilt lihtsalt leida mõõte mida füüsilise mudeli koostamisel vaja teada ja teisest küljest saab seda visuaalseks mudeliks teisendada. Kuna SolidWorks otse Wavefront OBJ faile ei salvesta, tuleb kasutada mingit kolmandat programmi. Selleks sobib väga hästi Blender mis oskab importida WRML formaati, mida salvestab SolidWorks, ja eksportida OBJ formaati, mida loeb MSRS.

Nii nagu SolidWorksi puhulgi, jääb Blenderi tundmaõppimine huvilise enda teha. Blender on vabavaraline programm mille kohta on ka palju õpetusi olemas. Mida selle näite puhul Blenderis peale formaatide teisendamise veel teha tuli, oli värvide andmine mudelitele kuna Blender importis WRML 1.0 mudelid vaid ühe värviga.

Keskelt kollaseks värvitud ratas

OBJ formaati eksportimisel peab aga jälgima, et vaikimis valitud Rotate X90 saaks ära keelatud, sest see keerab mudeleid kasutult 90 kraadi X teljel. Teine oluline asi on pinnanormaalide Normals lubamine kuna vastasel juhul visuaalsed mudeleid MSRS-is ei valgustata ja need paistavad mustadena. Eksportimise aken:

Sai öeldud, et SolidWorks lihtsutab füüsilise mudeli koostamist ja seda kahel põhjusel. Esiteks saab juba mudeli loomisel selle mõõte määrata, teiseks võimaldab SolidWorks leida mõõte mida muidu peaks ise arvutama hakkama. Näiteks järgneval pildil on manipulaatori teine lüli koos haaratsitega, millel on mõõdetud erinevused kolmel teljel kahe rohelise punkti vahel.

Põhjus sellise mõõdu võtmiseks selgub näite käigus.

3. Projekti loomine

Projekti loomiseks käivita Visual Studio 2008. Menüüst File vali New ja Project. Avaneb aken kus saab valida projekti tüüpi ja seda seadistada. Projekti tüübiks vali Microsoft Robotics kategooriast DSS Service (2.0). Et näidet lihtsam läbi teha oleks pane projekti nimeks VirtualBallPicker. Projekti kausta võib ise valida.

Uue projekti loomise aken

Edasi tuleb teha projekti tüübi spetsiifilised seadistused. Tegu on teenus-projektiga mille puhul tuleb määrata teenuse nimi, nimeruum ja identifikaator. Ära tuleks keelata subscription manager kasutamine, sest seda pole vaja, ja kel soovi siis ka automaatne kommentaaride lisamine, sest sellega kaasnevad XML stiilis kommentaarid. Neid XML kommentaare on vaja siis kui projektist genereeritakse juhend, muul juhul nad lihtsalt teevad koodi mõttetult kirjuks. Kui Visual Studio hakkab XML kommentaare nõudma, saab need hoiatusteated projekti seadete all „Build“ menüüs „Suppress warnings“ lahtrisse „3003,1591,1592,1573,1571,1570,1572“ kirjutades ära keelata.

DSS teenuse seadistamine

Teenuse puhul võib ka ära määrata partner-teenused. Üks ja ainuke vajalik partner-teenuse roboti simuleerimisel on simulatsioonimootor (Simulation Enginge). Tuleb see nimekirjast üles otsida ja partneriks lisada. Nime võiks simulatsioonimootoril samaks jätta kuid loomise poliisiks peaks „CreateAlways“ valima.

Teenuse partnerite seadmistamine

Peale OK vajutamist tekitatakse projekt koos failidega VirtualBallPicker.cs, VirtualBallPicker.manifest.xml ja VirtualBallPickerTypes.cs. Nüüd tuleb projektiga mõned teegid siduda. Failipuus References kategoorial teist nuppu vajutades, valida Add reference…. Avaneb dialoogiaken kust saab Browse ribalt manuaalselt DLL faile projektiga siduda. Otsi üles MRSR kaust ja mine selle bin kausta kust lisa oma projekti järgmised failid:

  • Microsoft.Xna.Framework.dll
  • PhysicsEngine.dll
  • RoboticsCommon.dll
  • SimulationCommon.dll
  • SimulationEngine.dll

Üks asi mida peavad 64-bitise Windowsi omanikud tegema: Project - VirtualBallPicker Properties… alt tuleb valida Debug sektsioon ja üles otsida rida kus on kirjas programm (external program) mis silumisel käivitatakse. Kui see programm on dsshost.exe tuleb see muuta dsshost32.exe-ks. Asi selles, et mitte kõik MSRS-iga kaasa tulnud teenused pole 64 bitised ja neid tuleb 32 bitises keskkonnas käivitada.

Avades VirtualBallPicker.cs faili leiab sealt teenuse klassi. Selle faili algusesse tuleb lisada projektiga seotud teekide nimeruumi kasutamise (using) käsud. Kui faili olulistesse kohtadesse kommentaarid lisada, alakriipsud muutujate algusest ära võtta, teenusele kirjeldus kirjutada ja simulatsioonimootori liidese muutuja deklareerida, peaks fail välja nägema sedasi:

using System;
using System.Collections.Generic;
using System.ComponentModel;
 
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
 
using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;
 
using W3C.Soap;
using engine = Microsoft.Robotics.Simulation.Engine.Proxy;
 
namespace VirtualBallPicker
{
    [Contract(Contract.Identifier)]
    [DisplayName("VirtualBallPicker")]
    [Description("Remotely controllable virtual robot that can pick balls and put them into cart")]
    class VirtualBallPickerService : DsspServiceBase
    {
        // Service state variable
        [ServiceState]
        VirtualBallPickerState state = new VirtualBallPickerState();
 
        // Service port
        [ServicePort("/VirtualBallPicker", AllowMultipleInstances = true)]
        VirtualBallPickerOperations mainPort = new VirtualBallPickerOperations();
 
        // Service partner - simulation engine
        [Partner("SimulationEngine", Contract = engine.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        engine.SimulationEnginePort simulationEnginePort = new engine.SimulationEnginePort();
        SimulationEnginePort globalSimPort = null;
 
        /*
         * Constructor
         */
        public VirtualBallPickerService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }
 
        /*
         * Service start
         */
        protected override void Start()
        {
            base.Start();
        }
    }
}

Koos teekide nimekirjaga avaneb Visual Studiost selline vaade:

Projekti aken

Et kontrollida kas projekt on korrektne ja töötab, võib proovi-käivituse teha. Selleks tuleb Debug menüüst valida Start Debugging või vajutada F5 ning käivituma peaks dsshost32.exe mis omakorda käivitab simulatsioonikeskkonna koos värskelt loodud teenusega. Kuna praegu simulaatorisse midagi lisatud pole, pole sealt ka peale musta pildi midagi oodata:

Proovi käivitus

Kui projektis on vigu, annab Visual Studio vea koha peal teate, kui aga teenuste süsteemiga on probleeme, ilmub viga dsshost32.exe aknasse. Peab mainima, et viimasest on algajal raske adekvaatset infot vea põhjuse kohta saada, aga üritama peab. Algajal on targem aeg-ajalt teha proovi-käivitusi, sest siis on selgem kust viga võis tulla. Just sellepärast toimub ka näites roboti „ehitamine“ samm-sammult, igat muudatust järgi proovides.

4. Maailma loomine

Enne kui hakkame roboti mudelit koostama, loome sellele virtuaalse keskkonna või maailma (world) kus robot tegutseda saab. MSRS-iga tulevad küll kaasa mõned näidismaailmad, kuid nende kasutamist demonstreerin hiljem. Praeguses näites loome maailma programmeerimise teel.

Otsi VirtualBallpicker.cs failis üles funktsioon Start ja kirjuta sinna järgnevad read:

        /*
         * Service start
         */
        protected override void Start()
        {
            // Refer to simulation engine global instance port
            globalSimPort = SimulationEngine.GlobalInstancePort;
 
            // Setup camera viewpoint
            SetupCamera();
 
            // Add objects (entities) in our simulated world
            PopulateWorld();
 
            base.Start();
        }

Start funktsioon asendab override direktiiviga baasklassi samanimelist funktsiooni. Samas ei tohiks baasklassi Start funktsiooni kutsumata jätta, nii et seda tuleb base.Start(); käsuga ka teha. Üldiselt on soovitav baasklassi funktsioon välja kutsuda viimasena. Selle konkreetse funktsiooni puhul võib vastupidi tehes juhtuda, et teenus käivitatakse ja osapooled hakkavad juba suhtlema, kuid simulaator pole veel seadistatud. Funktsiooni alguses olev omistuslause teeb simulatsioonimootori poole pöördumise käsud edaspidi lühemaks, omistades globalSimPort muutujale simulatsioonimootori globaalse instantsi pordi, õigemine viida (reference). Kuigi simulatsioonimootor on ka teenus on selline globaalne port erand. Kindlalt ei oska põhjust sellele öelda, kuid ilmselt on üks globaalne port sellepärast, et korraga ei saa rohkem kui üks simulataator töötada ja eraldi instantsi loomine pole efektiivne. Järgnevalt kahest funktsioonist mida Start funktsioonis välja kutsutakse:

        /*
         * Setup camera
         */
        private void SetupCamera()
        {
            // Set up initial view
            CameraView view = new CameraView();
            view.EyePosition = new Vector3(0.0f, 1.0f, 2.0f);
            view.LookAtPoint = new Vector3(0.0f, 0.3f, 0.0f);
            simulationEngine.Update(view);
        }

Siinkohal toimub kaamera, ehk vaatepunkti seadistamine. Kolmemõõtmelise vektoriga määratakase ära nii vaatepunkt kui fokuseerimispunkt. Vaatepunkt asub Z teljel 2m kaugusel ja Y teljel 1m kõrgusel maailma keskpunktist. Vaade on suunatud maailma keskpunktist 0.3m kõrgusele.

        /*
         * Populate world
         */
        private void PopulateWorld()
        {
            AddSky();
            AddGround();
        }

PopulateWorld on maailma sisustav funktsioon. Praeguses näites toimub taeva ja maa lisamine.

        /*
         * Add sky
         */
        void AddSky()
        {
            // Sky
            SkyDomeEntity sky = new SkyDomeEntity
            (
                "skydome.dds",  // sky texture
                "sky_diff.dds"  // lightning texture
            );
 
            globalSimPort.Insert(sky);
 
            // Sun
            LightSourceEntity sun = new LightSourceEntity();
 
            sun.State.Name = "Sun";
            sun.Type       = LightSourceEntityType.Directional;
            sun.Color      = new Vector4(0.8f, 0.8f, 0.8f, 1.0f); // rgba
            sun.Direction  = new Vector3(0.5f, -0.75f, 0.5f);
 
            globalSimPort.Insert(sun);
        }

Taevas luuakse poolsfäärist millele määratakse tekstuur. Lisaks määratakse maailma valgustuse tekstuur. Mõlemad tekstuurid pärinevad MSRS alamkataloogist „store/media“. Üldiselt loetakse kõiki meediafaile sellest kataloogist. Muudele kataloogidele ligi saamiseks tuleb määrata muu kataloogi relatiivne asukoht. Absoluutse asukohaga faile autoril laadida ei õnnestunud. Kui taeva tekstuur on lihtsasti arusaadav 360 kraadine pilt, siis valgustuse tekstuur on midagi keerukamat. Tegu on DDS (Microsoft DirectDraw Surface) failiga mis sisaldab kuubi 6 tahu tekstuuri ja selle abil valgustatakse kõigi maailma objektide pindasid ja tekitatakse neile taeva peegeldusi. Loe lähemalt.

Peale taeva tekitatakse ka valgusallikas - virtuaalne päike mille valgus langeb paralleelsete kiirtena ~45 kraadi nurga all. Määratud on valguse värv. Valgusallikal on ka CastsShadows parameeter mille tõeseks määramisel peaks maailma varjud tekkima, kuid autoril ei õnnestunud varjusid mitte kuidagi tekitada. Kahjuks pole varjude kohta ei MSRS õpetuses ega veebis eriti räägitud.

        /*
         * Add ground
         */
        void AddGround()
        {
            // Ground material
            MaterialProperties carpetProperties = new MaterialProperties
            (
                "carpet",  // name
                0.1f,      // restitution
                0.5f,      // dynamic friction
                0.8f       // static friction
            );
 
            // Create a large horizontal plane, at zero elevation.
            HeightFieldEntity ground = new HeightFieldEntity
            (
                "Ground",         // name
                "03RamieSc.dds",  // ground texture
                carpetProperties  // material
            );
 
            globalSimPort.Insert(ground);
        }

Maapinnaks on kõrguskaardi objekt millele kõrguse informatsiooni pole omistatud - seega on see tasane. Maapinna nimeks on määratud „Ground“ ja selle tekstuurifail pärineb sealtsamast kust taeva omagi. Oluline on aga maapinnale määrata õige materjal, sest sellega on tavaliselt enamus simulatsiooniobjekte kontaktis. Materjali määramiseks on materjali omadusi kirjeldav MaterialProperties klass mida saab määrata nii maapinnale kui ka kõigile teistele füüsilistele objektidele.

Maapinna tekstuur on vaiba oma ja sellest lähtuvalt on sellele ka vaiba omadused määratud. Restitution on parameeter mis puudutab simulaatsioonis tasakaaluoleku saavutamist. Kahjuks ei oska täpselt öelda mismoodi see toimub või mis ühikutega tegu on, kuid selle väärtus peab PhysX dokumenatsiooni järgi olema 0 ja 1 vahel, kuigi 1 lähedasi väärtusi soovitatakse vältida. 0 peaks olema ideaalselt täpseks simuleerimiseks, kuid see võib tähendada pidevat kehadevahelist liikumist tasakaaluoleku saavutamise nimel ja seega vaiba puhul üleliigset tööd simulaatorile, nii et näites on parameetriks kompromissina 0.1 pandud.

Dynamic friction on dünaamiline hõõrdetegur, ehk hõõrdetegur pinnal libisedes. Static friction on hõõrdetegur pinnal seistes. Hõõrtegur 0 tähendab, et hõõrdumist siusliselt pole. Hõõrdetegur 1 tähendab, et hõõrdejõud on võrdne raskusjõuga. Kui staatiline hõõrdetegur on näiteks 10, siis tähendab see, et objekti liikuma panemiseks peab rakendama raskusjõust 10 korda suuremat jõudu.

Kuigi materiali kirjelduse konstruktoriga antakse kaasa 3 materjali omadust, on selles klassis ka Advanced parameeter millega saab määrata hõõrdeteguri ja libisemissuuna vahelise seose, vetruvuse ja optilised parameetrid. Nii et võimalik on näiteks simuleerida suuska mis edasi libiseb paremini kui tagasi.

Peale taeva ja maa loomist võib vaadata kuidas need välja näevad:

Maailma testimine

5. Roboti olemiklassi loomine

Järgmisena tekitame projekti virtuaalse roboti olemi (entity) klassi. Olemist võib mõelda ka kui mudeli kirjeldusest. Olemist instantsi luues ja seda simulatsiooni lisades saab sellest konkreetne objekt. Seega võib simulatsioonis olla mitu ühte liiki, ehk samast olemist pärinenud objekti. Olemi klassi loomiseks tuleb failipuus projekti VirtualBallPicker nimel teise hiirenupuga klikkides menüüst valida Add - New Item…. Uueks elemendiks projektis saab Visual C# klass nimega BallPickerEntity.cs.

Uus olemi klass

Ka selle klassi faili tuleb viidad lisada. Uus BallPickerEntity klass peab laiendama VisualEntity klassi, mis on baasklass kõigile nähtavatele simulatsiooni objektidele. Fail peaks välja nägema sedasi:

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Text;
 
using Microsoft.Dss.Core.Attributes;
using Microsoft.Robotics.PhysicalModel;
using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using Microsoft.Robotics.Simulation.Physics;
 
using xna = Microsoft.Xna.Framework;
using xnagrfx = Microsoft.Xna.Framework.Graphics;
 
namespace VirtualBallPicker
{
    public class BallPickerEntity : VisualEntity
    {
    }
}

6. Füüsilise mudeli kirjeldamine

Olemiklassis hakkame kirjeldama roboti mudelit - nii füüsilist kui visuaalset. Algatuseks lisame klassile 2 konsktruktorit:

		/*
		 * Constructor
		 */
		public BallPickerEntity()
		{
			State.Name = "Ball picker " + Guid.NewGuid().ToString();
		}
 
		/*
		 * Constructor
		 */
		public BallPickerEntity(string name, Pose pose)
		{            
			State.Name = name;
			State.Pose = pose;			
		}

Esimene konstruktor on ilma parameetriteta ja selle lisamine on kohustuslik. State muutuja tuleneb VisualEntity klassist ja selle abil toimub ka mudeli kirjeldamine. Parameetriteta konstruktoris genereeritakse olemi objektile suvaline nimi „Ball picker “ algusega. Suvalist nime on vaja kui simulatsiooni lisatakse rohkem kui üks samaliiki robot. Nime võiks teoreetiliselt ja muude MSRS näidete põhjal ka hiljem omistada, kuid õpetuse autoril ilma nimeta objekti lisamine simulatsioonikeskkonda ei õnnestunud. See võib mingi arusaamatus või MSRS-i viga olla, kuid sellegipoolest on lihtsam ja loogilisem kui nimi saaks kohe algul pandud. Teises konstruktoris saab kasutaja nime ise määrata ning lisaks sellele ka roboti asukohta ning asendit. Viimaseid seob Pose andmetüüp mis kätkeb endas nii 3-mõõtmelist vektorit (Vector3) kui kvaterniooni (Quaternion).

Järgnevalt asendame (sisuliselt laiendame) override direktiiviga VisualEntity seadistusfunktsiooni Initialize:

		/*
		 * Initialization
		 */
		public override void Initialize(xnagrfx.GraphicsDevice device,
		                                PhysicsEngine physicsEngine)
		{         		
			CreateMaterials(); 		
			CreateChassis();
 
			base.CreateAndInsertPhysicsEntity(physicsEngine);
			base.PhysicsEntity.SolverIterationCount = 128;
			base.Initialize(device, physicsEngine);	
		}
 

Initialize on funktsioon mida kutsub välja simulatsioonimootor kui olemi objektid on simulaatorisse lisatud ja see käivitub. Seega on siin õige koht mudel ära kirjeldada. Kuna näidisrobot tuleb üsna keeruline, ei hakka me kogu mudeli kirjeldust ühte funktsiooni kokku panema ja teeme seda alamfunktsioonidega. CreateChassis on üks neist alamfunktsioonidest mis kirjeldab roboti veermikku, CreateMaterials on aga materjalide loomiseks. Neile järgnevad 3 rida tähendavad aga järgmist:

Kui olemil on füüsiline mudel, tuleb see eraldi luua. Initialize funktsioonil on parameetriteks graafikamootori ja füüsikamootori instants. CreateAndInsertPhysicsEntity abil luuakse füüsikamootorisse olemis kirjeldatud füüsiline mudel. Täisarvuline parameeter PhysicsEntity.SolverIterationCount näitab mitu korda füüsikamootor teostab kontaktis olevate ja liigenditega seotud objekti vahel kokkupõrke tehteid ühel simulatsiooni (aja)sammul. See number peaks olema suurem kui soovitakse täpset simuleerimist ka paljude kontaktide (kokkupuudete) ja liigendite puhul. Üle 255 ei tohiks seda arvu määrata. Viimane rida aga kutsub välja olemi baasklassi Initialize funktsiooni - seega toimub justkui baasklassi funktsiooni laiendamine.

Materjalide funktsioonis loome materjali kirjelduse mida saab üle terve klassi kasutada. Selleks on plastiku materjal ja selle kohta tuleb klassi päisesse lisada järgnev muutuja:

        // Materials
        private MaterialProperties plasticMaterial;

Seejärel tuleb luua CreateMaterials funktsioon kus plasticMaterial muutujale omistatakse uue plastiku omadused. See toimub samamoodi nagu maapinna materjali loomine ja nagu eelpool öeldud saab ka sellele materjalile määrata rohkem parameetreid kui konstruktoris kaasa antakse.

        /*
         * Create materials
         */
        private void CreateMaterials()
        {
            // Material for robot body
            plasticMaterial = new MaterialProperties
            (
                "plastic", // name
                0.01f,     // restitution
                0.7f,      // dynamic friction
                2.0f       // static friction
            );
        }

Jõudsime nüüd füüsilise mudeli loomiseni. Selleks tuleb luua CreateChassis funktsioon:

		/*
		 * Create the robot chassis
		 */
		private void CreateChassis()
		{			        
			// Bottom half of the chassis
			BoxShapeProperties chassisBottom = new BoxShapeProperties
			(
				5.0f,                                          // mass
				new Pose(new Vector3(0.00f, -0.005f, 0.00f)),  // pose
				new Vector3(0.30f, 0.05f, 0.40f)               // dimension 
			);            
			chassisBottom.Material = plasticMaterial;            
			State.PhysicsPrimitives.Add(new BoxShape(chassisBottom));
 
			// Chassis below link base
			BoxShapeProperties chassisArm = new BoxShapeProperties
			(
				2.0f,                                          // mass
				new Pose(new Vector3(0.00f, 0.045f, -0.10f)),  // pose
				new Vector3(0.30f, 0.05f, 0.20f)               // dimension 
			);
			chassisArm.Material = plasticMaterial;            
			State.PhysicsPrimitives.Add(new BoxShape(chassisArm));
 
			// Chassis box left side
			BoxShapeProperties chassisLeftSide = new BoxShapeProperties
			(
				0.1f,                                          // mass
				new Pose(new Vector3(-0.14f, 0.045f, 0.10f)),  // pose
				new Vector3(0.02f, 0.05f, 0.20f)               // dimension 
			);
			chassisLeftSide.Material = plasticMaterial;            
			State.PhysicsPrimitives.Add(new BoxShape(chassisLeftSide));     
 
			// Chassis box right side
			BoxShapeProperties chassisRightSide = new BoxShapeProperties
			(
				0.1f,                                          // mass
				new Pose(new Vector3(0.14f, 0.045f, 0.10f)),   // pose
				new Vector3(0.02f, 0.05f, 0.20f)               // dimension 
			);
			chassisRightSide.Material = plasticMaterial;            
			State.PhysicsPrimitives.Add(new BoxShape(chassisRightSide));
 
			// Chassis box back side
			BoxShapeProperties chassisBackSide = new BoxShapeProperties
			(
				0.1f,                                          // mass
				new Pose(new Vector3(0.00f, 0.045f, 0.19f)),   // pose
				new Vector3(0.26f, 0.05f, 0.02f)               // dimension 
			);
			chassisBackSide.Material = plasticMaterial;            
			State.PhysicsPrimitives.Add(new BoxShape(chassisBackSide));
		}

Kere luuakse mitmest kujundist - risttahukast (kastist). Sarnaselt materjali andmestruktuuriga, toimub ka siin esmalt kirjelduse (properties) muutuja loomine. Kasti kirjelduse konstruktoris määratakse ära kasti mass kilogrammides, selle asukoht ning ühtlasi ka asend ja selle mõõdud. Kasti asukoht kehtib kasti keskpunkti suhtes, ehk mõõdud on siis nagu topelt raadiused. Nii nagu materjali puhul, saab ka kasti puhul seda mida konstruktoris määrata ei saa, teha hiljem. Kasti ja ka teiste füüsiliste objektide kirjeldustes on võimalik määrata massikese, lineaarne- ning pöördsumbuvus (damping) ning mõningad muud parameetreid. Peale kasti kirjelduse loomist luuakse selle põhjal kasti objekt ja lisatakse see füüsilisse mudelisse.

Kere moodustavad kokku 5 kasti. Kõige esimene neist moodustab põhja, teised moodustavad roboti küljed. Keskele jääb auk kuhu palle koguda. Kastide mõõdud tulevad SolidWorksi mudelist, massid on ise välja mõeldud.

Keerulist mudelit pole mõtet ühe korraga ära kirjeldada, vahepeal võiks vaadata kuidas see ka välja paistab. Et olem simulatsiooni saada tuleb VirtualBallPicker.cs faili muuta. Klassi VirtualBallPicker päisesse tuleb lisada BallPickerEntity tüüpi muutuja:

        // Ball picker entity
        private BallPickerEntity entity = null;

Ja PopulateWorld funktsiooni tuleb lisada järgnevad read mis loovad roboti olemist instantsi ja lisavad selle simulaatorisse:

        // Add our robot to simulation
        entity = new BallPickerEntity("My robot", new Pose(new Vector3(0.0f, 0.06f, 0.0f)));
        globalSimPort.Insert(entity);

Kui nüüd kõik korras on ja projekt käivitada, avaneb simulatsioonikeskkond, kus maailma keskpunktis on lebamas näha roboti kere. Et täpsemalt näha mismoodi mudel üles on ehitatud tuleb simulatsiooniakna menüüst Render valida kas Physics või Combined. Kiirklahv erinevate visualiseerimismeetodite valimiseks on F2. Kombineeritud pilt näeb välja sedasi:

Kere mudel

7. Visuaalse mudeli importimine

Et robot reaalsem välja näeks tuleb sellele ka visuaalne mudel määrata. Kui visuaalsed mudelid on eelnevalt loodud tuleb ära määrata nende asukoht. Seda on mõistlik konstantidega teha. Faili BallPickerEntity.cs klassi BallPickerEntity päisesse on vaja lisada järgnevad read:

        // Mesh locations
        private const String ModelsPath  = "../../../Documents/Robotiklubi/MSRS Simulation/Models/obj";
        private const String ChassisMesh  = ModelsPath + "/chassis.obj";
        private const String WheelMesh    = ModelsPath + "/wheel.obj";

Esmalt määratakse ära visuaalsete mudelite kausta relatiivne asukoht MSRS „store/media“ kausta suhtes, seejärel määratakse ära iga konkreetse roboti detaili mudelifaili nimi. Seejärel on täpsemalt määratud kere ja ratta visuaalse mudeli failinimed. Nüüd tuleb eelnevalt loodud CreateChassis funktsiooni tagasi pöörduda ja sinna lõppu järgnevad read lisada:

            // Attach mesh model
            State.Assets.Mesh = ChassisMesh;
            MeshRotation = new Vector3(0.0f, 180.0f, 0.0f);

Need read omistavad kere olemile visuaalse mudelifaili, mida simulaator käivitamisel loeb. Kuna mudelifailid on pärit SolidWorksist kus mudel on teistpidi, tuleb ühildamiseks visuaalset mudelit Y teljel 180 kraadi pöörata - selleks on Vector3 tüüpi MeshRotation parameeter. Nagu järeldada võib, siis vektori kolm mõõdet näitavad nurka vastaval teljel. Üldiselt on nurga ühikud küll radiaanides, kuid selle parameetri puhul on need erandlikult kraadides. Muide, kui visuaalne ja füüsiline mudel on nihkes, saab visuaalset mudelit MeshTranslation parameetri abil nihutada ja kui mudelid on erinevas mõõtkavas, saab MeshScale parameetriga visuaalset mudelit skaleerida. Teenuse käivitamisel peaks simulatsioonis nüüd näha olema visuaalne mudel:

Kere visuaalse mudeliga

Kui simulatsiooniaken käivitub ebaharilikult kaua ja lõpuks ilmub teade, et mingit .bos faili ei leitud on põhjus ilmselt vales visuaalse mudeli failinimes. Sel juhul tuleks üle kontrollida kas asukoht on õige antud ja kas fail on olemas. Jutt käib muidugi .OBJ failist. Nagu eespool selgitatud on .BOS fail .OBJ failist genereeritud ja seda otsib MSRS eelisjärjekorras ning .OBJ faili puudumisel annab veateate ka ainult .BOS faili kohta.

8. Veermiku loomine

Järgmisena lisame robotile rattad. Selleks on mitu võimalust. MSRS-is on olemas WheelEntity olem mis on sisuliselt PhysX-i WheelShape. Tegu on spetsiaaselt auto rataste võimalikult reaalseks simuleerimiseks mõeldud olemiga, mis arvestab mootori pöördemomenti, ratta pöördenurka, amortisaatorite parameetreid, pidurdusjõudu ja kõike muud sellist. Paraku on sel ka üks suur puudus - ratas pole füüsiliselt mitte ketta, silindri ega isegi kera kujuga vaid lihtsalt üks vektor, mis on suunatud maa poole. Seega saab rattal vaid maapinnaga kontakt tekkida. Kui ratas asub rattakoopas pole sel tähtsust, aga kui tegu on robotiga millel rattad asuvad kerest väljas võib ratas ka kallakute, seintega või teiste objektidega kokku puutuda ja sel juhul pole ilmselt aksepteeritav ratta nendest takistustest läbi sõitmine.

Järneval pildil on kujutatud WheelEntity füüsilist mudelit. Simulaatoris kuvatakse ratas küll ringina, kuid kontakt tekib ainult siniselt märgitud vektoriga. Punasega kujutatud objektidega sel rattal kontakti ei teki:

WheelEntity

Niisiis, kui tegu on robotiga millel vaja kokkupõrkeid täpselt simuleerida ja millel pole vedrustust ning suurt kiirust saab ratta tekitada füüsilisest (või mitmest) kujundist. Üks lihtsamaid ratta vorme on kera, kuid kera häda on jällegi selle laius - takistustest lähedalt mööda sõites põrkub robot nendega, kuigi visuaalselt rattal justkui kontakti poleks. Selles näites aga tuleb rattana kasutusele üks kujundite kombinatsioon.

CustomWheel

Et ära kasutada kera kumerust sujuvalt veereva ratta simuleerimiseks ja samas hoida ratas kitsana on näites ratas loodud paljudest ringina asetsevatest keradest. Ratta võiks luua ka kastidest, kuid kera eeliseks on selle kumerus ka külgsuunas - täpselt nagu õhukummiga ratasdelgi. Muidugi, kitsa ja suure diameetriga ratta puhul jääb ratta keskosa füüsiliselt kirjeldamata, kuid samas ei satu see osa ilmselt nii tihti millegagi kontakti kui välispind. Keradest moodustatud ratas näeb külgvaates välja sedasi:

CustomWheel

Mida rohkem ja tihedamalt kerasid rattasse lisada seda siledam selle välispind on, kuid samas nõuab see arvutilt (või graafikakaardilt) rohkem arvutusjõudlust. Kui kerasid on vähe siis robot rappub sõites - kuigi seda on võimalik liigendite parameetreid muutes vähendada. Järgnevalt on toodud CustomWheel klass mida on näites rataste mudeli loomisel kasutatud. Peale eelnevalt selgitatud füüsiliste kujundite loomise seotakse siin need kujundid liigendite abil ka kerega. Kuna liigenditest tuleb juttu ka manipulaatori juures, jätaks siinkohal selle klassi pikemalt lahti seletamata ja selgitaks kuidas seda kasutada. Esmalt tuleb aga oma projekti tekitada uus fail - CustomWheel.cs ja kopeerida sinna järgnev kood:

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Text;
 
using Microsoft.Dss.Core.Attributes;
using Microsoft.Robotics.PhysicalModel;
using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using Microsoft.Robotics.Simulation.Physics;
 
using xna = Microsoft.Xna.Framework;
using xnagrfx = Microsoft.Xna.Framework.Graphics;
 
namespace VirtualBallPicker
{
    /*
     * Custom wheel properties
     */
    public class CustomWheelProperties
    {
        public VisualEntity Parent;
        public float Radius   = 1.0f;
        public float Width    = 0.1f;
        public float Mass     = 1.0f;
        public int NumSpheres = 8;
        public float MaxForce = 1000.0f;
        public SpringProperties AxialSpring;
        public MaterialProperties Material;
        public string Mesh;
    }
 
    /*
     * Custom wheel
     */
    public class CustomWheel
    {
        private MultiShapeEntity entity;
        private PhysicsJoint joint;
        private CustomWheelProperties properties;
        private Vector3 direction;
        private float speed = 0.0f;
 
        /*
         * Constructor
         */
        public CustomWheel(string name, CustomWheelProperties properties, Vector3 position, Vector3 direction)
        {
            entity = new MultiShapeEntity();
            entity.State.Name = name;
            entity.State.Pose = new Pose(position);
 
            this.properties = properties;
            this.direction  = direction;
        }
 
        /*
         * Initialization
         */
        public void Initialize(xnagrfx.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            // Each sphere radius
            float sphere_radius = properties.Radius - properties.Width / 2.0f;
 
            // Create wheel physical model from several spheres
            for (int i = 0; i < properties.NumSpheres; i++)
            {
                // Place spheres radially
                Pose spherePose = new Pose();
                double rad = (2.0 * Math.PI) / properties.NumSpheres * i;
 
                spherePose.Position += new Vector3(
                    0.0f,
                    sphere_radius * (float)Math.Cos(rad),
                    sphere_radius * (float)Math.Sin(rad)
                );
 
                SphereShapeProperties sphereProperties = new SphereShapeProperties
                (
                    properties.Mass / properties.NumSpheres, // mass
                    spherePose,                              // pose
                    properties.Width / 2.0f                  // radius
                );
 
                sphereProperties.Material = properties.Material;
 
                // Add sphere to entity
                entity.SphereShapes.Add(new SphereShape(sphereProperties));
            }
 
            // Wheel mesh
            entity.State.Assets.Mesh = properties.Mesh;
            entity.MeshRotation = new Vector3(0.0f, 90.0f - 90.0f * direction.X, 0.0f);
 
            // Little bit of damping
            entity.State.MassDensity.AngularDamping = 2.0f;
            entity.State.MassDensity.LinearDamping  = 2.0f;
 
            // Initalize entity
            entity.Initialize(device, physicsEngine);
 
            // Joint angular properties
            JointAngularProperties wheelAngular = new JointAngularProperties();
 
            wheelAngular.TwistMode  = JointDOFMode.Free;     // local axis
            wheelAngular.Swing1Mode = JointDOFMode.Locked;   // normal
            wheelAngular.Swing2Mode = JointDOFMode.Locked;   // binormal
 
            wheelAngular.TwistDrive = new JointDriveProperties
            (
                JointDriveMode.Velocity,  // mode
                properties.AxialSpring,   // spring properties
                properties.MaxForce       // force limit
            );
 
            // Joint properties
            JointProperties jointProperties = new JointProperties(wheelAngular, null, null);
            jointProperties.EnableCollisions = false;
 
            // Create joint
            joint = PhysicsJoint.Create(jointProperties);
            joint.State.Name = entity.State.Name + " joint";
 
            // Joint connectors
            joint.State.Connectors[0] = new EntityJointConnector
            (
                properties.Parent,            // entity
                Vector3.YAxis,                // normal
                Vector3.XAxis,                // local axis
                entity.State.Pose.Position    // connector point
            );
 
            joint.State.Connectors[1] = new EntityJointConnector
            (
                entity,                       // entity
                 Vector3.YAxis,                // normal
                Vector3.XAxis,                // local axis
                new Vector3()                 // connector point
            );
 
            // Manual projection settings - to avoid bizarre movements
            joint.State.Projection = new JointProjectionProperties();
            joint.State.Projection.ProjectionMode = JointProjectionMode.PointMinimumDistance;
            joint.State.Projection.ProjectionDistanceThreshold = 0.01f;
            joint.State.Projection.ProjectionAngleThreshold = 0.01f;
 
            // Insert joint to physics engine
            physicsEngine.InsertJoint(joint);
        }
 
        /*
         * Pose update
         */
        public void Update(FrameUpdate update)
        {
            // Apply velocity in local axis direction
            joint.SetAngularDriveVelocity(new Vector3(speed, 0.0f, 0.0f));
 
            entity.Update(update);
        }
 
        /*
         * Entity render
         */
        public void Render(VisualEntity.RenderMode renderMode, MatrixTransforms transforms, CameraEntity currentCamera)
        {
            entity.Render(renderMode, transforms, currentCamera);
        }
 
        /*
         * Speed
         */
        public float Speed
        {
            set
            {
                speed = value;
            }
            get
            {
                return speed;
            }
        }
    }
}

CustomWheel klassi kasutamiseks peab BallPickerEntity.cs faili BallPickerEntity klassi päisesse lisama nelja ratast tähistavad muutujad:

        // Child entities
        private CustomWheel wheelFL, wheelFR, wheelRL, wheelRR;

BallPickerEntity klassi Initialize funktsioonis tuleb peale CreateChassis funktsiooni nüüd välja kutsuda ka CreateWheels funktsioon, mille kood on järgnev:

        /*
         * Create wheels
         */
        private void CreateWheels()
        {
            // Wheel material
            MaterialProperties wheelMaterial = new MaterialProperties
            (
                "rubber",  // name
                0.01f,     // restitution
                0.9f,      // dynamic friction
                8.0f       // static friction
            );
 
            // Wheel axial spring properties
            SpringProperties axialProperties = new SpringProperties
            (
                1000.0f,   // spring coefficent
                50.0f,     // damper coefficent
                0.01f      // equilibrium position
            );
 
            // Wheel properties
            CustomWheelProperties wheelProperties = new CustomWheelProperties();
 
            wheelProperties.Parent      = this;
            wheelProperties.Mass        = 0.1f;
            wheelProperties.Radius      = 0.06f;
            wheelProperties.Width       = 0.02f;
            wheelProperties.NumSpheres  = 32;
            wheelProperties.MaxForce    = 100.0f;
            wheelProperties.Material    = wheelMaterial;
            wheelProperties.AxialSpring = axialProperties;
            wheelProperties.Mesh        = WheelMesh;
 
            // Create wheel objects
            wheelFL = new CustomWheel("wheel front left",  wheelProperties, new Vector3(-0.17f, 0.0f, -0.13f), Vector3.NegativeXAxis);
            wheelFR = new CustomWheel("wheel front right", wheelProperties, new Vector3( 0.17f, 0.0f, -0.13f), Vector3.XAxis);
            wheelRL = new CustomWheel("wheel rear left",   wheelProperties, new Vector3(-0.17f, 0.0f,  0.13f), Vector3.NegativeXAxis);
            wheelRR = new CustomWheel("wheel rear right",  wheelProperties, new Vector3( 0.17f, 0.0f,  0.13f), Vector3.XAxis);
        }

Kõigist neli ratast luuakse CustomWheel klassist mille parameetriteks on ratta nimi, selle omadused, asukoht ning asend. Ratta omadused on kõik kirjeldatud CustomWheelProperties klassiga mille parameetrid on järgnevad:

Parent Olemi objekt millega ratas läbi liigendi seotud on.
Mass Ratta mass.
Radius Ratta raadius.
Width Ratta laius.
NumSpheres Kerade arv millest ratas koosneb. Need jaotatakse ühtalselt täisringi peale.
Material Ratta materjal.
Mesh Visuaalse mudeli failinimi.
MaxForce Maksimaalne jõud mida liigend kannatab. Suurema jõu korral ratas murdub.
AxialSpring Ratta pöörlemistelje vetruvuse parameetrid. Näiteks ülekande lõtkude simuleerimiseks. Üldiselt peaks pöördtelje vetruvus jäik olema.

Kui on soovi ratastele püstsuunas vetruvus tekitada, võib ratta liigendi püstteljel liikuvaks teha ja sellele soovitud vetruvuse määrata. Sellisel juhul hakkavad aga rattad suure koormusega piltikult öeldes altpoolt laiali vajuma. Autor pole seda küll järgi proovinud, kuid liigenditega on juhendite andmetel võimalik ka lineaarset liikumist koos vetruvusega tekitada.

Kuna rattad on eraldi olemid mida otse simulaatorisse ei lisata, peab nende füüsiliseks simuleerimiseks ja visuaalseks kuvamiseks vastavaid funktsioone ise välja kutsuma. Selleks tuleb roboti (BallPickerEntity) olemi vastavaid funktsioone laiendada. Esimene funktsioon mida laiendama peab on Initialize. Selle funktsiooni lõppu tuleb lisada järgnevad read:

            // Initialize wheels
            wheelFL.Initialize(device, physicsEngine);
            wheelFR.Initialize(device, physicsEngine);
            wheelRL.Initialize(device, physicsEngine);
            wheelRR.Initialize(device, physicsEngine);

Sarnaselt Initalize funktsiooniga tuleb laiendada ka olemi oleku uuendamise funktsiooni Update:

        /*
         * Update
         */
        public override void Update(FrameUpdate update)
        {
            // Update wheels
            wheelFL.Update(update);
            wheelFR.Update(update);
            wheelRL.Update(update);
            wheelRR.Update(update);
 
            base.Update(update);
        }

Ning seejärel ka Render funktsiooni mis teostab kuvamist:

        /*
         * Render
         */
        public override void Render(RenderMode renderMode, MatrixTransforms transforms, CameraEntity currentCamera)
        {
            // Render wheels
            wheelFL.Render(renderMode, transforms, currentCamera);
            wheelFR.Render(renderMode, transforms, currentCamera);
            wheelRL.Render(renderMode, transforms, currentCamera);
            wheelRR.Render(renderMode, transforms, currentCamera);
 
            base.Render(renderMode, transforms, currentCamera);
        }

Nüüd on rattad lisatud ja võib järgi vaadata kuidas kere ratastel välja näeb. Kui kõik on korrektne, peaks ratastel robot lihtsalt keset maapinda seisma. Vale rataste positsiooni korral võib aga juhtuda, et robot hakkab pööraselt ringi lendama. Asi on selles, et üksteisega kattuvate objektide korral hakkab simulaator nende vahel põrkumist simuleerima ning kui objekte siduv liigend on ei ole purunev, tekivadki suured jõud mis objekti lennutavad. Kuna liigendi loomine on jäetud CustomWheel klassi hooleks pole eksimisvõimalus kuigi suur - pigem võib see tekkida manipulaatori loomisel. Pilt ratastel robotikerest:

Robot ratastel

:!: Rattad mis robotile lisatud sai, olid määratud relatiivse asukohaga kere suhtes. Kui robot või ükskõik mis objekt koosneb mitmest liigendiga seotud mudelist, peab arvestama inertsijõududega mis tekivad objekti paigutamisega null-punktist väljapoole. Asi on selles, et iga füüsiline mudel omab absoluutset asukohta maailma suhtes - kokku seob erinevaid füüsilise mudeleid vaid liigend. Kui aga peamine füüsiline objekt lisatakse simulaatoris kuhugi mujale kui null-punkti siis simulaatori käivitamisel tekib liigendiga seotud füüsiliste objektide vahel tõmbejõud mis põhjustab objekti liikumist või lausa lendamist, olenevalt massidest ja distantsist.

Sellest kuidas robot lisada mitte-null-punkti loe lähemalt infomaterjalides toodud liigendite õpetuse peatükist 2.6.

9. Juhtimisteenuse loomine

Virtuaalse roboti liikuma saamiseks on kaks variant: tuleb kasutada juhtpulti või autonoomset juhtimisalgoritmi. Esmalt on lihtsam teha robot käsitsi juhitavaks ja sellest variandist järgnev peatükk ka kirjutab.

Roboti juhtfunktsioonid

Robotile olemile tuleb tekitada funktsioon mis võimaldab sõidukiirust määrata. Klassi BallPickerEntity päisesse võiks roboti sõidukiiruse seadistamiseks määrata koefitsendi WheelSpeedCoefficent:

        // Constants
        private const float WheelSpeedCoefficent= 5.0f;

Klassi lõppu tuleb lisada järgmised kaks funktsiooni:

        /*
         * Value limiting
         */
        private float Limit(float value, float min, float max)
        {
            return (value < min ? min : (value > max ? max : value));
        }
 
        /*
         * Drive command
         */
        public void Drive(float powerLeft, float powerRight)
        {
            wheelFL.Speed = WheelSpeedCoefficent * Limit(powerLeft,  -1.0f, 1.0f);
            wheelRL.Speed = WheelSpeedCoefficent * Limit(powerLeft,  -1.0f, 1.0f);
            wheelFR.Speed = WheelSpeedCoefficent * Limit(powerRight, -1.0f, 1.0f);
            wheelRR.Speed = WheelSpeedCoefficent * Limit(powerRight, -1.0f, 1.0f);
        }

Esimene funktsioon Limit on lühike abifunktsioon arvude piiramiseks minimaalse ja maksimaalse väärtusega. Seda funktsiooni kasutatakse Drive funktsioonis etteantud sõidukiiruste piiramiseks. Drive funktsiooni parameetriteks on vasaku ja parema poole mootorite sõidekiirused vahemikus -1 kuni +1 kus -1 on täiskiirus tagasi, +1 täiskiirus edasi ja 0 on seismine. Sõidukiirused piiratakse, korrutatakse koefitsendiga ja omistatakse vastava poole rataste Speed parameetritele. Kiiruste alusel pööratakse CustomWheel klassis rataste liigendeid ja simuleeritakse seeläbi mootoreid.

VirtualBallPickerTypes.cs

VirtualBallPicker projekti luues loodi automaatselt ka VirtualBallPickerTypes.cs fail millega senini pole näites midagi tehtud. Tegu on teenuse lepingut (Contract), oleku andmestruktuuri (State klass) ja liidest (PortSet) kirjeldava failiga. Teenuse lepingu identifikaator on faili automaatselt lisatud, oleku klassist ja liidesest on olemas struktuur. Fail peaks välja nägema sedasi:

using System;
using System.Collections.Generic;
using System.ComponentModel;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;
 
namespace VirtualBallPicker
{
    public sealed class Contract
    {
        [DataMember]
        public const string Identifier = "http://www.robotiklubi.ee/2009/01/virtualballpicker.html";
    }
 
    [DataContract]
    public class VirtualBallPickerState
    {
    }
 
    [ServicePort]
    public class VirtualBallPickerOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop, Get>
    {
    }
 
    public class Get : Get<GetRequestType, PortSet<VirtualBallPickerState, Fault>>
    {
        public Get()
        {
        }
 
        public Get(GetRequestType body)
            : base(body)
        {
        }
 
        public Get(GetRequestType body, PortSet<VirtualBallPickerState, Fault> responsePort)
            : base(body, responsePort)
        {
        }
    }
}

Olekuklassi VirtualBallPickerState on järgnevalt tagasiside demonstreerimiseks lisatud parameetrid aeg ning kiirus. Mõlematel on olemas nii väärtust hoidev muutuja kui parameeter mis tagastab või määrab muutuja väärtust. Olekuklassi objekti kasutatakse teenuse oleku pärimiseks.

    // Used for returning state
    [DataContract]
    public class VirtualBallPickerState
    {
        private DateTime time;
        private float speed;
 
        // Time
        [DataMember]
        public DateTime Time
        {
            get { return time; }
            set { time = value; }
        }
 
        // Speed
        [DataMember]
        public float Speed
        {
            get { return speed; }
            set { speed = value; }
        }
    }

Sarnaselt olekuklassiga kasutatakse teenusele andmete edastamiseks teateklassi või mitut. Järgnevalt on toodud programmikood DriveRequest teate klassist mille võiks peale olekuklassi lisada. DriveRequest teate klassil on kaks parameetrit - vasaku ja parema poole rataste sõidukiirus (PowerLeft ja PowerRight).

    // Used for issuing a drive request
    [DataContract]
    public class DriveRequest
    {
        private float powerLeft, powerRight;
 
        // The power with which to drive left wheels, from -1 to +1
        [DataMember, DataMemberConstructor]
        public float PowerLeft
        {
            get { return powerLeft; }
            set { powerLeft = value; }
        }
 
        // The power with which to drive right wheels, from -1 to +1
        [DataMember, DataMemberConstructor]
        public float PowerRight
        {
            get { return powerRight; }
            set { powerRight = value; }
        }
    }

Teenuse liideses tuleb ära defineerida lubatud operatsioonid. Esimene neist on päringu (Get) tüüpi ja teine uuenduse (Update) tüüpi. Päringuga tagastatakse VirtualBallPickerState tüüpi objekt, uuendusega edastatakse DriveRequest tüüpi objekt. Kirjapilt liidesest näeb välja järgnev:

    // Operations
    public class Get      : Get<GetRequestType, PortSet<VirtualBallPickerState, Fault>> { }
    public class Drive    : Update<DriveRequest, PortSet<DefaultUpdateResponseType, Fault>> { }
 
    // Service port
    [ServicePort]
    public class VirtualBallPickerOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop, Get, Drive>
    {
    }

Liidese operatsioonid

VirtualBallPicker.cs faili VirtualBallPickerService klassi lõppu tuleb lisada kaks „funktsiooni“ mis tegelevad teenuse oleku tagastamise ja sõidukäskude vastuvõtmisega. Nii nagu MSRS teenuste arhitektuuris selgitatud sai, toimub asünkrootsete operatsioonide täitmine ülesannete loetelu abil. Järgnevalt on näha kuidas see välja näeb:

        /*
         * Handler that processes state getting
         */
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public IEnumerator<ITask> OnGet(Get get)
        {
            // Time
            state.Time = DateTime.Now;
 
            // Speed
            state.Speed = Vector3.Length(entity.State.Velocity);
 
            // Post state
            get.ResponsePort.Post(state);
 
            yield break;
        }
 
        /*
         * Handler that processes a drive message
         */
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> OnDrive(Drive drive)
        {
            entity.Drive(drive.Body.PowerLeft, drive.Body.PowerRight);
 
            yield break;
        }

Loetelude „loendajate“ (Enumerator) tagastamise funktsioonide nimed OnGet ja OnDrive on suvaliselt määratud - nendest midagi ei sõltu. Kompilaator seob funktsioonid liideste operatsioonidega läbi funktsioonide parameetri.

Oleku päring võetakse täitmisele olenamata sellest kas mõni teenus juba olekut pärib või mitte, sest päring ei riku teenuse oleku järjepidavust. Kiiruse määramine toimub ainult ükshaaval, sest samaaegne kiiruse muutmine võib põhjustada ettearvamatuid tulemusi - näiteks vasaku poole mootorid võivad saada kiiruse ühtelt teenuselt, parema poole omad teiselt. Meetodid määrab ära kompilaatori parameeter ServiceHandlerBehavior.

Juhtimisteenus

Juhtpult, mille loomisest järgnevalt juttu tuleb, on sarnaselt roboti simulaatorilegi teenus. Tarkvaralahendusse (Solution) tuleb tekitada uus DSS Service (2.0) projekt nimega BallPickerRemoteControl mille partnerteenuseks on esimesena loodud VirtualBallPickerService.

Juhtimisteenuse loomine

Uude teenuse projekti tuleb lisada viit .NET teegile Microsoft.Ccr.Adapters.WinForms ja see selle kasutamine teenuse koodi ka kirja panna. Teenuse kood on järgnev:

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Windows.Forms;
using System.Threading;
 
using Microsoft.Ccr.Core;
using Microsoft.Ccr.Adapters.WinForms;  // add reference!
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;
 
using virtualballpicker = VirtualBallPicker.Proxy;
 
namespace BallPickerRemoteControl
{
    [Contract(Contract.Identifier)]
    [DisplayName("BallPickerRemoteControl")]
    [Description("Remote control interface to control virtual ball picker in simulation")]
    class BallPickerRemoteControlService : DsspServiceBase
    {
        // Service state
        [ServiceState]
        BallPickerRemoteControlState state = new BallPickerRemoteControlState();
 
        // Main service port
        [ServicePort("/BallPickerRemoteControl", AllowMultipleInstances = true)]
        BallPickerRemoteControlOperations mainPort = new BallPickerRemoteControlOperations();
 
        // Virtual ball picker partner
        [Partner("VirtualBallPickerService", Contract = virtualballpicker.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        virtualballpicker.VirtualBallPickerOperations virtualBallPickerServicePort = new virtualballpicker.VirtualBallPickerOperations();
 
        // Form
        ControlInterface form;
 
        /*
         * Service constructor
         */
        public BallPickerRemoteControlService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }
 
        /*
         * Service start
         */
        protected override void Start()
        {
            // Run form
            WinFormsServicePort.Post(new RunForm(CreateForm));
 
            // Get state
            Activate(Arbiter.ReceiveFromPortSet<virtualballpicker.VirtualBallPickerState>(
                false, virtualBallPickerServicePort.Get(), GetStateHandler));
 
            base.Start();
        }
 
        /*
         * Form constructor
         */
        Form CreateForm()
        {
            return form = new ControlInterface(virtualBallPickerServicePort);
        }
 
        /*
         * Get state handler
         */
        private void GetStateHandler(virtualballpicker.VirtualBallPickerState state)
        {
            // Post state to form
            if (form != null)
                form.OnStateReceive(state);
 
            // Get again in 50ms
            Thread.Sleep(50);
            Activate(Arbiter.ReceiveFromPortSet<virtualballpicker.VirtualBallPickerState>(
                false, virtualBallPickerServicePort.Get(), GetStateHandler));
        }
    }
}

Teenusesse on lisatud ControlInterface tüüpi muutuja form mis viitab peagi loodavale aknale. Akna loomine toimub teenuse käivitamisel läbi spetsiaalse aknaliidese (WinFormsServicePort). Akna konstruktorile antakse parameetrina kaasa partnerteenuse VirtualBallPickerService (virtuaalse roboti teenuse) liides virtualBallPickerServicePort kustkaudu saadetakse juhtimiskäske robotile.

Teenuse käivtamisel esitatakse partnerteenusele oleku päring mille saabumisel käivitatakse GetStateHandler funktsioon. Siinkohal pole kasutatud ülesannete loendit kuna päring toimub perioodiliselt - peale oleku saabumist tehakse lõimes 50 millisekundit paus ja esitatakse uus päring. Kuna aken töötab eraldi lõimes (mitte teenustesüsteemi pärast), siis paus teenuses ei sega akna tööd. Oleku saabumisel edastatakse oleku objekt akna OnStateReceive funktsioonile, kus toimub oleku parameetrite kuvamine.

Juhtpuldi aken

Juhtpuldiks on aken mida saab tekitada BallPickerRemoteControl projekti nimel teise hiirenupuga Add - Windows form… valides. Programmikoodi asemele ilmub uus aken. Paremale projekti failipuu alla ilmub Properties sektsioon kust saab muuta akna ja sellesse lisatavate objektide parameetreid. Valides akna tuleb Properties sektsioonis sellele nimeks ControlInterface panna.

Aknasse nuppude ja muude objektide lisamiseks tuleks nähtavaks teha Toolbox. See käib Visual Studio peamenüüst View - Toolbox vajutades. Akna vasakusse serva tuleks lisada neli „nuppu“, õigemini neli Label-it, sest nendega on pärastpoole teatud asjade puhul lihtsam opereerida. Nuppude nimedeks peaks vastavalt suunale määrama btnForward, btnBackward, btnLeft ja btnRight. Akna paremale serva läheb DataGridView nimega dgvStatus. Kõiki peensusi akna loomisel pole selle õpetuse juures kirjeldama hakatud, sest see läheb teemast välja. Akende loomise kohta leiab väga palju infot nii Visual Studio õpetusest kui internetist.

Järgnevalt on toodud kogu akna programmikood:

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
 
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.Core.DsspHttp;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;
 
using virtualballpicker = VirtualBallPicker.Proxy;
 
namespace BallPickerRemoteControl
{
    public partial class ControlInterface : Form
    {
        private virtualballpicker.VirtualBallPickerOperations port;
        private List<Keys> downKeys = new List<Keys>();
 
        public ControlInterface(virtualballpicker.VirtualBallPickerOperations _port)
        {
            // Simulator port
            port = _port;
 
            // Make form
            InitializeComponent();
 
            // Relate buttons with keys
            btnForward.Tag   = (object)Keys.Up;
            btnBackward.Tag  = (object)Keys.Down;
            btnLeft.Tag      = (object)Keys.Left;
            btnRight.Tag     = (object)Keys.Right;
 
            // Attach form keypress handlers
            this.KeyPreview = true;
            this.KeyDown += new KeyEventHandler(HandleKeyDown);
            this.KeyUp   += new KeyEventHandler(HandleKeyUp);
 
            // Attach mouse click handlers to buttons (labels)
            foreach (Control grpCtrl in this.Controls)
            {
                if (grpCtrl is GroupBox)
                {
                    foreach (Control btnCtrl in grpCtrl.Controls)
                    {
                        if (btnCtrl is Label)
                        {
                            btnCtrl.MouseDown += new MouseEventHandler(HandleMouseDown);
                            btnCtrl.MouseUp   += new MouseEventHandler(HandleMouseUp);
                        }
                    }
                }
            }
 
            // Create status parameters list
            dgvStatus.Rows.Add(new string[] { "Timestamp", "" });
            dgvStatus.Rows.Add(new string[] { "Speed",     "" });
        }
 
        /*
         * Key down handler
         */
        private void HandleKeyDown(object sender, KeyEventArgs e)
        {
            if (!downKeys.Contains(e.KeyData))
                downKeys.Add(e.KeyData);
            HandleUserControl();
            e.Handled = true;
        }
 
        /*
         * Key up handler
         */
        private void HandleKeyUp(object sender, KeyEventArgs e)
        {
            downKeys.Remove(e.KeyData);
            HandleUserControl();
            e.Handled = true;
        }
 
        /*
         * Mouse up handler
         */
        private void HandleMouseDown(object sender, MouseEventArgs e)
        {
            Keys key = (Keys)((Label)sender).Tag;
            if (!downKeys.Contains(key))
                downKeys.Add(key);
            HandleUserControl();
        }
 
        /*
         * Mouse up handler
         */
        private void HandleMouseUp(object sender, MouseEventArgs e)
        {
            Keys key = (Keys)((Label)sender).Tag;
            downKeys.Remove(key);
            HandleUserControl();
        }
 
        /*
         * Form focus lost
         */
        private void ControlForm_Deactivate(object sender, EventArgs e)
        {
            downKeys.Clear();
            HandleUserControl();
        }
 
        /*
         * User control handler
         */
        private void HandleUserControl()
        {
            float x, y, motorLeft, motorRight;
 
            // Calculate speed and turn
            y = TwoButtonValue(ButtonCheck(btnForward), ButtonCheck(btnBackward));
            x = TwoButtonValue(ButtonCheck(btnRight),   ButtonCheck(btnLeft));
 
            // Differential drive
            motorLeft  = Limit(y + x, -1.0f, 1.0f);
            motorRight = Limit(y - x, -1.0f, 1.0f);
 
            // Post commands
            port.Drive(motorLeft, motorRight);
        }
 
        /*
         * Value limiting
         */
        private float Limit(float value, float min, float max)
        {
            return (value < min ? min : (value > max ? max : value));
        }
 
        /*
         * Value from two keys
         */
        private float TwoButtonValue(bool up, bool down)
        {
            return (up ? 1.0f : 0.0f) - (down ? 1.0f : 0.0f);
        }
 
        /*
         * Visualize button presses and return state
         */
        private bool ButtonCheck(Label btn)
        {
            bool isDown = downKeys.Contains((Keys)btn.Tag);
            btn.BackColor = (isDown ? Color.Coral : SystemColors.Control);
 
            return isDown;
        }
 
        /*
         * Simulator state receiver
         */
        public void OnStateReceive(virtualballpicker.VirtualBallPickerState state)
        {
            int i = 0;
 
            if (this.IsDisposed)
                return;
 
            dgvStatus.Rows[i++].Cells[1].Value = state.Time.ToString();
            dgvStatus.Rows[i++].Cells[1].Value = String.Format("{0:0.000} m/s", state.Speed);
        }
    }
}

Seda programmikoodi täielikult lahti seletama ei hakka, sest see ei puuduta otseselt MSRS-i. Aga mõned selgitused siiski:

Juhtimisaknas on kasutatud Label-e sest need ei oma fookust. Programmikoodi uurimisel on näha, et juhtida saab nii hiirekursoriga „nuppudel“ vajutades kui klaviatuuri nooli kasutades. Kuna hiireklikiga fokuseeritaks tavaline Button ära siis nooleklahvide vajutusi enam kinni ei püütaks (küll aga kõiki teisi). Pealegi oli mõte klaviatuuri klahvivajutusi kuidagi ka kuvada ja kuna nuppude allavajutatud olekuid programmis tekitada ei saanud, siis toimubki see Label-i taustavärvi muutmisega.

Programmikoodis on näha port muutuja millele omistatakse teenuse poolt akna konstruktoriga kaasa antud virtuaalse roboti teenuse liides. Selle kaudu saadetakse nupuvajutuste peale sõidukäsk (Drive). Funktsioon OnStateReceive, mida kutsub välja teenus oleku saabumisel, kuvab oleku parameetreid dgvStatus tabelis.

Testimine

Nüüd on kõik kood kirjutatud mis vajalik roboti sõidutamiseks. Enne proovimist tuleb siiski teha veel paar seadistust. Esiteks tuleb tarkvaralahenduses StartUp projektiks määrata BallPickerRemoteControl - see toimub projekti nimel hiire teise nupuga vajutades ja menüüst Set as StartUp project valides. Vastava projekti nimi peaks jämedas kirjas ilmuma.

Teiseks tuleb BallPickerRemoteControl projekti Debug seadistustes muuta Command line arguments: lahtrit. Et koos juhtimisteenusega ka virtuaalse roboti teenus käivitada peab lisama veel ühe „/m“ argumendi VirtualBallPicker manifestiga. Lahtri sisu on järgnev:

/p:50000 /t:50001 /m:"../Documents/Robotiklubi/MSRS Simulation/BallPickerRemoteControl/BallPickerRemoteControl.manifest.xml" /m:"../Documents/Robotiklubi/MSRS Simulation/VirtualBallPicker/VirtualBallPicker.manifest.xml"

Kaustade nimed peab igaüks muidugi vastavalt oma seadistustele ise ära muutma. Kui nüüd projekt käivitada peaks juhtimisaknast saama robotit sõidutada:

Sõiduproov

10. Manipulaatori loomine

Järgmisena tuleb käsile manipulaatori (käpa) lisamine sõitvale robotiplatvormile. Manipulaatori saab luua mitmest liigenditega ühendatud füüsiliselt mudelist. Robotil, mis sai SolidWorksis disainitud, on 4 lüli millest esimene on püstteljel pöörduv manipulaatori alus, järgmised 2 on põikiteljel pöörduvad ja neljas moodustab haaratsi.

Tuleb taas avada VirtualBallPicker projekti BallPickerEntity.cs fail ja klassi BallPickerEntity päisesse lisada mõned read, nii et see paistaks välja järgmiselt:

        // Mesh locations
        private const String ModelsPath  = "../../../Documents/Robotiklubi/MSRS Simulation/Models/obj";
        private const String ChassisMesh  = ModelsPath + "/chassis.obj";
        private const String WheelMesh    = ModelsPath + "/wheel.obj";
        private const String ArmBaseMesh  = ModelsPath + "/arm_base.obj";
        private const String ArmLink1Mesh = ModelsPath + "/arm_link1.obj";
        private const String ArmLink2Mesh = ModelsPath + "/arm_link2.obj";
        private const String ArmLink3Mesh = ModelsPath + "/arm_link3.obj";
 
        // Constants
        private const int NumLinks = 4;
        private const float WheelSpeedCoefficent = 5.0f;
        private const float LinkSpeedCoefficent  = 0.5f;
 
        // Materials
        private MaterialProperties plasticMaterial;
 
        // Child entities
        private CustomWheel wheelFL, wheelFR, wheelRL, wheelRR;
        private MultiShapeEntity[] linkEntity;
        private PhysicsJoint[] linkJoint;
 
        // Arm link angles
        private float[] linkAngle      = new float[NumLinks];
        private float[] linkDeltaAngle = new float[NumLinks];

Kere visuaalsete mudelifailid asukohad, rataste kiiruse konstant WheelSpeedCoefficent, materjali muutuja plasticMaterial ja rataste muutujad peaks eelnevalt juba lisatud olema, nii et neid dubleerima ei pea. Analoogselt ratastele on ka manipulaatori puhul kasutusel visuaalsed mudelifailid ja lülide liigutamise kiiruse koefitsent. Erinevalt ratastest pole manipulaatori lülide jaoks aga spetsiaalselt klassi sest kõik lülid on erinevad nii vormilt kui parameetritelt ja ühiseid jooni pole nii palju, et oleks mõtet eraldi klass luua.

Kõik lülide olemid on MultiShapeEntity tüüpi, mis laiendab VisualEntity klassi ja teeb tüüpilistest füüsiliste kujunditest mudeli koostamise lihtsamaks. Lülide olemid, liigendid ja nurgad on kõik massiivid millede pikkus on ära määratud konstandiga NumLinks. Ujukomaarvu (float) tüüpi massiiv linkAngle peab arvet lülide nurkade kohta ja linkDeltaAngle on nende muutumise kiirus. Nurkade kasutamine selgub manipulaatori liigutamise juures, esmalt tuleb aga manipulaator tekitada.

BallPickerEntity klassi Initialize funktsioonis tuleks peale rataste tekitamise funktsiooni välja kutsuda ka CreateArm funktsioon. Funktsioon, mis klassi lõppu lisada, on ise allpool. Kuna see on üsna pikk koodijada on kommentaarid lisatud ridade vahele. Funktsiooni saab erinevaid lõike kokku kleepides.

        /*
         * Create arm (manipulator)
         */
        private void CreateArm()
        {
            int i;
 
            linkEntity = new MultiShapeEntity[NumLinks];
            linkJoint  = new PhysicsJoint[NumLinks];
 
            // Create entities
            for (i = 0; i < NumLinks; i++)
            {
                linkEntity[i] = new MultiShapeEntity();
                linkEntity[i].State.Name = "arm link " + i.ToString();
                linkEntity[i].State.MassDensity.AngularDamping = 10.0f;
                linkEntity[i].State.MassDensity.LinearDamping  = 10.0f;
                linkAngle[i] = 0.0f;
                linkDeltaAngle[i] = 0.0f;
            }

Esimese asjana luuakse lülide olemite ja liigendite massiivid. Igale lülile omistatakse nimi vastavalt järjekorranumbrile. Lülide olemite parameetrid State.MassDensity.AngularDamping ja State.MassDensity.LinearDamping on pöörd- ja sirgliikumise summutamiseks. Sumbuvust on mõistlik lisada manipulaatori järskude tõmbluste vältimiseks. Täpsemalt saab nendest parameetritest aru neile erinevaid väärtusi andes.

			// Create link 0 - arm base			
			BoxShapeProperties link0Box = new BoxShapeProperties
			(				 
				1.0f,                                            // mass
				new Pose(new Vector3(0.0f, 0.01f, 0.0f)),        // pose
				new Vector3(0.14f, 0.02f, 0.14f)                 // dimensions
			);
 
			BoxShapeProperties link0BoxA = new BoxShapeProperties
			(				 
				0.3f,                                            // mass
				new Pose(new Vector3(-0.04f, 0.045f, 0.0f)),     // pose
				new Vector3(0.04f, 0.05f, 0.06f)                 // dimensions
			);
 
			BoxShapeProperties link0BoxB = new BoxShapeProperties
			(				 
				0.3f,                                            // mass
				new Pose(new Vector3(0.04f, 0.045f, 0.0f)),      // pose
				new Vector3(0.04f, 0.05f, 0.06f)                 // dimensions
			);
 
			link0Box.Material = plasticMaterial;			
			linkEntity[0].BoxShapes.Add(new BoxShape(link0Box));
			linkEntity[0].BoxShapes.Add(new BoxShape(link0BoxA));
			linkEntity[0].BoxShapes.Add(new BoxShape(link0BoxB));
			linkEntity[0].State.Assets.Mesh = ArmBaseMesh;

See koodilõik lisab manipulaatori aluse olemile linkEntity[0] kolm kasti. SolidWorksi mudelil oli manipulaatori alus ümmargune, kuid silindrit füüsilise lihtobjektina MSRS-is (ega PhysX-is) pole, seega on kasutatud lihtsustust ja silindrit asendab kast. Kuna manipulaatori alus satub üsna harva pallidega kontakti siis pole sel ka erilist tähtsust. Kaks teist kasti (link0BoxA ja link0BoxB) tekitavad manipulaatori alusele 2 järgmise lüli kinnitusalust. Nendegi puhul on kumer visuaalne mudel kandilise kastiga asendatud. Kastid on füüsilise mudelisse lisatud selliselt, et null-punktiks jääks manipulaatori aluse põhja keskpunkt. Viimasena on lülile omistatud visuaalne mudel.

            // Create link 1
            BoxShapeProperties link1Box = new BoxShapeProperties
            (
                0.6f,                                             // mass
                new Pose(new Vector3(0.0f, 0.0f, -0.075f)),       // pose
                new Vector3(0.04f, 0.04f, 0.19f)                  // dimensions
            );
 
            link1Box.Material = plasticMaterial;
            linkEntity[1].BoxShapes.Add(new BoxShape(link1Box));
            linkEntity[1].State.Assets.Mesh = ArmLink1Mesh;
 
            // Create link 2
            BoxShapeProperties link2Box = new BoxShapeProperties
            (
                0.4f,                                             // mass
                new Pose(new Vector3(0.0f, 0.0f, -0.055f)),       // pose
                new Vector3(0.04f, 0.04f, 0.15f)                  // dimensions
            );
 
            BoxShapeProperties link2Box1A = new BoxShapeProperties
            (
                0.05f,                                            // mass
                new Pose(new Vector3(-0.015f, 0.015f, -0.1521f)), // pose
                new Vector3(0.01f, 0.01f, 0.0442f)                // dimensions
            );
 
            BoxShapeProperties link2Box2A = new BoxShapeProperties
            (
                0.05f,                                            // mass
                new Pose(new Vector3(0.015f, 0.015f, -0.1521f)),  // pose
                new Vector3(0.01f, 0.01f, 0.0442f)                // dimensions
            );
 
            BoxShapeProperties link2Box1B = new BoxShapeProperties
            (
                0.05f,                                            // mass
                new Pose                                          // pose
                (
                    new Vector3(-0.015f, -0.00268f, -0.18975f),
                    Quaternion.FromAxisAngle(new AxisAngle(Vector3.XAxis, (float)Math.PI / -4.0f))
                ),
                new Vector3(0.01f, 0.01f, 0.05414f)               // dimensions
            );
 
            BoxShapeProperties link2Box2B = new BoxShapeProperties
            (
                0.05f,                                            // mass
                new Pose                                          // pose
                (
                    new Vector3(0.015f, -0.00268f, -0.18975f),
                    Quaternion.FromAxisAngle(new AxisAngle(Vector3.XAxis, (float)Math.PI / -4.0f))),
                new Vector3(0.01f, 0.01f, 0.05414f)               // dimensions
            );
 
            link2Box.Material   = plasticMaterial;
            link2Box1A.Material = plasticMaterial;
            link2Box2A.Material = plasticMaterial;
            link2Box1B.Material = plasticMaterial;
            link2Box2B.Material = plasticMaterial;
 
            linkEntity[2].BoxShapes.Add(new BoxShape(link2Box));
            linkEntity[2].BoxShapes.Add(new BoxShape(link2Box1A));
            linkEntity[2].BoxShapes.Add(new BoxShape(link2Box2A));
            linkEntity[2].BoxShapes.Add(new BoxShape(link2Box1B));
            linkEntity[2].BoxShapes.Add(new BoxShape(link2Box2B));
 
            linkEntity[2].State.Assets.Mesh = ArmLink2Mesh;
 
            // Create ink 3
            BoxShapeProperties link3BoxA = new BoxShapeProperties
            (
                0.1f,                                            // mass
                new Pose(new Vector3(0.0f, 0.0f, -0.03f)),       // pose
                new Vector3(0.02f, 0.01f, 0.08f)                 // dimensions
            );
 
            BoxShapeProperties link3BoxB = new BoxShapeProperties
            (
                0.05f,                                           // mass
                new Pose                                         // pose
                (
                    new Vector3(0.0f, 0.01915f, -0.08394f),
                    Quaternion.FromAxisAngle(new AxisAngle(Vector3.XAxis, (float)Math.PI / 3.0f))
                ),
                new Vector3(0.02f, 0.01f, 0.05f)                 // dimensions
            );
 
            link3BoxA.Material = plasticMaterial;
            link3BoxB.Material = plasticMaterial;
 
            linkEntity[3].BoxShapes.Add(new BoxShape(link3BoxA));
            linkEntity[3].BoxShapes.Add(new BoxShape(link3BoxB));
            linkEntity[3].State.Assets.Mesh = ArmLink3Mesh;

Kolm eelnenud koodilõigus toodud lüli on loodud samal meetodil mis esimenegi. Erinevus seisneb ainult nimes ja mudelis. Pika murdosaga arvud mis füüsiliste kujundite asukohtadeks on antud pärinevad SolidWorksist. Mudeli loomise peatükis on kirjutatud mõõdu võtmisest SolidWorksis ja siinkohal ongi seda vaja läinud. Kuna neljas lüli on manipulaatori haarats mis haarab palle, ei saa seal lubada ebatäpset mudelit ja seepärast ongi see täpselt ära kirjeldatud. Kuna haaratsitel on 45 kraadine jõnks siis tuleb ka füüsilisi mudeleid täpselt samapalju keerata.

            // Convert imported mesh models to current coordinate system
            for (i = 0; i < NumLinks; i++)
            {
                linkEntity[i].MeshRotation = new Vector3(0.0f, 180.0f, 0.0f);
            }

Kuna SolidWorksi mudelid on teistpidi siis siin toimub kõigi lülide visuaalse mudeli korraga paika keeramine.

 
            // Joint drive properties
            JointDriveProperties driveProperties = new JointDriveProperties
            (
                JointDriveMode.Position,  // mode
                new SpringProperties
                (
                    100.0f,  // spring coefficent
                    5.0f,    // damper coefficent
                    0.1f     // equilibrium position
                ),
                500.0f       // force limit
            );

Selles lõigus luuakse driveProperties objekt mis hakkab iseloomustama liigendite pöördliikumist. Esimene JointDriveProperties klassi konstruktori parameeter JointDriveMode.Position näitab, et liigendit hakatakse juhtima nurga järgi. Teine variant sellest on JointDriveMode.Velocity mis tähendab liigendi juhtimist pöörlemise kiirusega ja seda režiimi on kasutatud ka eelnevalt toodud CustomWheel klassis. Teine konstruktori parameeter on juba tuttav vetruvus ja kolmas on maksimaalne mida liigend talub, enne kui see puruneb.

            // Joint Swing1 angular properties
            JointAngularProperties commonAngularSwing1 = new JointAngularProperties();
            commonAngularSwing1.TwistMode  = JointDOFMode.Locked;   // local axis
            commonAngularSwing1.Swing1Mode = JointDOFMode.Free;     // normal
            commonAngularSwing1.Swing2Mode = JointDOFMode.Locked;   // binormal
            commonAngularSwing1.SwingDrive = driveProperties;
 
            // Joint Twist angular properties
            JointAngularProperties commonAngularTwist = new JointAngularProperties();
            commonAngularTwist.TwistMode  = JointDOFMode.Free;      // local axis
            commonAngularTwist.Swing1Mode = JointDOFMode.Locked;    // normal
            commonAngularTwist.Swing2Mode = JointDOFMode.Locked;    // binormal
            commonAngularTwist.TwistDrive = driveProperties;

Kaks muutujat commonAngularSwing1 ja commonAngularTwist määravad ära teljed millel liigendi pöördub. Siinkohal leiab abi järgmisest joonisest:

MSRS liigendi teljed

Koodis ja joonisel nähtavad TwistMode, Swing1Mode ja Swing2Mode on pöördliikumise režiimid Local axis, Normal ja Binormal telgedel. Nimetatud teljed tähistavad olemi lokaalset teljestikku. Globaalselt, ehk maailmas, tähistavad nad X, Y ja Z telgesid. Lokaalne teljestik võib algajal roboti loomise päris keeruliseks teha ja lihtsuse huvides on mõistlik lokaalne teljestik hoida ühtne globaalsega. Mismoodi see käib, selgub peagi.

Iga telje puhul määratakse ära kas see on lukustatud (locked), ehk selle ümber pöördumist ei toimu, vaba (free) või piiratud (limited). Vabale teljele tuleb määrata pöörliikumise parameeter. Esimesel, commonAngularSwing1 teljeparameetril on pöörlemine lubatud Normal teljel ja sellega iseloomustatakse manipulaatori alust, teisel commonAngularTwist teljeparameetril on pöörlemine lubatud Local axis teljel ja sellega iseloomustatakse manipulaatori kolme ülejäänud lüli.

            // Create joints
            for (i = 0; i < NumLinks; i++)
            {
                // Joint properties
                JointProperties jointProperties = new JointProperties((i == 0 ? commonAngularSwing1 : commonAngularTwist), null, null);
                jointProperties.EnableCollisions = false;
 
                // Create joint
                linkJoint[i] = PhysicsJoint.Create(jointProperties);
                linkJoint[i].State.Name = linkEntity[i].State.Name + " joint";
 
                // Manual projection settings - to avoid bizarre movements
                linkJoint[i].State.Projection = new JointProjectionProperties();
                linkJoint[i].State.Projection.ProjectionMode = JointProjectionMode.PointMinimumDistance;
                linkJoint[i].State.Projection.ProjectionDistanceThreshold = 0.01f;
                linkJoint[i].State.Projection.ProjectionAngleThreshold = 0.01f;
            }

See koodijupp hoolitseb liigendite loomise eest. Kuna enamus programmikoodi manipulaatori lülidel ühine siis täidetakse käsud tsükliliselt. Esimesna luuakse liigendi parameetri klassist JointProperties instants. Klass konstruktori esimeseks parameetriks on liigendi telge iseloomustav muutuja. Eespool sai öeldud, et manipulaatori esimene lüli pöördub püstteljel (Y) ja teised põikiteljel (X) seepärast on liigendi parameetri konstruktori esimene parameeter valikuline vastavalt liigendi järjekorranumbrile.

Liigendi parameetril määratakse tõeseks EnableCollisions parameeter mis tagab selle, et mõlemad liigendiga seotud füüsilised olemid ei põrku ega hõõrdu. Justnimelt höördumine on põhjus miks omavahelise kontakti kontrolli peaks ära keelama. Kui olemid on lähestikku ja mõned lülid lausa kattuvad, siis hakkab höördumine mõjutama lülide liikumist. Võiks lülid ka mitte-kattuvalt ja väikeste vahedega kirjeldada, kuid see on keerukam ja tüütu. Seega on põhjuseks lihtsus. Halb on see, et lülid võivad peale teha 360 kraadi pöördeid mis reaalselt on võimatu, kuid lülide nurkasid võib linkAngle massiivis ka piirata. Oluline on teada, et kokkupõrke kontrolli lubamine/keelamine kehtib ainult liigendiga seotud olemite vahel. See tähendab, et alati põrkuvad näiteks esimene ja kolmas lüli, teine ja neljas, jne.

Edasi luuakse juba konktreetne lüli instants ja antakse sellele nimi vastavalt liigendi järjekorranumbrile. State.Projection parameeter on jällegi üks näide simulatsioonis ebaharilike tõmblemiste välistamiseks. Seda ei pruugi vaja minna, kuid hea on teada, et midagi sellist on olemas. Autor ei oska jällegi ka täpselt öelda millega on tegu, aga niipalju kui selle kohta lugeda võib hoiab see ära liigendi pöörlemise lukustatud telgedel. Jah tõesti - liigend võib mingitel juhtudel pöörduda ka lukustatud teljel. Selle näite käigus kirjeldatud ekstreemsusi ei tohiks juhtuda aga mahukamate simulatsioonide ja suuremate jõudue korral on kõik võimalik.

            // Chass and link 0 joint connectors
            linkJoint[0].State.Connectors[0] = new EntityJointConnector
            (
                this,                           // entity
                Vector3.YAxis,                  // normal
                Vector3.XAxis,                  // local axis
                new Vector3(0.0f, 0.07f, -0.1f) // connector point
            );
 
            linkJoint[0].State.Connectors[1] = new EntityJointConnector
            (
                linkEntity[0],                  // entity
                Vector3.YAxis,                  // normal
                Vector3.XAxis,                  // local axis
                new Vector3(0.0f, 0.0f, 0.0f)   // connector point
            );
 
            // Link 0 and 1 joint connectors
            linkJoint[1].State.Connectors[0] = new EntityJointConnector
            (
                linkEntity[0],                  // entity
                Vector3.YAxis,                  // normal
                Vector3.XAxis,                  // local axis
                new Vector3(0.0f, 0.05f, 0.0f)  // connector point
            );
 
            linkJoint[1].State.Connectors[1] = new EntityJointConnector
            (
                linkEntity[1],                  // entity
                Vector3.YAxis,                  // normal
                Vector3.XAxis,                  // local axis
                new Vector3(0.0f, 0.0f, 0.0f)   // connector point
            );
 
            // Link 1 and 2 joint connectors
            linkJoint[2].State.Connectors[0] = new EntityJointConnector
            (
                linkEntity[1],                  // entity
                Vector3.YAxis,                  // normal
                Vector3.XAxis,                  // local axis
                new Vector3(0.0f, 0.0f, -0.15f) // connector point
            );
 
            linkJoint[2].State.Connectors[1] = new EntityJointConnector
            (
                linkEntity[2],                  // entity
                Vector3.YAxis,                  // normal
                Vector3.XAxis,                  // local axis
                new Vector3(0.0f, 0.0f, 0.0f)   // connector point
            );
 
            // Link 2 and 3 joint connectors
            linkJoint[3].State.Connectors[0] = new EntityJointConnector
            (
                linkEntity[2],                  // entity
                Vector3.YAxis,                  // normal
                Vector3.XAxis,                  // local axis
                new Vector3(0.0f, 0.0f, -0.11f) // connector point
            );
 
            linkJoint[3].State.Connectors[1] = new EntityJointConnector
            (
                linkEntity[3],                  // entity
                Vector3.YAxis,                  // normal
                Vector3.XAxis,                  // local axis
                new Vector3(0.0f, 0.0f, 0.0f)   // connector point
            );
        }

Sellega lõppes CreateArm funktsioon. Viimasena on lülid liigenditega ära seotud. Liigendil on EntityJointConnector tüüpi State.Connectors massiiv pikkusega 2 mis hõlmab mõlema olemi liitekoha seadistusi. EntityJointConnector konstruktori esimene parameeter on olemi instants, teine ja kolmas on olemi lokaalsed teljed, mida aga juba eelnevalt sai soovitatud hoida globaalse teljestikuga ühtsena. Neljas parameeter on olemi liitepunkti relatiivne koordinaat (olemi suhtes). Kuna lülid said koostatud selliselt, et nende null-punkis asub pöördtelg siis ongi iga liigendi ühe lüli liitepunktiks null-punkt.

Nüüd tuleb tagasi Initialize funktsiooni pöörduda ja lisada sinna järgnevad read mis hoolitsevad lülide olemite ja liigendite loomise eest koos roboti olemise ajal:

            // Initalize link entities and joints
            for (int i = 0; i < NumLinks; i++)
            {
                linkEntity[i].Initialize(device, physicsEngine);
                physicsEngine.InsertJoint(linkJoint[i]);
            }

Täpselt nagu oli ratastega, tuleb ka manipulaatori lülide olemeid uuendada roboti olemi BallPickerEntity funktsioonis Update:

			// Update links
			Quaternion target;
 
			for (int i = 0; i < NumLinks; i++)
			{
				// Calculate angle
				linkAngle[i] += linkDeltaAngle[i] * (float)update.ElapsedRealTime;
 
				// Apply new orientation				
				target = Quaternion.FromAxisAngle(new AxisAngle((i == 0 ? Vector3.NegativeYAxis : Vector3.XAxis), linkAngle[i]));
				linkJoint[i].SetAngularDriveOrientation(target);
 
				// Update
				linkEntity[i].Update(update);
			}

Uuendamisel toimub liigendi nurkade linkAngle väärtustele nurkade muudu linkDeltaAngle liitmine. Muut on sisuliselt nurga muutumise kiirus radiaanides sekundis sest see korrutatakse ajaga mis möödus eelmisest uuendusfunktsioonist. Arvutatud nurga väärtused omistatakse manipulaatori liigenditele SetAngularDriveOrientation funktsiooni kasutades. Quaternion.FromAxisAngle funktsioon loob kvaterniooni teljest ja nurgast. Kuna esimene lüli oli püstteljel manipulaatori alus siis selle teljeks on Y ja teistel X. Vector3.NegativeYAxis on kasutatud pöörlemissuuna inverteerimiseks.

Loomulikult tuleb lülisid sarnaselt ratastele ka kuvada ja seda funktsioonis Render:

            // Render links
            for (int i = 0; i < NumLinks; i++)
            {
                linkEntity[i].Render(renderMode, transforms, currentCamera);
            }

Manipulaatori lüli liigutamiseks peab BallPickerEntity klassi viimasena lisama veel MoveLink funktsiooni:

        /*
         * Arm link moving command
         */
        public void MoveLink(int index, float deltaAngle)
        {
            if ((index < 0) || (index >= NumLinks)) return;

            linkDeltaAngle[index] = LinkMoveCoefficent * deltaAngle;
        }

Edasi tuleb jällegi toimida nagu rataste juhtimise puhul. VirtualBallPickerTypes.cs faili lisada LinkMoveRequest teate klass:

    // Used for issuing a arm link move request
    [DataContract]
    public class LinkMoveRequest
    {
        private int linkIndex;
        private float deltaAngle;
 
        // The link index
        [DataMember, DataMemberConstructor]
        public int LinkIndex
        {
            get { return linkIndex; }
            set { linkIndex = value; }
        }
 
        // The link angle change in degrees
        [DataMember, DataMemberConstructor]
        public float DeltaAngle
        {
            get { return deltaAngle; }
            set { deltaAngle = value; }
        }
    }

Juurde tuleb tekitada liidese uuendusoperatsioon:

    public class MoveLink : Update<LinkMoveRequest, PortSet<DefaultUpdateResponseType, Fault>> { }

Ja see ka liidesesse lisada:

    // Service operations
    [ServicePort]
    public class VirtualBallPickerOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop, GetState, Drive, MoveLink>
    {
    }

VirtualBallPicker.cs faili, peale rataste sõiduoperatsioonidega tegelevat ülesanneteloendid OnDrive tuleb lisada ülesannete loend mis manipulaatori lüli liigutab:

        /*
         * Handler that processes a arm moving message
         */
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> OnMoveLink(MoveLink move)
        {
            entity.MoveLink(move.Body.LinkIndex, move.Body.DeltaAngle);
 
            yield break;
        }

Nüüd on virtuaalse roboti teenussesse kõik manipulaatorit puudutav lisatud ja tuleb juhtimisteenuses lisada funktsioonid selle liigutamiseks. Esmalt tuleks juhtakna koodi ControlInterface konstruktorisse sõidunuppude seadistuse juurde lisada manipulaatori liigutamise nupud:

            btnArmLeft.Tag   = (object)Keys.A;
            btnArmRight.Tag  = (object)Keys.D;
            btnLink1Up.Tag   = (object)Keys.W;
            btnLink1Down.Tag = (object)Keys.S;
            btnLink2Up.Tag   = (object)Keys.R;
            btnLink2Down.Tag = (object)Keys.F;
            btnRelease.Tag   = (object)Keys.C;
            btnCrab.Tag      = (object)Keys.V;

Ja teise täiendusena lisada HandleUserControl funktsiooni virtuaalse roboti liidesele manipulaatori liigutamise käske saatavad read. Iga rida saadab manipulaatori ühe liigendi kiiruse.

            port.MoveLink(0, TwoButtonValue(ButtonCheck(btnArmRight), ButtonCheck(btnArmLeft)));
            port.MoveLink(1, TwoButtonValue(ButtonCheck(btnLink1Up), ButtonCheck(btnLink1Down)));
            port.MoveLink(2, TwoButtonValue(ButtonCheck(btnLink2Up), ButtonCheck(btnLink2Down)));
            port.MoveLink(3, TwoButtonValue(ButtonCheck(btnCrab), ButtonCheck(btnRelease)));

Kui kõik eelnev on tehtud võib simulaatorit testida.

11. Pallide lisamine

Lõplikust näitest on puudu veel vaid pallid. Nendega pole aga midagi keerulist. Esmalt tuleb VirtualBallPicker.cs faili VirtualBallPickerService klassi lisada palli lisamise funktsioon AddBall:

        /*
         * Add ball
         */
        void AddBall(float mass, float radius, MaterialProperties material, Vector3 pos,  Vector4 color)
        {
            SingleShapeEntity ballEntity = new SingleShapeEntity();
            SphereShapeProperties ballProperties = new SphereShapeProperties
            (
                mass,          // mass
                new Pose(pos), // pose
                radius         // radius
            );
 
            ballProperties.Material = material;
            ballEntity.SphereShape = new SphereShape(ballProperties);
            ballEntity.State.Name = "ball " + Guid.NewGuid().ToString();
            ballEntity.SphereShape.State.DiffuseColor = color;
 
            globalSimPort.Insert(ballEntity);
        }

Kõik klassid ja parameetrid selles funktsioonis peaksid nüüdseks selged olema ja seepärast neid kirjeldama ei hakka. Järgmisena tuleb teha pallide lisamise funktsioon:

        /*
         * Balls adding
         */
        private void AddBalls(int count)
        {
            // Create ball material
            MaterialProperties ballMaterial = new MaterialProperties
            (
                "ball",  // name
                0.01f,   // restition
                0.5f,    // dynamic friction
                0.5f     // static friction
            );
 
            // Add 25 random balls
            Random random = new Random();
            for (int i = 0; i < count; i++)
            {
                Vector4 color;
 
                // Make random color - red or blue
                if (random.NextDouble() > 0.5)
                    color = new Vector4(0.7f, 0.2f, 0.2f, 1.0f);
                else
                    color = new Vector4(0.2f, 0.2f, 0.7f, 1.0f);
 
                // Position ball randomly within 10 x 10 m area
                AddBall
                (
                    0.05f, // mass
                    0.03f, // radius
                    ballMaterial, // material
                    new Vector3
                    (
                        10.0f * ((float)random.NextDouble() - 0.5f),
                        1.0f,
                        10.0f * ((float)random.NextDouble() - 0.5f)
                    ),
                    color
                );
            }
        }

Selle funktsiooni parameetriks on pallide arv mida tekitada. Kasutusel on juhuslike arvude genereerimise klass Random mille abil leitakse pallidele suvaline asukoht 10 x 10 meetri ulatuses. Lisaks kasutatakse juhuslikku värvi valikut. Tõenäosusega 50% saavad pallid nii punast kui sinist värvi.

Pallide lisamise eest vastutab PopulateWorld funktsioon kuhu tuleks lisada rida:

            AddBalls(25);

Sellega ongi näide läbi tehtud ja võib proovida kuidas robotiga pallide korjamine toimub. Allpool on ka viited videodele mis tehtud pallide korjamisest.

Meedia

Pildid

Oops, aga mitte muhvigi ei leitud.

Videod

Failid

Infomaterjalid

Autor

juhendid/msrs_simulatsioon.txt · Viimati muutnud: 2016/12/08 14:24 persoon raimond.vaba