diff options
| -rw-r--r-- | data/Base_1.urdf | 65 | ||||
| -rw-r--r-- | data/Base_2.urdf | 415 | ||||
| -rw-r--r-- | data/Pendulum_Tendon_1_Cart_Rail.urdf | 485 | ||||
| -rw-r--r-- | examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h | 41 | ||||
| -rw-r--r-- | examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp | 17 | ||||
| -rw-r--r-- | examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp | 98 | ||||
| -rw-r--r-- | examples/TinyRenderer/TinyRenderer.cpp | 8 | ||||
| -rw-r--r-- | examples/TinyRenderer/geometry.h | 2 | ||||
| -rw-r--r-- | examples/TinyRenderer/model.cpp | 8 | ||||
| -rw-r--r-- | examples/TinyRenderer/our_gl.cpp | 47 | ||||
| -rw-r--r-- | examples/TinyRenderer/tgaimage.cpp | 17 | ||||
| -rw-r--r-- | examples/pybullet/examples/inverted_pendulum_tendon_actuation.py | 167 | ||||
| -rw-r--r-- | examples/pybullet/pybullet.c | 35 | ||||
| -rw-r--r-- | setup.py | 2 |
14 files changed, 1348 insertions, 59 deletions
diff --git a/data/Base_1.urdf b/data/Base_1.urdf new file mode 100644 index 000000000..e7b251a1d --- /dev/null +++ b/data/Base_1.urdf @@ -0,0 +1,65 @@ +<?xml version="1.0"?> +<!--https://valerolab.org/--> +<!-- +Code used for PID control of an inverted pendulum actuated by strings. +--> +<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>
\ No newline at end of file diff --git a/data/Base_2.urdf b/data/Base_2.urdf new file mode 100644 index 000000000..44ff0d44d --- /dev/null +++ b/data/Base_2.urdf @@ -0,0 +1,415 @@ +<?xml version="1.0"?> +<!--https://valerolab.org/--> +<!-- +Code used for PID control of an inverted pendulum actuated by strings. +--> +<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>
\ No newline at end of file diff --git a/data/Pendulum_Tendon_1_Cart_Rail.urdf b/data/Pendulum_Tendon_1_Cart_Rail.urdf new file mode 100644 index 000000000..a680987a5 --- /dev/null +++ b/data/Pendulum_Tendon_1_Cart_Rail.urdf @@ -0,0 +1,485 @@ +<?xml version="1.0"?> +<!--https://valerolab.org/--> +<!-- +Code used for PID control of an inverted pendulum actuated by strings. +--> +<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>
\ No newline at end of file diff --git a/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h index 147d8c911..3050cadde 100644 --- a/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h +++ b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h @@ -63,48 +63,35 @@ static bool UrdfFindMeshFile( fn = fn.substr(drop_it_model.length()); std::list<std::string> shorter; - shorter.push_back("../.."); - shorter.push_back(".."); - shorter.push_back("."); + shorter.push_back("../../"); + shorter.push_back("../"); + shorter.push_back("./"); int cnt = urdf_path.size(); for (int i = 0; i < cnt; ++i) { if (urdf_path[i] == '/' || urdf_path[i] == '\\') { - shorter.push_back(urdf_path.substr(0, i)); + shorter.push_back(urdf_path.substr(0, i) + "/"); } } + shorter.push_back(""); // no prefix shorter.reverse(); std::string existing_file; - - + for (std::list<std::string>::iterator x = shorter.begin(); x != shorter.end(); ++x) { - for (std::list<std::string>::iterator x = shorter.begin(); x != shorter.end(); ++x) - { - std::string attempt = *x + "/" + fn; - int f = fileIO->fileOpen(attempt.c_str(), "rb"); - if (f<0) - { - //b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str()); - continue; - } - fileIO->fileClose(f); - existing_file = attempt; - //b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str()); - break; - } - } - if (existing_file.empty()) - { - std::string attempt = fn; + std::string attempt = *x + fn; int f = fileIO->fileOpen(attempt.c_str(), "rb"); - if (f>=0) + if (f<0) { - existing_file = attempt; - fileIO->fileClose(f); + //b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str()); + continue; } + fileIO->fileClose(f); + existing_file = attempt; + //b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str()); + break; } if (existing_file.empty()) diff --git a/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp b/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp index 224afaf9a..cb6bacd33 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp @@ -93,6 +93,7 @@ bool btExtractJointBodyFromTempLinks(btAlignedObjectArray<TempLink>& links, Eige collisionShape = compound->getChildShape(0); } } + switch (collisionShape->getShapeType()) { case BOX_SHAPE_PROXYTYPE: @@ -102,6 +103,7 @@ bool btExtractJointBodyFromTempLinks(btAlignedObjectArray<TempLink>& links, Eige param0 = box->getHalfExtentsWithMargin()[0] * 2; param1 = box->getHalfExtentsWithMargin()[1] * 2; param2 = box->getHalfExtentsWithMargin()[2] * 2; + break; } case SPHERE_SHAPE_PROXYTYPE: @@ -124,7 +126,20 @@ bool btExtractJointBodyFromTempLinks(btAlignedObjectArray<TempLink>& links, Eige } default: { - btAssert(0); + //approximate by its box + btTransform identity; + identity.setIdentity(); + btVector3 aabbMin, aabbMax; + collisionShape->getAabb(identity, aabbMin, aabbMax); + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); + btScalar margin = collisionShape->getMargin(); + btScalar lx = btScalar(2.) * (halfExtents.x() + margin); + btScalar ly = btScalar(2.) * (halfExtents.y() + margin); + btScalar lz = btScalar(2.) * (halfExtents.z() + margin); + param0 = lx; + param1 = ly; + param2 = lz; + shapeType = cShape::eShapeBox; } } } diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 6ffb5d2c1..063959dfe 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -66,6 +66,8 @@ struct TinyRendererVisualShapeConverterInternalData // Maps bodyUniqueId to a list of visual shapes belonging to the body. btHashMap<btHashInt, btAlignedObjectArray<b3VisualShapeData> > m_visualShapesMap; + btAlignedObjectArray<unsigned char> m_checkeredTexels; + int m_uidGenerator; int m_upAxis; int m_swWidth; @@ -560,10 +562,61 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa } // case mesh case URDF_GEOM_PLANE: - // TODO: plane in tiny renderer - // TODO: export visualShapeOut for external render - break; + { + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray<int>(); + glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); + glmesh->m_indices->push_back(0); + glmesh->m_indices->push_back(1); + glmesh->m_indices->push_back(2); + glmesh->m_indices->push_back(0); + glmesh->m_indices->push_back(2); + glmesh->m_indices->push_back(3); + glmesh->m_scaling[0] = 1; + glmesh->m_scaling[1] = 1; + glmesh->m_scaling[2] = 1; + glmesh->m_scaling[3] = 1; + + btScalar planeConst = 0; + btVector3 planeNormal = visual->m_geometry.m_planeNormal; + btVector3 planeOrigin = planeNormal * planeConst; + btVector3 vec0, vec1; + btPlaneSpace1(planeNormal, vec0, vec1); + + btScalar vecLen = 128; + btVector3 verts[4]; + + verts[0] = planeOrigin + vec0 * vecLen + vec1 * vecLen; + verts[1] = planeOrigin - vec0 * vecLen + vec1 * vecLen; + verts[2] = planeOrigin - vec0 * vecLen - vec1 * vecLen; + verts[3] = planeOrigin + vec0 * vecLen - vec1 * vecLen; + + GLInstanceVertex vtx; + vtx.xyzw[0] = verts[0][0]; vtx.xyzw[1] = verts[0][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; + vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; + vtx.uv[0] = vecLen; vtx.uv[1] = vecLen; + glmesh->m_vertices->push_back(vtx); + + vtx.xyzw[0] = verts[1][0]; vtx.xyzw[1] = verts[1][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; + vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; + vtx.uv[0] = 0; vtx.uv[1] = vecLen; + glmesh->m_vertices->push_back(vtx); + + vtx.xyzw[0] = verts[2][0]; vtx.xyzw[1] = verts[2][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; + vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; + vtx.uv[0] = 0; vtx.uv[1] = 0; + glmesh->m_vertices->push_back(vtx); + + vtx.xyzw[0] = verts[3][0]; vtx.xyzw[1] = verts[3][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; + vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; + vtx.uv[0] = vecLen; vtx.uv[1] = 0; + glmesh->m_vertices->push_back(vtx); + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + break; + } default: { b3Warning("TinyRenderer: unknown visual geometry type %i\n", visual->m_geometry.m_type); @@ -783,6 +836,45 @@ int TinyRendererVisualShapeConverter::convertVisualShapes( { B3_PROFILE("convertURDFToVisualShape"); convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO, m_data->m_flags); + if ((vis->m_geometry.m_type == URDF_GEOM_PLANE) || (vis->m_geometry.m_type == URDF_GEOM_SPHERE)) + { + int texWidth = 1024; + int texHeight = 1024; + if (m_data->m_checkeredTexels.size() == 0) + { + + int red = 173; + int green = 199; + int blue = 255; + //create a textured surface + + m_data->m_checkeredTexels.resize(texWidth * texHeight * 3); + for (int i = 0; i < texWidth * texHeight * 3; i++) + m_data->m_checkeredTexels[i] = 255; + + for (int i = 0; i < texWidth; i++) + { + for (int j = 0; j < texHeight; j++) + { + int a = i < texWidth / 2 ? 1 : 0; + int b = j < texWidth / 2 ? 1 : 0; + + if (a == b) + { + m_data->m_checkeredTexels[(i + j * texWidth) * 3 + 0] = red; + m_data->m_checkeredTexels[(i + j * texWidth) * 3 + 1] = green; + m_data->m_checkeredTexels[(i + j * texWidth) * 3 + 2] = blue; + } + } + } + } + MyTexture2 texData; + texData.m_width = texWidth; + texData.m_height = texHeight; + texData.textureData1 = &m_data->m_checkeredTexels[0]; + texData.m_isCached = true; + textures.push_back(texData); + } } rgbaColor[0] = visualShape.m_rgbaColor[0]; diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 0e383a54b..6412888a4 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -167,11 +167,11 @@ struct Shader : public IShader Vec2f uv = varying_uv * bar; Vec3f reflection_direction = (bn * (bn * m_light_dir_local * 2.f) - m_light_dir_local).normalize(); - float specular = std::pow(b3Max(reflection_direction.z, 0.f), - m_model->specular(uv)); - float diffuse = b3Max(0.f, bn * m_light_dir_local); + float specular = std::pow(b3Max(reflection_direction.z, 0.f), + m_model->specular(uv)); + float diffuse = b3Max(0.f, bn * m_light_dir_local); - color = m_model->diffuse(uv); + color = m_model->diffuse(uv); color[0] *= m_colorRGBA[0]; color[1] *= m_colorRGBA[1]; color[2] *= m_colorRGBA[2]; diff --git a/examples/TinyRenderer/geometry.h b/examples/TinyRenderer/geometry.h index f2100e57e..c17518c9f 100644 --- a/examples/TinyRenderer/geometry.h +++ b/examples/TinyRenderer/geometry.h @@ -315,8 +315,10 @@ template <size_t DimRows,size_t DimCols,class T> std::ostream& operator<<(std::o ///////////////////////////////////////////////////////////////////////////////// typedef vec<2, float> Vec2f; +typedef vec<2, double> Vec2d; typedef vec<2, int> Vec2i; typedef vec<3, float> Vec3f; +typedef vec<3, double> Vec3d; typedef vec<3, int> Vec3i; typedef vec<4, float> Vec4f; typedef mat<4, 4, float> Matrix; diff --git a/examples/TinyRenderer/model.cpp b/examples/TinyRenderer/model.cpp index 29f13dd4d..6bbaa5167 100644 --- a/examples/TinyRenderer/model.cpp +++ b/examples/TinyRenderer/model.cpp @@ -166,10 +166,10 @@ TGAColor Model::diffuse(Vec2f uvf) // bool repeat = true; // if (repeat) { - uvf[0] = std::modf(uvf[0], &val); - uvf[1] = std::modf(uvf[1], &val); - } - Vec2i uv(uvf[0] * diffusemap_.get_width(), uvf[1] * diffusemap_.get_height()); + uvf[0] = std::modf(uvf[0], &val); + uvf[1] = std::modf(uvf[1], &val); + } + Vec2i uv(uvf[0] * diffusemap_.get_width(), uvf[1] * diffusemap_.get_height()); return diffusemap_.get(uv[0], uv[1]); } return TGAColor(255, 255, 255, 255); diff --git a/examples/TinyRenderer/our_gl.cpp b/examples/TinyRenderer/our_gl.cpp index b71c09e75..30d2024e8 100644 --- a/examples/TinyRenderer/our_gl.cpp +++ b/examples/TinyRenderer/our_gl.cpp @@ -61,19 +61,25 @@ Matrix lookat(Vec3f eye, Vec3f center, Vec3f up) return ModelView; } -Vec3f barycentric(Vec2f A, Vec2f B, Vec2f C, Vec2f P) +Vec3d barycentric(Vec2f A1, Vec2f B1, Vec2f C1, Vec2f P1) { - Vec3f s[2]; + + Vec2d A(A1.x, A1.y); + Vec2d B(B1.x, B1.y); + Vec2d C(C1.x, C1.y); + Vec2d P(P1.x, P1.y);; + + Vec3d s[2]; for (int i = 2; i--;) { s[i][0] = C[i] - A[i]; s[i][1] = B[i] - A[i]; s[i][2] = A[i] - P[i]; } - Vec3f u = cross(s[0], s[1]); + Vec3d u = cross(s[0], s[1]); if (std::abs(u[2]) > 1e-2) // dont forget that u[2] is integer. If it is zero then triangle ABC is degenerate - return Vec3f(1.f - (u.x + u.y) / u.z, u.y / u.z, u.x / u.z); - return Vec3f(-1, 1, 1); // in this case generate negative coordinates, it will be thrown away by the rasterizator + return Vec3d(1. - (u.x + u.y) / u.z, u.y / u.z, u.x / u.z); + return Vec3d(-1., 1., 1.); // in this case generate negative coordinates, it will be thrown away by the rasterizator } void triangleClipped(mat<4, 3, float> &clipc, mat<4, 3, float> &orgClipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix &viewPortMatrix) @@ -119,25 +125,28 @@ void triangleClipped(mat<4, 3, float> &clipc, mat<4, 3, float> &orgClipc, IShade { for (P.y = bboxmin.y; P.y <= bboxmax.y; P.y++) { - float frag_depth = 0; + double frag_depth = 0; { - Vec3f bc_screen = barycentric(pts2[0], pts2[1], pts2[2], P); - Vec3f bc_clip = Vec3f(bc_screen.x / screenSpacePts[0][3], bc_screen.y / screenSpacePts[1][3], bc_screen.z / screenSpacePts[2][3]); + Vec3d bc_screen = barycentric(pts2[0], pts2[1], pts2[2], P); + Vec3d bc_clip = Vec3d(bc_screen.x / screenSpacePts[0][3], bc_screen.y / screenSpacePts[1][3], bc_screen.z / screenSpacePts[2][3]); bc_clip = bc_clip / (bc_clip.x + bc_clip.y + bc_clip.z); - frag_depth = -1 * (clipc[2] * bc_clip); + Vec3d clipd(clipc[2].x, clipc[2].y, clipc[2].z); + frag_depth = -1. * (clipd * bc_clip); if (bc_screen.x < 0 || bc_screen.y < 0 || bc_screen.z < 0 || zbuffer[P.x + P.y * image.get_width()] > frag_depth) continue; } - Vec3f bc_screen2 = barycentric(orgPts2[0], orgPts2[1], orgPts2[2], P); - Vec3f bc_clip2 = Vec3f(bc_screen2.x / orgScreenSpacePts[0][3], bc_screen2.y / orgScreenSpacePts[1][3], bc_screen2.z / orgScreenSpacePts[2][3]); + Vec3d bc_screen2 = barycentric(orgPts2[0], orgPts2[1], orgPts2[2], P); + Vec3d bc_clip2 = Vec3d(bc_screen2.x / orgScreenSpacePts[0][3], bc_screen2.y / orgScreenSpacePts[1][3], bc_screen2.z / orgScreenSpacePts[2][3]); bc_clip2 = bc_clip2 / (bc_clip2.x + bc_clip2.y + bc_clip2.z); - float frag_depth2 = -1 * (orgClipc[2] * bc_clip2); - - bool discard = shader.fragment(bc_clip2, color); + Vec3d orgClipd(orgClipc[2].x, orgClipc[2].y, orgClipc[2].z); + double frag_depth2 = -1. * (orgClipd * bc_clip2); + Vec3f bc_clip2f(bc_clip2.x, bc_clip2.y, bc_clip2.z); + bool discard = shader.fragment(bc_clip2f, color); + if (!discard) { zbuffer[P.x + P.y * image.get_width()] = frag_depth; @@ -182,14 +191,16 @@ void triangle(mat<4, 3, float> &clipc, IShader &shader, TGAImage &image, float * { for (P.y = bboxmin.y; P.y <= bboxmax.y; P.y++) { - Vec3f bc_screen = barycentric(pts2[0], pts2[1], pts2[2], P); - Vec3f bc_clip = Vec3f(bc_screen.x / pts[0][3], bc_screen.y / pts[1][3], bc_screen.z / pts[2][3]); + Vec3d bc_screen = barycentric(pts2[0], pts2[1], pts2[2], P); + Vec3d bc_clip = Vec3d(bc_screen.x / pts[0][3], bc_screen.y / pts[1][3], bc_screen.z / pts[2][3]); bc_clip = bc_clip / (bc_clip.x + bc_clip.y + bc_clip.z); - float frag_depth = -1 * (clipc[2] * bc_clip); + Vec3d clipd(clipc[2].x, clipc[2].y, clipc[2].z); + double frag_depth = -1. * (clipd * bc_clip); if (bc_screen.x < 0 || bc_screen.y < 0 || bc_screen.z < 0 || zbuffer[P.x + P.y * image.get_width()] > frag_depth) continue; - bool discard = shader.fragment(bc_clip, color); + Vec3f bc_clipf(bc_clip.x, bc_clip.y, bc_clip.z); + bool discard = shader.fragment(bc_clipf, color); if (frag_depth < -shader.m_farPlane) discard = true; if (frag_depth > shader.m_nearPlane) diff --git a/examples/TinyRenderer/tgaimage.cpp b/examples/TinyRenderer/tgaimage.cpp index 706baefe3..68e91054b 100644 --- a/examples/TinyRenderer/tgaimage.cpp +++ b/examples/TinyRenderer/tgaimage.cpp @@ -296,6 +296,23 @@ bool TGAImage::unload_rle_data(std::ofstream &out) const TGAColor TGAImage::get(int x, int y) const { + if (x < 0) + { + x = 0; + } + if (y < 0) + { + y = 0; + } + if (x >= width) + { + x = width - 1; + } + if (y >= height) + { + y = height - 1; + } + if (!data || x < 0 || y < 0 || x >= width || y >= height) { return TGAColor(128.f, 128.f, 128.f, 255.f); diff --git a/examples/pybullet/examples/inverted_pendulum_tendon_actuation.py b/examples/pybullet/examples/inverted_pendulum_tendon_actuation.py new file mode 100644 index 000000000..d9b075931 --- /dev/null +++ b/examples/pybullet/examples/inverted_pendulum_tendon_actuation.py @@ -0,0 +1,167 @@ +""" +https://valerolab.org/ + +PID control of an inverted pendulum actuated by strings. +""" + +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() + + + diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f22366b65..f6794482a 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6667,11 +6667,44 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* return NULL; } + if (!rayFromObjList || !rayToObjList) + { + PyErr_SetString(SpamError, "rayFromPositions and rayToPositions must be not None."); + return NULL; + } + commandHandle = b3CreateRaycastBatchCommandInit(sm); b3RaycastBatchSetNumThreads(commandHandle, numThreads); - if (rayFromObjList) + + int raysAdded = 0; +#ifdef PYBULLET_USE_NUMPY + // Faster approach if both inputs can be converted into ndarray. + if (PyArray_Check(rayFromObjList) && PyArray_Check(rayToObjList)) { + b3PushProfileTiming(sm, "extractPythonFromToNumpy"); + PyArrayObject* rayFromPyArrayObj = (PyArrayObject*)PyArray_FROMANY(rayFromObjList, NPY_DOUBLE, 1, 2, NPY_ARRAY_CARRAY_RO); + PyArrayObject* rayToPyArrayObj = (PyArrayObject*)PyArray_FROMANY(rayToObjList, NPY_DOUBLE, 1, 2, NPY_ARRAY_CARRAY_RO); + + // If there is error, this will fall back to default method and error messages will be reported there. + if (rayFromPyArrayObj && rayToPyArrayObj + && PyArray_SAMESHAPE(rayFromPyArrayObj, rayToPyArrayObj) + && PyArray_DIMS(rayFromPyArrayObj)[PyArray_NDIM(rayFromPyArrayObj) - 1] == 3) + { + int len = (PyArray_NDIM(rayFromPyArrayObj) == 2) ? PyArray_DIMS(rayFromPyArrayObj)[0] : 1; + if (len <= MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING) + { + b3RaycastBatchAddRays(sm, commandHandle, PyArray_DATA(rayFromPyArrayObj), PyArray_DATA(rayToPyArrayObj), len); + raysAdded = 1; + } + } + if (rayFromPyArrayObj) Py_DECREF(rayFromPyArrayObj); + if (rayToPyArrayObj) Py_DECREF(rayToPyArrayObj); + b3PopProfileTiming(sm); + } +#endif + if (!raysAdded) { + // go back to default method. PyObject* seqRayFromObj = PySequence_Fast(rayFromObjList, "expected a sequence of rayFrom positions"); PyObject* seqRayToObj = PySequence_Fast(rayToObjList, "expected a sequence of 'rayTo' positions"); @@ -501,7 +501,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name='pybullet', - version='2.8.5', + version='2.8.6', description= 'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description= |