mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 21:30:09 +00:00
Inverted Pendulum Tendon Actutation
This commit is contained in:
Darío Urbina
parent
01d74be777
commit
5081ac5b34
61
data/Base_1.urdf
Normal file
61
data/Base_1.urdf
Normal file
@ -0,0 +1,61 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot name="myfirst">
|
||||||
|
<material name="blue">
|
||||||
|
<color rgba="0 0 0.8 1"/>
|
||||||
|
</material>
|
||||||
|
<material name="white">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
<material name="other">
|
||||||
|
<color rgba="1 0 0.8 1"/>
|
||||||
|
</material>
|
||||||
|
<velocity name="vel">
|
||||||
|
<speed spd="50"/>
|
||||||
|
</velocity>
|
||||||
|
|
||||||
|
|
||||||
|
<link name="Base">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.3 0.3 1.5"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.3 0.3 1.5"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="100"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="pulley1">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".5"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="blue"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".5"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="Base_pulley1" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="Base"/>
|
||||||
|
<child link="pulley1"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="-1.57075 0 0" xyz="0 .35 .15"/>
|
||||||
|
</joint>
|
||||||
|
<!---->
|
||||||
|
</robot>
|
||||||
411
data/Base_2.urdf
Normal file
411
data/Base_2.urdf
Normal file
@ -0,0 +1,411 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot name="myfirst">
|
||||||
|
<material name="blue">
|
||||||
|
<color rgba="0 0 0.8 1"/>
|
||||||
|
</material>
|
||||||
|
<material name="white">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
<material name="other">
|
||||||
|
<color rgba="1 0 0.8 1"/>
|
||||||
|
</material>
|
||||||
|
<velocity name="vel">
|
||||||
|
<speed spd="50"/>
|
||||||
|
</velocity>
|
||||||
|
|
||||||
|
|
||||||
|
<link name="Base">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.3 0.3 1.5"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.3 0.3 1.5"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="100"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="pulley1">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".5"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="blue"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".5"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="Base_pulley1" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="Base"/>
|
||||||
|
<child link="pulley1"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="-1.57075 1.57075 0" xyz="0 .35 .15"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_1">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="blue"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="pulley1_tendon1_1" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="pulley1"/>
|
||||||
|
<child link="tendon1_1"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 3.1416" xyz="0 .55 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_2">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_1_tendon1_2" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_1"/>
|
||||||
|
<child link="tendon1_2"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz=".2 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_3">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_2_tendon1_3" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_2"/>
|
||||||
|
<child link="tendon1_3"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz=".2 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_4">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_3_tendon1_4" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_3"/>
|
||||||
|
<child link="tendon1_4"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 1" xyz=".2 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_5">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_4_tendon1_5" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_4"/>
|
||||||
|
<child link="tendon1_5"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 1" xyz=".2 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_6">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_5_tendon1_6" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_5"/>
|
||||||
|
<child link="tendon1_6"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz=".2 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_7">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_6_tendon1_7" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_6"/>
|
||||||
|
<child link="tendon1_7"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz=".2 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_8">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_7_tendon1_8" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_7"/>
|
||||||
|
<child link="tendon1_8"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz=".2 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_9">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_8_tendon1_9" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_8"/>
|
||||||
|
<child link="tendon1_9"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz=".2 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_10">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_9_tendon1_10" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_9"/>
|
||||||
|
<child link="tendon1_10"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz=".2 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_11">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_10_tendon1_11" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_10"/>
|
||||||
|
<child link="tendon1_11"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz=".2 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_12">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_11_tendon1_12" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_11"/>
|
||||||
|
<child link="tendon1_12"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz=".2 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_13">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_12_tendon1_13" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_12"/>
|
||||||
|
<child link="tendon1_13"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz=".2 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_14">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_13_tendon1_14" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_13"/>
|
||||||
|
<child link="tendon1_14"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz=".2 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
</robot>
|
||||||
481
data/Pendulum_Tendon_1_Cart_Rail.urdf
Normal file
481
data/Pendulum_Tendon_1_Cart_Rail.urdf
Normal file
@ -0,0 +1,481 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot name="myfirst">
|
||||||
|
<material name="blue">
|
||||||
|
<color rgba="0 0 0.8 1"/>
|
||||||
|
</material>
|
||||||
|
<material name="white">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
<material name="other">
|
||||||
|
<color rgba="1 0 0.8 1"/>
|
||||||
|
</material>
|
||||||
|
<velocity name="vel">
|
||||||
|
<speed spd="50"/>
|
||||||
|
</velocity>
|
||||||
|
|
||||||
|
|
||||||
|
<link name="Rail">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="4" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="blue"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="4" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="100"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="slider">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.1 0.6 0.3"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.1 0.6 0.3"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="100"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="rail_slider" type="prismatic">
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<parent link="Rail"/>
|
||||||
|
<child link="slider"/>
|
||||||
|
<limit effort="0" lower="-2" upper="2" velocity="25"/>
|
||||||
|
<origin rpy="-1.570796 0 0" xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="cart">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.1 0.6 0.3"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="blue"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.1 0.6 0.3"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="100"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="slider_cart" type="fixed">
|
||||||
|
<parent link="slider"/>
|
||||||
|
<child link="cart"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0.3"/>
|
||||||
|
</joint>
|
||||||
|
<!--*****************************************-->
|
||||||
|
|
||||||
|
<link name="pendulumAxis">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.1 0.1 .1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.1 0.1 .1"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="cart_pendulumAxis" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="cart"/>
|
||||||
|
<child link="pendulumAxis"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="205"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 .21"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="pendulum">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.1 2 .1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="blue"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.1 2 .1"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="1"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="pendulumAxis_pendulum" type="fixed">
|
||||||
|
<parent link="pendulumAxis"/>
|
||||||
|
<child link="pendulum"/>
|
||||||
|
<origin rpy="0 0 1.570796" xyz="1 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!--*****************************************-->
|
||||||
|
<link name="tendon1_1">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="cart_tendon1_1" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="cart"/>
|
||||||
|
<child link="tendon1_1"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .55 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="tendon1_2">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_1_tendon1_2" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_1"/>
|
||||||
|
<child link="tendon1_2"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .2 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="tendon1_3">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_2_tendon1_3" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_2"/>
|
||||||
|
<child link="tendon1_3"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .2 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="tendon1_4">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_3_tendon1_4" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_3"/>
|
||||||
|
<child link="tendon1_4"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .2 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="tendon1_5">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_4_tendon1_5" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_4"/>
|
||||||
|
<child link="tendon1_5"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .2 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="tendon1_6">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_5_tendon1_6" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_5"/>
|
||||||
|
<child link="tendon1_6"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .2 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="tendon1_7">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_6_tendon1_7" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_6"/>
|
||||||
|
<child link="tendon1_7"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .2 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="tendon1_8">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_7_tendon1_8" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_7"/>
|
||||||
|
<child link="tendon1_8"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .2 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="tendon1_9">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_8_tendon1_9" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_8"/>
|
||||||
|
<child link="tendon1_9"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .2 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="tendon1_10">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_9_tendon1_10" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_9"/>
|
||||||
|
<child link="tendon1_10"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .2 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="tendon1_11">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_10_tendon1_11" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_10"/>
|
||||||
|
<child link="tendon1_11"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .2 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="tendon1_12">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_11_tendon1_12" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_11"/>
|
||||||
|
<child link="tendon1_12"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .2 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="tendon1_13">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="other"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_12_tendon1_13" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_12"/>
|
||||||
|
<child link="tendon1_13"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .2 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="tendon1_14">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length=".1" radius=".05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="tendon1_13_tendon1_14" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<parent link="tendon1_13"/>
|
||||||
|
<child link="tendon1_14"/>
|
||||||
|
<limit effort="0" lower="1" upper="0" velocity="50"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 .2 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!---->
|
||||||
|
</robot>
|
||||||
161
examples/pybullet/examples/inverted_pendulum_tendon_actuation.py
Normal file
161
examples/pybullet/examples/inverted_pendulum_tendon_actuation.py
Normal file
@ -0,0 +1,161 @@
|
|||||||
|
import pybullet as p
|
||||||
|
import time
|
||||||
|
import math as m
|
||||||
|
import numpy as np
|
||||||
|
import pybullet_data
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
|
|
||||||
|
p.connect(p.GUI)
|
||||||
|
plane = p.loadURDF("plane.urdf")
|
||||||
|
|
||||||
|
"""_____________________________________________________________________________________________________________________________"""
|
||||||
|
"""Gains, motor forces, daq and timing parameters"""
|
||||||
|
|
||||||
|
""" Gains for pendulum angle"""
|
||||||
|
proportional_gain = 30000
|
||||||
|
integral_gain = 18000
|
||||||
|
derivative_gain = 22000
|
||||||
|
|
||||||
|
"""Motor force parameters"""
|
||||||
|
tension_force = 600
|
||||||
|
|
||||||
|
"""Control input parameters"""
|
||||||
|
u_factor = 1.5
|
||||||
|
u_lower_limit = tension_force
|
||||||
|
u_upper_limit=9000
|
||||||
|
|
||||||
|
"""Data aquisition, timing and history"""
|
||||||
|
time_steps = 2000
|
||||||
|
history = np.array( [[1000,-1000,0]] )
|
||||||
|
time_history = np.array([[0]])
|
||||||
|
previous_pendulum_angle = 0
|
||||||
|
previous_cart_position = 0
|
||||||
|
|
||||||
|
"""_____________________________________________________________________________________________________________________________"""
|
||||||
|
"""Loading URDF files"""
|
||||||
|
|
||||||
|
cubeStartPos = [-2.15,0,.75]
|
||||||
|
cubeStartPos2 = [0,0,1.4]
|
||||||
|
cubeStartPos3 = [2.15,0,.75]
|
||||||
|
|
||||||
|
PulleyStartOrientation = p.getQuaternionFromEuler([1.570796, 0, 0])
|
||||||
|
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
|
||||||
|
cubeStartOrientation2 = p.getQuaternionFromEuler([0,-1.570796,0])
|
||||||
|
|
||||||
|
base_1 = p.loadURDF("Base_1.urdf",cubeStartPos3, cubeStartOrientation, useFixedBase=1, flags=p.URDF_USE_SELF_COLLISION) #Base 1: magenta base and tendon
|
||||||
|
base_2 = p.loadURDF("Base_2.urdf",cubeStartPos, cubeStartOrientation, useFixedBase=1, flags=p.URDF_USE_SELF_COLLISION) #Base 2: white base and tendon
|
||||||
|
pendulum = p.loadURDF("Pendulum_Tendon_1_Cart_Rail.urdf",cubeStartPos2, cubeStartOrientation2, useFixedBase=1, flags=p.URDF_USE_SELF_COLLISION)
|
||||||
|
|
||||||
|
|
||||||
|
"""_____________________________________________________________________________________________________________________________"""
|
||||||
|
"""Getting access and information from specific joints in each body (each body has links and joint described in the URDF files):"""
|
||||||
|
nJoints = p.getNumJoints(base_1) #Base 1: magenta base and tendon
|
||||||
|
jointNameToId = {}
|
||||||
|
for i in range(nJoints):
|
||||||
|
jointInfo = p.getJointInfo(base_1, i)
|
||||||
|
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
|
||||||
|
Base_pulley_1 = jointNameToId['Base_pulley1']
|
||||||
|
|
||||||
|
nJoints = p.getNumJoints(pendulum)
|
||||||
|
jointNameToId = {}
|
||||||
|
for i in range(nJoints):
|
||||||
|
jointInfo = p.getJointInfo(pendulum, i)
|
||||||
|
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
|
||||||
|
last_tendon_link_1 = jointNameToId['tendon1_13_tendon1_14']
|
||||||
|
cart_pendulumAxis = jointNameToId['cart_pendulumAxis']
|
||||||
|
cart = jointNameToId['slider_cart']
|
||||||
|
|
||||||
|
nJoints = p.getNumJoints(base_2) #Base 2: white base and tendon
|
||||||
|
jointNameToId = {}
|
||||||
|
for i in range(nJoints):
|
||||||
|
jointInfo = p.getJointInfo(base_2, i)
|
||||||
|
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
|
||||||
|
last_tendon_link_2 = jointNameToId['tendon1_13_tendon1_14']
|
||||||
|
Base_pulley_2 = jointNameToId['Base_pulley1']
|
||||||
|
"""_____________________________________________________________________________________________________________________________"""
|
||||||
|
"""Creating new contraints (joints), with the information obtained in the previous step"""
|
||||||
|
|
||||||
|
pulley_1_tendon_magenta = p.createConstraint(base_1, Base_pulley_1, pendulum, last_tendon_link_1, p.JOINT_FIXED, [0, 0, 1], [0, 0, 0], [-.56, 0, 0])
|
||||||
|
tendon_white_cart = p.createConstraint(base_2, last_tendon_link_2, pendulum, cart, p.JOINT_FIXED, [0, 0, 1], [0, 0, 0], [0,-.55, 0])
|
||||||
|
|
||||||
|
"""_____________________________________________________________________________________________________________________________"""
|
||||||
|
"""Defining some motor conditions"""
|
||||||
|
p.setJointMotorControl2(pendulum, cart_pendulumAxis, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
|
||||||
|
p.setJointMotorControl2(base_1, Base_pulley_1, p.VELOCITY_CONTROL, targetVelocity=10, force=1000) #Base 1: magenta base and tendon
|
||||||
|
p.setJointMotorControl2(base_2, Base_pulley_2, p.VELOCITY_CONTROL, targetVelocity=10, force=-1000)#Base 2: white base and tendon
|
||||||
|
|
||||||
|
|
||||||
|
"""_____________________________________________________________________________________________________________________________"""
|
||||||
|
|
||||||
|
|
||||||
|
p.setGravity(0,0,-10)
|
||||||
|
|
||||||
|
|
||||||
|
for i in range (time_steps):
|
||||||
|
p.stepSimulation()
|
||||||
|
pendulum_angle = p.getJointState(pendulum,cart_pendulumAxis)
|
||||||
|
pendulum_angle = pendulum_angle[0]
|
||||||
|
angle_delta_error = -pendulum_angle
|
||||||
|
|
||||||
|
#PROPPORTIONAL
|
||||||
|
p_correction = proportional_gain * pendulum_angle
|
||||||
|
|
||||||
|
#INTEGRAL
|
||||||
|
i_correction = integral_gain * (previous_pendulum_angle + pendulum_angle)
|
||||||
|
previous_pendulum_angle = pendulum_angle
|
||||||
|
|
||||||
|
#DERIVATIVE
|
||||||
|
d_correction = derivative_gain * angle_delta_error
|
||||||
|
|
||||||
|
u = p_correction + i_correction + d_correction + 10
|
||||||
|
|
||||||
|
u = abs(u)
|
||||||
|
if u<u_lower_limit:
|
||||||
|
u=u_lower_limit
|
||||||
|
elif u>u_upper_limit:
|
||||||
|
u=u_upper_limit
|
||||||
|
|
||||||
|
if pendulum_angle > 0:
|
||||||
|
u_pulley_1 = u * u_factor #Base 1: magenta base and tendon
|
||||||
|
u_pulley_2 = -tension_force #Base 2: white base and tendon
|
||||||
|
#print(">0")
|
||||||
|
else:
|
||||||
|
u_pulley_1 = tension_force #Base 1: magenta base and tendon
|
||||||
|
u_pulley_2 = -u * u_factor #Base 2: white base and tendon
|
||||||
|
#print("<0")
|
||||||
|
|
||||||
|
p.setJointMotorControl2(base_1, Base_pulley_1, p.VELOCITY_CONTROL, targetVelocity=100, force = u_pulley_1)
|
||||||
|
p.setJointMotorControl2(base_2, Base_pulley_2, p.VELOCITY_CONTROL, targetVelocity=100, force = u_pulley_2)
|
||||||
|
|
||||||
|
history = np.append(history , [[ u_pulley_1, u_pulley_2, pendulum_angle]] , axis = 0)
|
||||||
|
|
||||||
|
time.sleep(1./240.)
|
||||||
|
|
||||||
|
print("Done with simulation")
|
||||||
|
|
||||||
|
fig, ax1 = plt.subplots()
|
||||||
|
ax1.set_xlabel("Time Steps")
|
||||||
|
ax1.set_ylabel("Activation Values")
|
||||||
|
ax1.plot(history[:,0],label="u_pulley_1")
|
||||||
|
ax1.plot(history[:,1],label="u_pulley_2")
|
||||||
|
ax1.set_ylim((-12000,12000))
|
||||||
|
|
||||||
|
plt.legend(loc='best', bbox_to_anchor=(0.5, 0., 0.5, 0.5),
|
||||||
|
ncol=1, mode=None, borderaxespad=0.)
|
||||||
|
plt.title("Ctrl Input and Angle History")
|
||||||
|
plt.grid(True)
|
||||||
|
color = 'tab:red'
|
||||||
|
ax2 = ax1.twinx()
|
||||||
|
ax2.set_ylabel('Pendulum Angle', color=color)
|
||||||
|
ax2.plot(np.rad2deg(history[:,2]),label="Angle",color=color)
|
||||||
|
ax2.tick_params(axis='y', labelcolor=color)
|
||||||
|
ax2.set_ylim((-90,90))
|
||||||
|
|
||||||
|
fig.tight_layout()
|
||||||
|
plt.show()
|
||||||
|
|
||||||
|
p.disconnect()
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue
Block a user