diff options
30 files changed, 1492 insertions, 115 deletions
diff --git a/build3/findOpenGLGlewGlut.lua b/build3/findOpenGLGlewGlut.lua index 5d53a25d9..0a3eb4e59 100644 --- a/build3/findOpenGLGlewGlut.lua +++ b/build3/findOpenGLGlewGlut.lua @@ -43,6 +43,37 @@ configuration{} end + function initX11() + if os.is("Linux") then + if _OPTIONS["enable_system_x11"] and (os.isdir("/usr/include") and os.isfile("/usr/include/X11/X.h")) then + links{"X11","pthread"} + else + print("No X11/X.h found, using dynamic loading of X11") + includedirs { + projectRootDir .. "examples/ThirdPartyLibs/optionalX11" + } + defines {"DYNAMIC_LOAD_X11_FUNCTIONS"} + links {"dl","pthread"} + end + end + end + + + function initX11() + if os.is("Linux") then + if _OPTIONS["enable_system_x11"] and (os.isdir("/usr/include") and os.isfile("/usr/include/X11/X.h")) then + links{"X11","pthread"} + else + print("No X11/X.h found, using dynamic loading of X11") + includedirs { + projectRootDir .. "examples/ThirdPartyLibs/optionalX11" + } + defines {"DYNAMIC_LOAD_X11_FUNCTIONS"} + links {"dl","pthread"} + end + end + end + function initGlew() configuration {} @@ -63,8 +94,9 @@ if os.is("Linux") then configuration{"Linux"} + initX11() if _OPTIONS["enable_system_glx"] then --# and (os.isdir("/usr/include") and os.isfile("/usr/include/GL/glx.h")) then - links{"X11","pthread"} + links{"pthread"} print("Using system GL/glx.h") else print("Using glad_glx") @@ -86,18 +118,4 @@ configuration{} end - function initX11() - if os.is("Linux") then - if _OPTIONS["enable_system_x11"] and (os.isdir("/usr/include") and os.isfile("/usr/include/X11/X.h")) then - links{"X11","pthread"} - else - print("No X11/X.h found, using dynamic loading of X11") - includedirs { - projectRootDir .. "examples/ThirdPartyLibs/optionalX11" - } - defines {"DYNAMIC_LOAD_X11_FUNCTIONS"} - links {"dl","pthread"} - end - end - end 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/data/torus_deform.urdf b/data/torus_deform.urdf index 20fa3e1a3..a3b49dc94 100644 --- a/data/torus_deform.urdf +++ b/data/torus_deform.urdf @@ -2,13 +2,13 @@ <robot name="torus"> <deformable name="torus"> <inertial> - <mass value="1" /> + <mass value="3" /> <inertia ixx="0.0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" /> </inertial> <collision_margin value="0.006"/> <repulsion_stiffness value="800.0"/> <friction value= "0.5"/> - <neohookean mu= "60" lambda= "200" damping= "0.01" /> + <neohookean mu= "180" lambda= "600" damping= "0.01" /> <visual filename="torus.vtk"/> </deformable> </robot> 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/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 17f164775..3fb7052ae 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2038,6 +2038,12 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() if (m_data->m_threadPool) delete m_data->m_threadPool; + for (int i = 0; i < m_data->m_savedStates.size(); i++) + { + delete m_data->m_savedStates[i].m_bulletFile; + delete m_data->m_savedStates[i].m_serializer; + } + delete m_data; } @@ -2515,6 +2521,17 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface } } } + + //delete textures + for (int i = 0; i < textures.size(); i++) + { + B3_PROFILE("free textureData"); + if (!textures[i].m_isCached) + { + free(textures[i].textureData1); + } + } + return graphicsIndex; } @@ -3236,15 +3253,15 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, #ifdef B3_ENABLE_TINY_AUDIO { SDFAudioSource audioSource; - int urdfLinkIndex = creation.m_mb2urdfLink[link]; + int urdfLinkIndex = creation.m_mb2urdfLink[i]; if (u2b.getLinkAudioSource(urdfLinkIndex, audioSource)) { - int flags = mb->getLink(link).m_collider->getCollisionFlags(); + int flags = mb->getLink(i).m_collider->getCollisionFlags(); mb->getLink(i).m_collider->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER); audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str()); if (audioSource.m_userIndex >= 0) { - bodyHandle->m_audioSources.insert(link, audioSource); + bodyHandle->m_audioSources.insert(i, audioSource); } } } @@ -5989,12 +6006,9 @@ struct FilteredClosestRayResultCallback : public btCollisionWorld::ClosestRayRes virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) { - if (m_collisionFilterMask >= 0) - { - bool collides = (rayResult.m_collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & m_collisionFilterMask) != 0; - if (!collides) - return m_closestHitFraction; - } + bool collides = (rayResult.m_collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & m_collisionFilterMask) != 0; + if (!collides) + return m_closestHitFraction; return btCollisionWorld::ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); } }; @@ -6013,12 +6027,9 @@ struct FilteredAllHitsRayResultCallback : public btCollisionWorld::AllHitsRayRes virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) { - if (m_collisionFilterMask >= 0) - { - bool collides = (rayResult.m_collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & m_collisionFilterMask) != 0; - if (!collides) - return m_closestHitFraction; - } + bool collides = (rayResult.m_collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & m_collisionFilterMask) != 0; + if (!collides) + return m_closestHitFraction; //remove duplicate hits: //same collision object, link index and hit fraction bool isDuplicate = false; diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 7398d930b..f2777e129 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -2309,6 +2309,29 @@ int b3RobotSimulatorClientAPI_NoDirect::createCollisionShape(int shapeType, stru scalarToDouble3(args.m_meshScale, meshScale); shapeIndex = b3CreateCollisionShapeAddMesh(command, args.m_fileName, meshScale); } + if (shapeType == GEOM_HEIGHTFIELD) + { + double meshScale[3]; + scalarToDouble3(args.m_meshScale, meshScale); + if (args.m_fileName) + { + shapeIndex = b3CreateCollisionShapeAddHeightfield(command, args.m_fileName, meshScale, args.m_heightfieldTextureScaling); + } + else + { + if (args.m_heightfieldData.size() && args.m_numHeightfieldRows>0 && args.m_numHeightfieldColumns>0) + { + shapeIndex = b3CreateCollisionShapeAddHeightfield2(sm, command, meshScale, args.m_heightfieldTextureScaling, + &args.m_heightfieldData[0], + args.m_numHeightfieldRows, + args.m_numHeightfieldColumns, + args.m_replaceHeightfieldIndex); + } + } + + + } + if (shapeType == GEOM_PLANE) { double planeConstant = 0; @@ -2546,6 +2569,27 @@ void b3RobotSimulatorClientAPI_NoDirect::restoreStateFromMemory(int stateId) statusType = b3GetStatusType(statusHandle); } +void b3RobotSimulatorClientAPI_NoDirect::removeState(int stateUniqueId) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) + { + b3Warning("Not connected"); + return; + } + + if (stateUniqueId >= 0) + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + if (b3CanSubmitCommand(sm)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitRemoveStateCommand(sm, stateUniqueId)); + statusType = b3GetStatusType(statusHandle); + } + } +} + bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation& visualShapeInfo) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index 1c92746b4..5c8f9e2ff 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -471,12 +471,23 @@ struct b3RobotSimulatorCreateCollisionShapeArgs btVector3 m_meshScale; btVector3 m_planeNormal; int m_flags; + + double m_heightfieldTextureScaling; + btAlignedObjectArray<float> m_heightfieldData; + int m_numHeightfieldRows; + int m_numHeightfieldColumns; + int m_replaceHeightfieldIndex; + b3RobotSimulatorCreateCollisionShapeArgs() : m_shapeType(-1), m_radius(0.5), m_height(1), m_fileName(NULL), - m_flags(0) + m_flags(0), + m_heightfieldTextureScaling(1), + m_numHeightfieldRows(0), + m_numHeightfieldColumns(0), + m_replaceHeightfieldIndex(-1) { m_halfExtents.m_floats[0] = 1; m_halfExtents.m_floats[1] = 1; @@ -889,6 +900,7 @@ public: int saveStateToMemory(); void restoreStateFromMemory(int stateId); + void removeState(int stateUniqueId); int getAPIVersion() const { 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..937ac28fb 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) + { + 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/TinyAudio/b3AudioListener.cpp b/examples/TinyAudio/b3AudioListener.cpp index 184b169ec..4e2022ffa 100644 --- a/examples/TinyAudio/b3AudioListener.cpp +++ b/examples/TinyAudio/b3AudioListener.cpp @@ -2,6 +2,7 @@ #include "b3SoundSource.h" #include "Bullet3Common/b3Logging.h" #include "b3WriteWavFile.h" +#include <math.h> template <class T> inline const T& MyMin(const T& a, const T& b) diff --git a/examples/TinyAudio/b3SoundEngine.cpp b/examples/TinyAudio/b3SoundEngine.cpp index e683bf19a..6e36b0d9c 100644 --- a/examples/TinyAudio/b3SoundEngine.cpp +++ b/examples/TinyAudio/b3SoundEngine.cpp @@ -164,7 +164,7 @@ int b3SoundEngine::loadWavFile(const char* fileName) } char resourcePath[1024]; - if (b3ResourcePath::findResourcePath(fileName, resourcePath, 1024)) + if (b3ResourcePath::findResourcePath(fileName, resourcePath, 1024, 0)) { b3ReadWavFile* wavFile = new b3ReadWavFile(); wavFile->getWavInfo(resourcePath); 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/Utils/ChromeTraceUtil.cpp b/examples/Utils/ChromeTraceUtil.cpp index a7eca7604..68a84effb 100644 --- a/examples/Utils/ChromeTraceUtil.cpp +++ b/examples/Utils/ChromeTraceUtil.cpp @@ -5,6 +5,7 @@ #include "LinearMath/btAlignedObjectArray.h" #include "Bullet3Common/b3Logging.h" #include <stdio.h> +#include <climits> struct btTiming { @@ -111,16 +112,15 @@ struct btTimings sprintf(newname, "%s%d", name, counter2++); #ifdef _WIN32 - fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%I64d.%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n", threadId, startTimeDiv1000, startTimeRem1000Str, newname); fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%I64d.%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}", threadId, endTimeDiv1000, endTimeRem1000Str, newname); - #else - fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n", + // Note: on 64b build, PRIu64 resolves in 'lu' whereas timings ('ts') have to be printed as 'llu'. + fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%llu.%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n", threadId, startTimeDiv1000, startTimeRem1000Str, newname); - fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}", + fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%llu.%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}", threadId, endTimeDiv1000, endTimeRem1000Str, newname); #endif #endif diff --git a/examples/pybullet/examples/constraint.py b/examples/pybullet/examples/constraint.py index 70d4a6fe9..60b883a5c 100644 --- a/examples/pybullet/examples/constraint.py +++ b/examples/pybullet/examples/constraint.py @@ -13,7 +13,6 @@ p.setRealTimeSimulation(1) cid = p.createConstraint(cubeId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 1]) print(cid) print(p.getConstraintUniqueId(0)) -prev = [0, 0, 1] a = -math.pi while 1: a = a + 0.01 diff --git a/examples/pybullet/examples/deformable_ball.py b/examples/pybullet/examples/deformable_ball.py index ffda4e6b7..68dd39aaa 100644 --- a/examples/pybullet/examples/deformable_ball.py +++ b/examples/pybullet/examples/deformable_ball.py @@ -14,7 +14,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn) boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) -ballId = p.loadSoftBody("ball.vtk", basePosition = [0,0,-1], scale = 0.5, mass = 0.1, useNeoHookean = 1, NeoHookeanMu = 20, NeoHookeanLambda = 20, NeoHookeanDamping = 0.001, useSelfCollision = 1, frictionCoeff = .5) +ballId = p.loadSoftBody("ball.vtk", basePosition = [0,0,-1], scale = 0.5, mass = 4, useNeoHookean = 1, NeoHookeanMu = 400, NeoHookeanLambda = 600, NeoHookeanDamping = 0.001, useSelfCollision = 1, frictionCoeff = .5, collisionMargin = 0.001) p.setTimeStep(0.001) p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25) p.setRealTimeSimulation(1) diff --git a/examples/pybullet/examples/deformable_torus.py b/examples/pybullet/examples/deformable_torus.py index 0408e8e90..fe64ae648 100644 --- a/examples/pybullet/examples/deformable_torus.py +++ b/examples/pybullet/examples/deformable_torus.py @@ -14,7 +14,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2]) boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) -bunnyId = p.loadSoftBody("torus.vtk", mass = 1, useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 800) +bunnyId = p.loadSoftBody("torus.vtk", mass = 3, useNeoHookean = 1, NeoHookeanMu = 180, NeoHookeanLambda = 600, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 800) bunny2 = p.loadURDF("torus_deform.urdf", [0,1,0], flags=p.URDF_USE_SELF_COLLISION) 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/examples/testrender.py b/examples/pybullet/examples/testrender.py index cbcda6e5b..41ced4716 100644 --- a/examples/pybullet/examples/testrender.py +++ b/examples/pybullet/examples/testrender.py @@ -15,7 +15,7 @@ import pybullet_data pybullet.connect(pybullet.DIRECT) -p.setAdditionalSearchPath(pybullet_data.getDataPath()) +pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) #pybullet.loadPlugin("eglRendererPlugin") pybullet.loadURDF("plane.urdf", [0, 0, -1]) diff --git a/examples/pybullet/examples/video_sync_mp4.py b/examples/pybullet/examples/video_sync_mp4.py index 3859de538..754101744 100644 --- a/examples/pybullet/examples/video_sync_mp4.py +++ b/examples/pybullet/examples/video_sync_mp4.py @@ -2,10 +2,15 @@ import pybullet as p import time import pybullet_data +#Once the video is recorded, you can extract all individual frames using ffmpeg +#mkdir frames +#ffmpeg -i test.mp4 "frames/out-%03d.png" + #by default, PyBullet runs at 240Hz -p.connect(p.GUI, options="--mp4=\"test.mp4\" --mp4fps=240") +p.connect(p.GUI, options="--width=1920 --height=1080 --mp4=\"test.mp4\" --mp4fps=240") p.setAdditionalSearchPath(pybullet_data.getDataPath()) +p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1) p.loadURDF("plane.urdf") diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py index 30d607734..fcbf2154b 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py @@ -21,7 +21,7 @@ jointFrictionForce = 0 class HumanoidStablePD(object): def __init__( self, pybullet_client, mocap_data, timeStep, - useFixedBase=True, arg_parser=None, useComReward=True): + useFixedBase=True, arg_parser=None, useComReward=False): self._pybullet_client = pybullet_client self._mocap_data = mocap_data self._arg_parser = arg_parser diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py index 500bbdc71..b6f1c5a3d 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py @@ -19,7 +19,7 @@ import datetime import os import time -import tf.compat.v1 as tf +import tensorflow.compat.v1 as tf from pybullet_envs.minitaur.envs import minitaur_logging_pb2 NUM_MOTORS = 8 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index a349bdc59..d9c5beac7 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6576,10 +6576,8 @@ static PyObject* pybullet_rayTestObsolete(PyObject* self, PyObject* args, PyObje to[0], to[1], to[2]); - if (collisionFilterMask >= 0) - { - b3RaycastBatchSetCollisionFilterMask(commandHandle, collisionFilterMask); - } + b3RaycastBatchSetCollisionFilterMask(commandHandle, collisionFilterMask); + if (reportHitNumber >= 0) { b3RaycastBatchSetReportHitNumber(commandHandle, reportHitNumber); @@ -6669,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"); @@ -6751,10 +6782,8 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* { b3RaycastBatchSetReportHitNumber(commandHandle, reportHitNumber); } - if (collisionFilterMask >= 0) - { - b3RaycastBatchSetCollisionFilterMask(commandHandle, collisionFilterMask); - } + b3RaycastBatchSetCollisionFilterMask(commandHandle, collisionFilterMask); + if (fractionEpsilon >= 0) { b3RaycastBatchSetFractionEpsilon(commandHandle, fractionEpsilon); @@ -8024,7 +8053,7 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB, &collisionShapeA, &collisionShapeB, &collisionShapePositionAObj, &collisionShapePositionBObj, - &collisionShapeOrientationA, &collisionShapeOrientationBObj, + &collisionShapeOrientationAObj, &collisionShapeOrientationBObj, &physicsClientId)) return NULL; @@ -501,7 +501,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name='pybullet', - version='2.8.4', + version='2.8.6', description= 'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description= diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index e3b8a462b..0c3e0b5eb 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -86,7 +86,8 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) { // add damping matrix m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); - if (m_implicit) + // Always integrate picking force implicitly for stability. + if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE) { m_lf[i]->addScaledElasticForceDifferential(-m_dt * m_dt, x, b); } @@ -176,7 +177,8 @@ void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack& r // add implicit force for (int i = 0; i < m_lf.size(); ++i) { - if (m_implicit) + // Always integrate picking force implicitly for stability. + if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE) { m_lf[i]->addScaledForces(dt, residual); } |