BowlerStudioExampleRobots/exampleCad.groovy at master · CommonWealthRobotics/BowlerStudioExampleRobots · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
import com.neuronrobotics.bowlerstudio.creature.ICadGenerator
import com.neuronrobotics.bowlerstudio.creature.MobileBaseCadManager
import com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics
import com.neuronrobotics.sdk.addons.kinematics.MobileBase
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR

import eu.mihosoft.vrl.v3d.CSG
import eu.mihosoft.vrl.v3d.Cube
import javafx.scene.transform.Affine

class myCadGen implements ICadGenerator{
	eu.mihosoft.vrl.v3d.parametrics.CSGDatabaseInstance csgdb;
	public myCadGen(eu.mihosoft.vrl.v3d.parametrics.CSGDatabaseInstance db) {
		csgdb=db;
	}
	@Override
	public ArrayList<CSG> generateCad(DHParameterKinematics d, int i) {
		MobileBaseCadManager manager  = MobileBaseCadManager.get(csgdb,d.getLinkConfiguration(i));
		TransformNR offset = d.getDHStep(i).inverse();

		ArrayList<CSG> back =[]
		back.add(new Cube(1).toCSG())
		for(CSG c:back)
			c.setManipulator(d.getLinkObjectManipulator(i))
		Affine lastLinkAffine = i==0? d.getRootListener() :d.getListener(i-1);
		Affine manipulator = d.getListener(i);

		if(manager!=null) {
			back.addAll(manager.getOriginVitaminsDisplay(csgdb,
				d.getAbstractLink(i),
				manipulator,offset));
			back.addAll(manager.getDefaultVitaminsDisplay(csgdb,
				d.getAbstractLink(i),
				manipulator));
			back.addAll(manager.getPreviousLinkVitaminsDisplay(csgdb,
				d.getAbstractLink(i),
				lastLinkAffine));
		}else{
			println "No manager found for "+d.getScriptingName()+" "+i
		}
		for(CSG c:back) {
			c.getStorage().set("no-physics",true)
		}
		if(manager!=null) {
			back.addAll(manager.getOriginVitamins(csgdb,
				d.getAbstractLink(i),
				manipulator,offset));
			back.addAll(manager.getDefaultVitamins(csgdb,
				d.getAbstractLink(i),
				manipulator));
			back.addAll(manager.getPreviousLinkVitamins(csgdb,
				d.getAbstractLink(i),
				lastLinkAffine));
		}
		for(CSG c:back) {
			if( c!=null)
				c.setManufacturing({return null})
		}
		return back;
	}

	@Override
	public ArrayList<CSG> generateBody(MobileBase b) {
		MobileBaseCadManager manager  = MobileBaseCadManager.get(csgdb,b);
		ArrayList<CSG> back =[]
		back.addAll(manager.getVitaminsDisplay(csgdb,b,b.getRootListener()));
		for(CSG c:back) {
			c.getStorage().set("no-physics",true)
		}
		back.addAll(manager.getVitamins(csgdb,b,b.getRootListener()));
		for(CSG c:back) {
			c.setManufacturing({return null})
		}
		for(DHParameterKinematics kin:b.getAllDHChains()) {
			CSG limbRoot =new Cube(1).toCSG()
			limbRoot.setManipulator(kin.getRootListener())
			back.add(limbRoot)

		}
		return back;
	}


}

return new myCadGen(csgdb)