mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 13:20:07 +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