-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtwolinkman.xml
49 lines (45 loc) · 2.16 KB
/
twolinkman.xml
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
<mujoco model="twolinkman">
<compiler coordinate="global" inertiafromgeom="true" angle="degree"/>
<default>
<joint limited="true" damping="1" armature="0"/>
<motor ctrlrange="-.4 .4" ctrllimited="true"/>
</default>
<option timestep="0.002" iterations="50" solver="PGS">
<flag energy="enable"/>
</option>
<worldbody>
<light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
<geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
<!--first manipulator-->
<body>
<joint name="joint1" type="hinge" pos="0 0 0" axis="0 1 0" range="-90 90" damping="5" stiffness="20" armature="0.02"/>
<geom name="link1" type="capsule" fromto="0 0 0 0 0 0.6" size=".05" rgba=".1 .9 .1 1"/>
<body>
<joint name="joint2" type="hinge" pos="0 0 0.65" axis="0 1 0" range="-90 90" damping="5" stiffness="20" armature="0.02"/>
<geom name="link2" type="capsule" fromto="0 0 0.65 0.0 0 1.2" size=".05" rgba=".9 .8 .1 1"/>
</body>
<body>
<joint name="joint3" type="hinge" pos="0 0 0.65" axis="0 1 0" range="-90 90" damping="5" stiffness="20" armature="0.02"/>
<geom name="link3" type="capsule" fromto="0 0 0.65 0.0 0 1.2" size=".05" rgba=".9 .8 .1 1"/>
</body>
</body>
<!--second manipulator-->
<body>
<joint name="joint1" type="hinge" pos="0 0 0" axis="0 1 0" range="-90 90" damping="5" stiffness="20" armature="0.02"/>
<geom name="link1" type="capsule" fromto="0 0 0 0 0 0.6" size=".05" rgba=".1 .9 .1 1"/>
<body>
<joint name="joint2" type="hinge" pos="0 0 0.65" axis="0 1 0" range="-90 90" damping="5" stiffness="20" armature="0.02"/>
<geom name="link2" type="capsule" fromto="0 0 0.65 0.0 0 1.2" size=".05" rgba=".9 .8 .1 1"/>
</body>
</body>
</worldbody>
<actuator>
<!-- ================= Torque actuators ================= -->
<motor name="joint1" gear="200" joint="joint1"/>
<motor name="joint2" gear="200" joint="joint2"/>
<!-- ================= Position actuators ================= -->
<!-- <position joint="joint1" name="Joint1" kp="100" ctrlrange="-1.57 1.57"/>
<position joint="joint2" name="Joint2" kp="100" ctrlrange="-1.57 1.57"/>
-->
</actuator>
</mujoco>