-
Notifications
You must be signed in to change notification settings - Fork 96
/
Copy pathnewtonsCradle.cpp
executable file
·125 lines (94 loc) · 4.3 KB
/
newtonsCradle.cpp
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
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
// This file is part of RaiSim. You must obtain a valid license from RaiSim Tech
// Inc. prior to usage.
#include "raisim/World.hpp"
#include "raisim/RaisimServer.hpp"
int main(int argc, char **argv) {
auto binaryPath = raisim::Path::setFromArgv(argv[0]);
/// create raisim world
raisim::World world;
world.setTimeStep(0.001);
/// create raisim objects
auto ground = world.addGround();
// simulator
world.setMaterialPairProp("steel", "steel", 0.1, 1.0, 0.0);
auto pin1 = world.addSphere(0.1, 0.8);
pin1->setAppearance("1,0,0,0.3");
pin1->setPosition(0.0, 0.0, 3.0);
pin1->setBodyType(raisim::BodyType::STATIC);
auto pin2 = world.addSphere(0.1, 0.8);
pin2->setAppearance("0,1,0,0.3");
pin2->setPosition(0.3, 0.0, 3.0);
pin2->setBodyType(raisim::BodyType::STATIC);
auto pin3 = world.addSphere(0.1, 0.8);
pin3->setAppearance("0,0,1,0.3");
pin3->setPosition(0.6, 0.0, 3.0);
pin3->setBodyType(raisim::BodyType::STATIC);
auto pin4 = world.addSphere(0.1, 0.8);
pin4->setAppearance("1,0,0,0.3");
pin4->setPosition(0.9, 0.0, 3.0);
pin4->setBodyType(raisim::BodyType::STATIC);
auto pin5 = world.addSphere(0.1, 0.8);
pin5->setPosition(0.9, 0.0, 6.0);
pin5->setBodyType(raisim::BodyType::STATIC);
auto pin6 = world.addSphere(0.1, 0.8);
pin6->setPosition(-3., 0.0, 7.0);
pin6->setBodyType(raisim::BodyType::STATIC);
auto pin7 = world.addSphere(0.1, 0.8);
pin7->setPosition(-4., 0.0, 7.0);
pin7->setBodyType(raisim::BodyType::STATIC);
auto anymalC = world.addArticulatedSystem(binaryPath.getDirectory() + "\\rsc\\anymal_c\\urdf\\anymal.urdf");
auto anymalB = world.addArticulatedSystem(binaryPath.getDirectory() + "\\rsc\\anymal\\urdf\\anymal.urdf");
/// anymalC joint PD controller
Eigen::VectorXd jointNominalConfig(anymalC->getGeneralizedCoordinateDim()), jointVelocityTarget(anymalC->getDOF());
jointNominalConfig << -3, 0, 4.54, 1.0, 0.0, 0.0, 0.0, 0.03, 0.4, -0.8, -0.03, 0.4, -0.8, 0.03, -0.4, 0.8, -0.03, -0.4, 0.8;
jointVelocityTarget.setZero();
Eigen::VectorXd jointPgain(anymalC->getDOF()), jointDgain(anymalC->getDOF());
jointPgain.tail(12).setConstant(100.0);
jointDgain.tail(12).setConstant(1.0);
anymalC->setGeneralizedCoordinate(jointNominalConfig);
anymalC->setGeneralizedForce(Eigen::VectorXd::Zero(anymalC->getDOF()));
anymalC->setPdGains(jointPgain, jointDgain);
anymalC->setPdTarget(jointNominalConfig, jointVelocityTarget);
anymalC->setName("anymalC");
jointNominalConfig[0] = -4;
anymalB->setGeneralizedCoordinate(jointNominalConfig);
anymalB->setGeneralizedForce(Eigen::VectorXd::Zero(anymalB->getDOF()));
anymalB->setPdGains(jointPgain, jointDgain);
anymalB->setPdTarget(jointNominalConfig, jointVelocityTarget);
anymalB->setName("anymalB");
auto ball1 = world.addSphere(0.1498, 0.8, "steel");
ball1->setPosition(0, 0.0, 1.0);
auto ball2 = world.addSphere(0.1499, 0.8, "steel");
ball2->setPosition(0.3, 0.0, 1.0);
auto ball3 = world.addSphere(0.1499, 0.8, "steel");
ball3->setPosition(0.6, 0.0, 1.0);
auto ball4 = world.addSphere(0.1499, 0.8, "steel");
ball4->setPosition(2.9, 0.0, 3.0);
auto box = world.addBox(.1, .1, .1, 1);
box->setPosition(0.9, 0.0, 4.2);
auto wire1 = world.addStiffWire(pin1, 0, {0,0,0}, ball1, 0, {0,0,0}, 2.0);
wire1->setVisualizationWidth(0.05);
wire1->setColor({1, 0, 0, 1});
world.addStiffWire(pin2, 0, {0,0,0}, ball2, 0, {0,0,0}, 2.0);
world.addStiffWire(pin3, 0, {0,0,0}, ball3, 0, {0,0,0}, 2.0);
world.addStiffWire(pin4, 0, {0,0,0}, ball4, 0, {0,0,0}, 2.0);
auto wire5 = world.addCompliantWire(pin5, 0, {0,0,0}, box, 0, {0., 0, 0}, 2.0, 200);
wire5->setStretchType(raisim::LengthConstraint::StretchType::BOTH);
auto wire6 = world.addCompliantWire(pin6, 0, {0,0,0}, anymalC, 0, {0., 0, 0}, 2.0, 1000);
wire6->setStretchType(raisim::LengthConstraint::StretchType::BOTH);
auto wire7 = world.addCustomWire(pin7, 0, {0,0,0}, anymalB, 0, {0., 0, 0}, 2.0);
wire7->setTension(310);
/// launch raisim server
raisim::RaisimServer server(&world);
server.launchServer(8080);
world.exportToXml(binaryPath.getDirectory(), "exportedWorld.xml");
for (int i=0; i< 5000000; i++) {
RS_TIMED_LOOP(int(world.getTimeStep()*1e6))
if (server.isConnected()) {
server.integrateWorldThreadSafe();
}
if (i == 5000)
world.removeObject(wire7);
}
server.killServer();
}