summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorerwincoumans <erwincoumans@google.com>2019-05-30 07:38:09 -0700
committerGitHub <noreply@github.com>2019-05-30 07:38:09 -0700
commit976c01f168c19ddca31fce4f67122e26acb99c0b (patch)
treea6a84b2ff0c3f67b2b03a83d714346de1a1a3a1a
parent78e1baede85026dea5eb43fbcda4bf8a08033031 (diff)
parent411ac4fcc3705b161691917d6d7aba05505079da (diff)
downloadbullet3-976c01f168c19ddca31fce4f67122e26acb99c0b.tar.gz
Merge pull request #2263 from erwincoumans/master
added Microtaur URDF file, with basic system identification
Diffstat
-rw-r--r--examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp22
-rw-r--r--examples/pybullet/gym/pybullet_data/quadruped/microtaur/channel.stlbin0 -> 1535884 bytes
-rw-r--r--examples/pybullet/gym/pybullet_data/quadruped/microtaur/channel.urdf29
-rw-r--r--examples/pybullet/gym/pybullet_data/quadruped/microtaur/d435i.stlbin0 -> 1027884 bytes
-rw-r--r--examples/pybullet/gym/pybullet_data/quadruped/microtaur/d435i.urdf29
-rw-r--r--examples/pybullet/gym/pybullet_data/quadruped/microtaur/microtaur.urdf1663
-rw-r--r--examples/pybullet/gym/pybullet_data/quadruped/microtaur/plate.stlbin0 -> 755484 bytes
-rw-r--r--examples/pybullet/gym/pybullet_data/quadruped/microtaur/plate.urdf29
-rw-r--r--examples/pybullet/gym/pybullet_data/quadruped/microtaur/quadru.py22
-rw-r--r--examples/pybullet/gym/pybullet_data/quadruped/microtaur/xavier.stlbin0 -> 1150184 bytes
-rw-r--r--examples/pybullet/gym/pybullet_data/quadruped/microtaur/xavier.urdf29
-rw-r--r--examples/pybullet/gym/pybullet_data/quadruped/microtaur/xm430w210.stlbin0 -> 915084 bytes
-rw-r--r--examples/pybullet/gym/pybullet_data/quadruped/microtaur/xm430w210.urdf29
-rw-r--r--examples/pybullet/gym/pybullet_envs/examples/microtaur.py467
-rw-r--r--examples/pybullet/pybullet.c3
15 files changed, 2312 insertions, 10 deletions
diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
index e41901674..253a338e0 100644
--- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
+++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
@@ -9,9 +9,9 @@
#include "../../CommonInterfaces/CommonFileIOInterface.h"
#include "../ImportURDFDemo/UrdfFindMeshFile.h"
#include <string>
-#include "../../Utils/b3ResourcePath.h"
#include <iostream>
#include <fstream>
+#include "../../Utils/b3ResourcePath.h"
#include "../ImportURDFDemo/URDF2Bullet.h"
#include "../ImportURDFDemo/UrdfParser.h"
#include "../ImportURDFDemo/urdfStringSplit.h"
@@ -1453,17 +1453,21 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger,
}
else
{
- int maxPathLen = 1024;
- fu.extractPath(relativeFileName, m_data->m_pathPrefix, maxPathLen);
+ //read file
+ int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r");
- std::fstream xml_file(relativeFileName, std::fstream::in);
- while (xml_file.good())
+ char destBuffer[8192];
+ char* line = 0;
+ do
{
- std::string line;
- std::getline(xml_file, line);
- xml_string += (line + "\n");
+ line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192);
+ if (line)
+ {
+ xml_string += (std::string(destBuffer) + "\n");
+ }
}
- xml_file.close();
+ while (line);
+ m_data->m_fileIO->fileClose(fileId);
if (parseMJCFString(xml_string.c_str(), logger))
{
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/microtaur/channel.stl b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/channel.stl
new file mode 100644
index 000000000..5bdce60d6
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/channel.stl
Binary files differ
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/microtaur/channel.urdf b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/channel.urdf
new file mode 100644
index 000000000..7fa546941
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/channel.urdf
@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.13"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="-0.019 -0.019 -0.1502"/>
+ <geometry>
+ <mesh filename="channel.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.038 0.038 0.305"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/microtaur/d435i.stl b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/d435i.stl
new file mode 100644
index 000000000..a00d3d25f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/d435i.stl
Binary files differ
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/microtaur/d435i.urdf b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/d435i.urdf
new file mode 100644
index 000000000..d519e1d3a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/d435i.urdf
@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.081"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0.0125"/>
+ <geometry>
+ <mesh filename="d435i.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.09 0.025 0.025"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/microtaur/microtaur.urdf b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/microtaur.urdf
new file mode 100644
index 000000000..d111046ed
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/microtaur.urdf
@@ -0,0 +1,1663 @@
+<?xml version="1.0"?>
+<!-- ======================================================================= -->
+<!--LICENSE: -->
+<!--Copyright (c) 2019, Erwin Coumans -->
+<!--Google Inc. -->
+<!--All rights reserved. -->
+<!-- -->
+<!--Redistribution and use in source and binary forms, with or without -->
+<!--modification, are permitted provided that the following conditions are -->
+<!--met: -->
+<!-- -->
+<!--1. Redistributions or derived work must retain this copyright notice, -->
+<!-- this list of conditions and the following disclaimer. -->
+<!-- -->
+<!--2. Redistributions in binary form must reproduce the above copyright -->
+<!-- notice, this list of conditions and the following disclaimer in the -->
+<!-- documentation and/or other materials provided with the distribution. -->
+<!-- -->
+<!--THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS -->
+<!--IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,-->
+<!--THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -->
+<!--PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -->
+<!--CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, -->
+<!--EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, -->
+<!--PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR -->
+<!--PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -->
+<!--LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING -->
+<!--NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -->
+<!--SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -->
+
+<robot name="microtaur">
+ <link name="base_chassis_link">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 -1.57079" xyz="0 0 0.036"/>
+ <mass value="0.64"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 -1.57079" xyz="-0.045 0.045 0.018"/>
+ <geometry>
+ <mesh filename="xavier.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black">
+ <color rgba="0.3 0.3 0.3 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 -1.57079" xyz="0 0 0.033"/>
+ <geometry>
+ <box size="0.092 0.105 0.045"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="plate0">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.08"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="-0.037 0.0045 0"/>
+ <geometry>
+ <mesh filename="plate.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black1">
+ <color rgba="0.1 0.1 0.1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.074 0.009 0.138"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="plate0_center" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="base_chassis_link"/>
+ <child link="plate0"/>
+ <origin rpy="-1.57079 0 -1.57079" xyz="0.063 0.028 0.005"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+ <link name="plate1">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.08"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="-0.037 0.0045 0"/>
+ <geometry>
+ <mesh filename="plate.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black1">
+ <color rgba="0.1 0.1 0.1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.074 0.009 0.138"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="plate1_center" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="base_chassis_link"/>
+ <child link="plate1"/>
+ <origin rpy="-1.57079 0 -1.57079" xyz="-0.063 0.028 0.005"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="plate2">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.08"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="-0.037 0.0045 0"/>
+ <geometry>
+ <mesh filename="plate.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black1">
+ <color rgba="0.1 0.1 0.1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.074 0.009 0.138"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="plate2_center" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="base_chassis_link"/>
+ <child link="plate2"/>
+ <origin rpy="-1.57079 0 1.57079" xyz="0.063 -0.028 0.005"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+ <link name="plate3">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.08"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="-0.037 0.0045 0"/>
+ <geometry>
+ <mesh filename="plate.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black1">
+ <color rgba="0.1 0.1 0.1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.074 0.009 0.138"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="plate3_center" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="base_chassis_link"/>
+ <child link="plate3"/>
+ <origin rpy="-1.57079 0 1.57079" xyz="-0.063 -0.028 0.005"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+ <link name="chassis_right">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="-1.57079 0 -1.57079" xyz="0 0 0"/>
+ <mass value="0.13"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="-1.57079 0 -1.57079" xyz="-0.1502 0.019 0.019 "/>
+ <geometry>
+ <mesh filename="channel.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="-1.57079 0 -1.57079" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.038 0.038 0.305"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="chassis_right_center" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="base_chassis_link"/>
+ <child link="chassis_right"/>
+ <origin rpy="0 0 0" xyz="0.0 -0.0955 0.019"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+ <link name="chassis_front">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="-1.57079 0 0" xyz="0 0 0"/>
+ <mass value="0.13"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="-1.57079 0 0" xyz="-0.019 -0.1502 0.019 "/>
+ <geometry>
+ <mesh filename="channel.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="bla">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="-1.57079 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.038 0.038 0.305"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="chassis_front_center" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="base_chassis_link"/>
+ <child link="chassis_front"/>
+ <origin rpy="0 0 0" xyz="0.1335 0.0 -0.019"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="dynamixel4">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.082"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0.011 0"/>
+ <geometry>
+ <mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black">
+ <color rgba=".3 .3 .3 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.024 0.047 0.034"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="dynamixel4_chassis_front" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_front"/>
+ <child link="dynamixel4"/>
+ <origin rpy="-1.57079 0 3.14159265359" xyz="0 0.135 -0.03"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+<link name="dynamixel12">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.082"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0.011 0"/>
+ <geometry>
+ <mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black">
+ <color rgba=".3 .3 .3 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.024 0.047 0.034"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="dynamixel12_chassis_front" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_front"/>
+ <child link="dynamixel12"/>
+ <origin rpy="-1.57079 0 0" xyz="0 0.055 -0.03"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="dynamixel3">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.082"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0.011 0"/>
+ <geometry>
+ <mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black">
+ <color rgba=".3 .3 .3 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.024 0.047 0.034"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="dynamixel3_chassis_front" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_front"/>
+ <child link="dynamixel3"/>
+ <origin rpy="-1.57079 0 0" xyz="0 -0.135 -0.03"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+<link name="dynamixel5">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.082"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0.011 0"/>
+ <geometry>
+ <mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black">
+ <color rgba=".3 .3 .3 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.024 0.047 0.034"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="dynamixel5_chassis_front" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_front"/>
+ <child link="dynamixel5"/>
+ <origin rpy="-1.57079 0 3.14159265359" xyz="0 -0.055 -0.03"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+ <link name="dynamixel8">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.082"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0.011 0"/>
+ <geometry>
+ <mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black">
+ <color rgba=".3 .3 .3 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.024 0.047 0.034"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="dynamixel8_chassis_rear" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_rear"/>
+ <child link="dynamixel8"/>
+ <origin rpy="-1.57079 0 3.14159265359" xyz="0 0.135 -0.03"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+<link name="dynamixel10">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.082"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0.011 0"/>
+ <geometry>
+ <mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black">
+ <color rgba=".3 .3 .3 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.024 0.047 0.034"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="dynamixel10_chassis_rear" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_rear"/>
+ <child link="dynamixel10"/>
+ <origin rpy="-1.57079 0 0" xyz="0 0.055 -0.03"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="dynamixel13">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.082"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0.011 0"/>
+ <geometry>
+ <mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black">
+ <color rgba=".3 .3 .3 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.024 0.047 0.034"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="dynamixel13_chassis_rear" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_rear"/>
+ <child link="dynamixel13"/>
+ <origin rpy="-1.57079 0 0" xyz="0 -0.135 -0.03"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+<link name="dynamixel7">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.082"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0.011 0"/>
+ <geometry>
+ <mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black">
+ <color rgba=".3 .3 .3 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.024 0.047 0.034"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="dynamixel7_chassis_rear" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_rear"/>
+ <child link="dynamixel7"/>
+ <origin rpy="-1.57079 0 3.14159265359" xyz="0 -0.055 -0.03"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="dynamixel9">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.082"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0.011 0"/>
+ <geometry>
+ <mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black">
+ <color rgba=".3 .3 .3 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.024 0.047 0.034"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="dynamixel9_chassis_front" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_front"/>
+ <child link="dynamixel9"/>
+ <origin rpy="1.57079 0 0" xyz="0 0 0.042"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+ <link name="d435">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.081"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0.0125"/>
+ <geometry>
+ <mesh filename="d435i.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.09 0.025 0.025"/>
+ </geometry>
+ </collision>
+ </link>
+
+
+ <joint name="d435_dynamixel9" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="dynamixel9"/>
+ <child link="d435"/>
+ <origin rpy="0 1.57079 0" xyz="0 0.04 0"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+ <link name="chassis_rear">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="-1.57079 0 0" xyz="0 0 0"/>
+ <mass value="0.13"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="-1.57079 0 0" xyz="-0.019 -0.1502 0.019 "/>
+ <geometry>
+ <mesh filename="channel.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="-1.57079 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.038 0.038 0.305"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="chassis_rear_center" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="base_chassis_link"/>
+ <child link="chassis_rear"/>
+ <origin rpy="0 0 0" xyz="-0.1335 0.0 -0.019"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+ <link name="chassis_left">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="-1.57079 0 -1.57079" xyz="0 0 0"/>
+ <mass value="0.13"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="-1.57079 0 -1.57079" xyz="-0.1502 0.019 0.019 "/>
+ <geometry>
+ <mesh filename="channel.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="-1.57079 0 -1.57079" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.038 0.038 0.305"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="chassis_left_center" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="base_chassis_link"/>
+ <child link="chassis_left"/>
+ <origin rpy="0 0 0" xyz="0.0 0.0955 0.019"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="motor_front_rightR_link">
+
+ <visual>
+ <geometry>
+ <cylinder length="0.003" radius="0.0134"/>
+ </geometry>
+ <material name="grey1">
+ <color rgba="0. 1. 0. 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <cylinder length="0.003" radius="0.0134"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="motor_front_rightR_joint" type="continuous">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_right"/>
+ <child link="motor_front_rightR_link"/>
+ <origin rpy="1.57075 0 0" xyz="0.1335 -0.035 -0.08"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="motor_front_rightL_link">
+<collision>
+ <geometry>
+ <cylinder length="0.003" radius="0.0134"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="motor_front_rightL_joint" type="continuous">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_right"/>
+ <child link="motor_front_rightL_link"/>
+ <origin rpy="1.57075 0 3.141592" xyz="0.1335 0.03 -0.08"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="motor_front_leftL_link">
+ <collision>
+ <geometry>
+ <cylinder length="0.003" radius="0.0134"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="motor_front_leftL_joint" type="continuous">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_left"/>
+ <child link="motor_front_leftL_link"/>
+ <origin rpy="1.57075 0 3.141592" xyz="0.1335 0.035 -0.08"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="motor_front_leftR_link">
+ <collision>
+ <geometry>
+ <cylinder length="0.003" radius="0.0134"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="motor_front_leftR_joint" type="continuous">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_left"/>
+ <child link="motor_front_leftR_link"/>
+ <origin rpy="1.57075 0 0" xyz="0.1335 -0.03 -0.08"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="motor_back_rightR_link">
+ <collision>
+ <geometry>
+ <cylinder length="0.003" radius="0.0134"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="motor_back_rightR_joint" type="continuous">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_right"/>
+ <child link="motor_back_rightR_link"/>
+ <origin rpy="1.57075 0 0" xyz="-0.1335 -0.035 -0.08"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="motor_back_rightL_link">
+ <collision>
+ <geometry>
+ <cylinder length="0.003" radius="0.0134"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="motor_back_rightL_joint" type="continuous">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_right"/>
+ <child link="motor_back_rightL_link"/>
+ <origin rpy="1.57075 0 3.141592" xyz="-0.1335 0.03 -0.08"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="motor_back_leftL_link">
+ <collision>
+ <geometry>
+ <cylinder length="0.003" radius="0.0134"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="motor_back_leftL_joint" type="continuous">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_left"/>
+ <child link="motor_back_leftL_link"/>
+ <origin rpy="1.57075 0 3.141592" xyz="-0.1335 0.035 -0.08"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="motor_back_leftR_link">
+ <collision>
+ <geometry>
+ <cylinder length="0.003" radius="0.0134"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="motor_back_leftR_joint" type="continuous">
+ <axis xyz="0 0 1"/>
+ <parent link="chassis_left"/>
+ <child link="motor_back_leftR_link"/>
+ <origin rpy="1.57075 0 0" xyz="-0.1335 -0.03 -0.08"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="toe_lower_leg_front_rightR_link">
+ <contact>
+ <lateral_friction value="1.0"/>
+ </contact>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ <material name="darkgray"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+ </inertial>
+ </link>
+ <joint name="toe_lower_leg_front_rightR_joint" type="fixed">
+ <parent link="lower_leg_front_rightR_link"/>
+ <child link="toe_lower_leg_front_rightR_link"/>
+ <origin xyz="0 0 0.20"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="toe_lower_leg_front_rightL_link">
+ <contact>
+ <lateral_friction value="1.0"/>
+ </contact>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ <material name="darkgray"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+ </inertial>
+ </link>
+ <joint name="toe_lower_leg_front_rightL_joint" type="fixed">
+ <parent link="lower_leg_front_rightL_link"/>
+ <child link="toe_lower_leg_front_rightL_link"/>
+ <origin xyz="0 0 0.20"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+
+
+
+ <link name="toe_lower_leg_front_leftR_link">
+ <contact>
+ <lateral_friction value="1.0"/>
+ </contact>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ <material name="darkgray"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+ </inertial>
+ </link>
+ <joint name="toe_lower_leg_front_leftR_joint" type="fixed">
+ <parent link="lower_leg_front_leftR_link"/>
+ <child link="toe_lower_leg_front_leftR_link"/>
+ <origin xyz="0 0 0.20"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+
+ <link name="toe_lower_leg_front_leftL_link">
+ <contact>
+ <lateral_friction value="1.0"/>
+ </contact>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ <material name="darkgray"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+ </inertial>
+ </link>
+ <joint name="toe_lower_leg_front_leftL_joint" type="fixed">
+ <parent link="lower_leg_front_leftL_link"/>
+ <child link="toe_lower_leg_front_leftL_link"/>
+ <origin xyz="0 0 0.20"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+
+
+
+ <link name="toe_lower_leg_back_rightR_link">
+ <contact>
+ <lateral_friction value="1.0"/>
+ </contact>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ <material name="darkgray"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+ </inertial>
+ </link>
+ <joint name="toe_lower_leg_back_rightR_joint" type="fixed">
+ <parent link="lower_leg_back_rightR_link"/>
+ <child link="toe_lower_leg_back_rightR_link"/>
+ <origin xyz="0 0 0.20"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="toe_lower_leg_back_rightL_link">
+ <contact>
+ <lateral_friction value="1.0"/>
+ </contact>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ <material name="darkgray"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+ </inertial>
+ </link>
+ <joint name="toe_lower_leg_back_rightL_joint" type="fixed">
+ <parent link="lower_leg_back_rightL_link"/>
+ <child link="toe_lower_leg_back_rightL_link"/>
+ <origin xyz="0 0 0.20"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+
+
+
+ <link name="toe_lower_leg_back_leftR_link">
+ <contact>
+ <lateral_friction value="1.0"/>
+ </contact>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ <material name="darkgray"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+ </inertial>
+ </link>
+ <joint name="toe_lower_leg_back_leftR_joint" type="fixed">
+ <parent link="lower_leg_back_leftR_link"/>
+ <child link="toe_lower_leg_back_leftR_link"/>
+ <origin xyz="0 0 0.20"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="toe_lower_leg_back_leftL_link">
+ <contact>
+ <lateral_friction value="1.0"/>
+ </contact>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ <material name="darkgray"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+ </inertial>
+ </link>
+ <joint name="toe_lower_leg_back_leftL_joint" type="fixed">
+ <parent link="lower_leg_back_leftL_link"/>
+ <child link="toe_lower_leg_back_leftL_link"/>
+ <origin xyz="0 0 0.20"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+
+ <link name="upper_leg_front_rightR_link">
+ <visual>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="hip_front_rightR_joint" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="motor_front_rightR_link"/>
+ <child link="upper_leg_front_rightR_link"/>
+ <origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+ <link name="lower_leg_front_rightR_link">
+ <contact>
+ <friction_anchor/>
+ <stiffness value="30000.0"/>
+ <damping value="1000.0"/>
+ <spinning_friction value=".3"/>
+ <lateral_friction value="1"/>
+ </contact>
+
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .2"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .2"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="knee_front_rightR_joint" type="continuous">
+ <axis xyz="0 1 0"/>
+ <parent link="upper_leg_front_rightR_link"/>
+ <child link="lower_leg_front_rightR_link"/>
+ <origin rpy="0 0 0" xyz="0.0 0.01 .055"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="upper_leg_front_rightL_link">
+ <visual>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="motor_front_rightL_link" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="motor_front_rightL_link"/>
+ <child link="upper_leg_front_rightL_link"/>
+ <origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+
+
+ <link name="lower_leg_front_rightL_link">
+ <contact>
+ <friction_anchor/>
+ <stiffness value="30000.0"/>
+ <damping value="1000.0"/>
+ <spinning_friction value=".3"/>
+ <lateral_friction value="1"/>
+ </contact>
+
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .198"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .198"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="knee_front_rightL_joint" type="continuous">
+ <axis xyz="0 1 0"/>
+ <parent link="upper_leg_front_rightL_link"/>
+ <child link="lower_leg_front_rightL_link"/>
+ <origin rpy="0 0 0" xyz="0.0 0.01 .055"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="upper_leg_front_leftR_link">
+ <visual>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="hip_front_leftR_joint" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="motor_front_leftR_link"/>
+ <child link="upper_leg_front_leftR_link"/>
+ <origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="lower_leg_front_leftR_link">
+ <contact>
+ <friction_anchor/>
+ <stiffness value="30000.0"/>
+ <damping value="1000.0"/>
+ <spinning_friction value=".3"/>
+ <lateral_friction value="1"/>
+ </contact>
+
+ <visual>
+ <origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .2"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .2"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="knee_front_leftR_joint" type="continuous">
+ <axis xyz="0 1 0"/>
+ <parent link="upper_leg_front_leftR_link"/>
+ <child link="lower_leg_front_leftR_link"/>
+ <origin rpy="0 0 0" xyz="0.0 0.01 .055"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+
+ <link name="upper_leg_front_leftL_link">
+ <visual>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="hip_front_leftL_joint" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="motor_front_leftL_link"/>
+ <child link="upper_leg_front_leftL_link"/>
+ <origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="lower_leg_front_leftL_link">
+ <contact>
+ <friction_anchor/>
+ <stiffness value="30000.0"/>
+ <damping value="1000.0"/>
+ <spinning_friction value=".3"/>
+ <lateral_friction value="1"/>
+ </contact>
+
+ <visual>
+ <origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .198"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .198"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="knee_front_leftL_joint" type="continuous">
+ <axis xyz="0 1 0"/>
+ <parent link="upper_leg_front_leftL_link"/>
+ <child link="lower_leg_front_leftL_link"/>
+ <origin rpy="0 0 0" xyz="0.0 0.01 .055"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+ <link name="upper_leg_back_rightR_link">
+ <visual>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="hip_back_rightR_joint" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="motor_back_rightR_link"/>
+ <child link="upper_leg_back_rightR_link"/>
+ <origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+ <link name="lower_leg_back_rightR_link">
+ <contact>
+ <friction_anchor/>
+ <stiffness value="30000.0"/>
+ <damping value="1000.0"/>
+ <spinning_friction value=".3"/>
+ <lateral_friction value="1"/>
+ </contact>
+
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .2032"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .2032"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="knee_back_rightR_joint" type="continuous">
+ <axis xyz="0 1 0"/>
+ <parent link="upper_leg_back_rightR_link"/>
+ <child link="lower_leg_back_rightR_link"/>
+ <origin rpy="0 0 0" xyz="0.0 0.01 .055"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="upper_leg_back_rightL_link">
+ <visual>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="hip_back_rightL_joint" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="motor_back_rightL_link"/>
+ <child link="upper_leg_back_rightL_link"/>
+ <origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+
+
+ <link name="lower_leg_back_rightL_link">
+ <contact>
+ <friction_anchor/>
+ <stiffness value="30000.0"/>
+ <damping value="1000.0"/>
+ <spinning_friction value=".3"/>
+ <lateral_friction value="1"/>
+ </contact>
+
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .2"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .2"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="knee_back_rightL_joint" type="continuous">
+ <axis xyz="0 1 0"/>
+ <parent link="upper_leg_back_rightL_link"/>
+ <child link="lower_leg_back_rightL_link"/>
+ <origin rpy="0 0 0" xyz="0.0 0.01 .055"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+
+ <link name="upper_leg_back_leftR_link">
+ <visual>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="hip_leftR_joint" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="motor_back_leftR_link"/>
+ <child link="upper_leg_back_leftR_link"/>
+ <origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+<link name="lower_leg_back_leftR_link">
+ <contact>
+ <friction_anchor/>
+ <stiffness value="30000.0"/>
+ <damping value="1000.0"/>
+ <spinning_friction value=".3"/>
+ <lateral_friction value="1"/>
+ </contact>
+
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .2032"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .2032"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="knee_back_leftR_joint" type="continuous">
+ <axis xyz="0 1 0"/>
+ <parent link="upper_leg_back_leftR_link"/>
+ <child link="lower_leg_back_leftR_link"/>
+ <origin rpy="0 0 0" xyz="0.0 0.01 .055"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+ <link name="upper_leg_back_leftL_link">
+ <visual>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <box size=".01 0.01 .11"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="hip_back_leftL_joint" type="fixed">
+ <axis xyz="0 0 1"/>
+ <parent link="motor_back_leftL_link"/>
+ <child link="upper_leg_back_leftL_link"/>
+ <origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+
+
+ <link name="lower_leg_back_leftL_link">
+ <contact>
+ <friction_anchor/>
+ <stiffness value="30000.0"/>
+ <damping value="1000.0"/>
+ <spinning_friction value=".3"/>
+ <lateral_friction value="1"/>
+ </contact>
+
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .2"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.65 0.65 0.75 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <geometry>
+ <box size=".01 0.01 .2"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0 0" xyz="0 0 .1"/>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="knee_back_leftL_joint" type="continuous">
+ <axis xyz="0 1 0"/>
+ <parent link="upper_leg_back_leftL_link"/>
+ <child link="lower_leg_back_leftL_link"/>
+ <origin rpy="0 0 0" xyz="0.0 0.01 .055"/>
+ <limit effort="100" velocity="100"/>
+ <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+</robot>
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/microtaur/plate.stl b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/plate.stl
new file mode 100644
index 000000000..5b6a098df
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/plate.stl
Binary files differ
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/microtaur/plate.urdf b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/plate.urdf
new file mode 100644
index 000000000..704e9802c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/plate.urdf
@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.13"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="-0.037 0.0045 0"/>
+ <geometry>
+ <mesh filename="plate.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.074 0.009 0.138"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/microtaur/quadru.py b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/quadru.py
new file mode 100644
index 000000000..252ee1c09
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/quadru.py
@@ -0,0 +1,22 @@
+import pybullet as p
+cin = p.connect(p.SHARED_MEMORY)
+if (cin < 0):
+ cin = p.connect(p.GUI)
+objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("quadruped/microtaur/microtaur.urdf", 0.858173,-0.698485,0.227967,-0.002864,0.000163,0.951778,0.306776)]
+ob = objects[0]
+jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.568555, 0.000000, -2.177277, 1.570089, 0.000000, -2.184705, 1.570229, 0.000000, -2.182261, 1.570008, 0.000000, -2.184197, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, -1.569978, 0.000000, 2.184092, -1.569669, 0.000000, 2.186906, -1.570584, 0.000000, 2.181503, -1.568404, 0.000000, 2.178427 ]
+for jointIndex in range (p.getNumJoints(ob)):
+ p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
+
+cid0 = p.createConstraint(1,35,1,32,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
+p.changeConstraint(cid0,maxForce=1000.000000)
+cid1 = p.createConstraint(1,7,1,10,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
+p.changeConstraint(cid1,maxForce=1000.000000)
+cid2 = p.createConstraint(1,41,1,38,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
+p.changeConstraint(cid2,maxForce=1000.000000)
+cid3 = p.createConstraint(1,13,1,16,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
+p.changeConstraint(cid3,maxForce=1000.000000)
+p.setGravity(0.000000,0.000000,-10.000000)
+p.stepSimulation()
+p.disconnect()
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/microtaur/xavier.stl b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/xavier.stl
new file mode 100644
index 000000000..7df3364e5
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/xavier.stl
Binary files differ
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/microtaur/xavier.urdf b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/xavier.urdf
new file mode 100644
index 000000000..47b58a758
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/xavier.urdf
@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="xavier">
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.64"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="-0.045 -0.045 -0.018"/>
+ <geometry>
+ <mesh filename="xavier.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.092 0.105 0.045"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/microtaur/xm430w210.stl b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/xm430w210.stl
new file mode 100644
index 000000000..10a55eaaf
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/xm430w210.stl
Binary files differ
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/microtaur/xm430w210.urdf b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/xm430w210.urdf
new file mode 100644
index 000000000..cba3b292d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/quadruped/microtaur/xm430w210.urdf
@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.082"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0.011 0"/>
+ <geometry>
+ <mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
+ </geometry>
+ <material name="black">
+ <color rgba=".3 .3 .3 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.024 0.047 0.034"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_envs/examples/microtaur.py b/examples/pybullet/gym/pybullet_envs/examples/microtaur.py
new file mode 100644
index 000000000..2d5b843a3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/examples/microtaur.py
@@ -0,0 +1,467 @@
+import pybullet as p
+import pybullet_data as pd
+
+import time
+import math
+
+
+def drawInertiaBox(parentUid, parentLinkIndex, color):
+ return
+ dyn = p.getDynamicsInfo(parentUid, parentLinkIndex)
+ mass = dyn[0]
+ frictionCoeff = dyn[1]
+ inertia = dyn[2]
+ if (mass > 0):
+ Ixx = inertia[0]
+ Iyy = inertia[1]
+ Izz = inertia[2]
+ boxScaleX = 0.5 * math.sqrt(6 * (Izz + Iyy - Ixx) / mass)
+ boxScaleY = 0.5 * math.sqrt(6 * (Izz + Ixx - Iyy) / mass)
+ boxScaleZ = 0.5 * math.sqrt(6 * (Ixx + Iyy - Izz) / mass)
+
+ halfExtents = [boxScaleX, boxScaleY, boxScaleZ]
+ pts = [[halfExtents[0], halfExtents[1], halfExtents[2]],
+ [-halfExtents[0], halfExtents[1], halfExtents[2]],
+ [halfExtents[0], -halfExtents[1], halfExtents[2]],
+ [-halfExtents[0], -halfExtents[1], halfExtents[2]],
+ [halfExtents[0], halfExtents[1], -halfExtents[2]],
+ [-halfExtents[0], halfExtents[1], -halfExtents[2]],
+ [halfExtents[0], -halfExtents[1], -halfExtents[2]],
+ [-halfExtents[0], -halfExtents[1], -halfExtents[2]]]
+
+ p.addUserDebugLine(pts[0],
+ pts[1],
+ color,
+ 1,
+ parentObjectUniqueId=parentUid,
+ parentLinkIndex=parentLinkIndex)
+ p.addUserDebugLine(pts[1],
+ pts[3],
+ color,
+ 1,
+ parentObjectUniqueId=parentUid,
+ parentLinkIndex=parentLinkIndex)
+ p.addUserDebugLine(pts[3],
+ pts[2],
+ color,
+ 1,
+ parentObjectUniqueId=parentUid,
+ parentLinkIndex=parentLinkIndex)
+ p.addUserDebugLine(pts[2],
+ pts[0],
+ color,
+ 1,
+ parentObjectUniqueId=parentUid,
+ parentLinkIndex=parentLinkIndex)
+
+ p.addUserDebugLine(pts[0],
+ pts[4],
+ color,
+ 1,
+ parentObjectUniqueId=parentUid,
+ parentLinkIndex=parentLinkIndex)
+ p.addUserDebugLine(pts[1],
+ pts[5],
+ color,
+ 1,
+ parentObjectUniqueId=parentUid,
+ parentLinkIndex=parentLinkIndex)
+ p.addUserDebugLine(pts[2],
+ pts[6],
+ color,
+ 1,
+ parentObjectUniqueId=parentUid,
+ parentLinkIndex=parentLinkIndex)
+ p.addUserDebugLine(pts[3],
+ pts[7],
+ color,
+ 1,
+ parentObjectUniqueId=parentUid,
+ parentLinkIndex=parentLinkIndex)
+
+ p.addUserDebugLine(pts[4 + 0],
+ pts[4 + 1],
+ color,
+ 1,
+ parentObjectUniqueId=parentUid,
+ parentLinkIndex=parentLinkIndex)
+ p.addUserDebugLine(pts[4 + 1],
+ pts[4 + 3],
+ color,
+ 1,
+ parentObjectUniqueId=parentUid,
+ parentLinkIndex=parentLinkIndex)
+ p.addUserDebugLine(pts[4 + 3],
+ pts[4 + 2],
+ color,
+ 1,
+ parentObjectUniqueId=parentUid,
+ parentLinkIndex=parentLinkIndex)
+ p.addUserDebugLine(pts[4 + 2],
+ pts[4 + 0],
+ color,
+ 1,
+ parentObjectUniqueId=parentUid,
+ parentLinkIndex=parentLinkIndex)
+
+
+toeConstraint = True
+useMaximalCoordinates = False
+useRealTime = 1
+
+#the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance
+fixedTimeStep = 1. / 100
+numSolverIterations = 50
+
+if (useMaximalCoordinates):
+ fixedTimeStep = 1. / 500
+ numSolverIterations = 200
+
+speed = 10
+amplitude = 0.8
+jump_amp = 0.5
+maxForce = 3.5
+kneeFrictionForce = 0
+kp = 1
+kd = .5
+maxKneeForce = 1000
+
+physId = p.connect(p.SHARED_MEMORY)
+
+if (physId < 0):
+ p.connect(p.GUI)
+#p.resetSimulation()
+p.setAdditionalSearchPath(pd.getDataPath())
+angle = 0 # pick in range 0..0.2 radians
+orn = p.getQuaternionFromEuler([0, angle, 0])
+p.loadURDF("plane.urdf", [0, 0, 0], orn)
+p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
+p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
+ "genericlogdata.bin",
+ maxLogDof=16,
+ logFlags=p.STATE_LOG_JOINT_TORQUES)
+p.setTimeOut(4000000)
+
+p.setGravity(0, 0, 0)
+p.setTimeStep(fixedTimeStep)
+
+orn = p.getQuaternionFromEuler([0, 0, 0.4])
+p.setRealTimeSimulation(0)
+quadruped = p.loadURDF("quadruped/microtaur/microtaur.urdf", [1, -1, .3],
+ orn,
+ useFixedBase=False,
+ useMaximalCoordinates=useMaximalCoordinates,
+ flags=p.URDF_USE_IMPLICIT_CYLINDER)
+nJoints = p.getNumJoints(quadruped)
+
+jointNameToId = {}
+for i in range(nJoints):
+ jointInfo = p.getJointInfo(quadruped, i)
+ jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
+
+motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
+motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
+knee_front_rightL_joint = jointNameToId['knee_front_rightL_joint']
+hip_front_rightR_joint = jointNameToId['hip_front_rightR_joint']
+knee_front_rightR_joint = jointNameToId['knee_front_rightR_joint']
+motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
+motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
+hip_front_leftR_joint = jointNameToId['hip_front_leftR_joint']
+knee_front_leftR_joint = jointNameToId['knee_front_leftR_joint']
+motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
+motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
+knee_front_leftL_joint = jointNameToId['knee_front_leftL_joint']
+motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint']
+knee_back_rightR_joint = jointNameToId['knee_back_rightR_joint']
+motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
+motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
+knee_back_rightL_joint = jointNameToId['knee_back_rightL_joint']
+motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint']
+knee_back_leftR_joint = jointNameToId['knee_back_leftR_joint']
+motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
+motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
+knee_back_leftL_joint = jointNameToId['knee_back_leftL_joint']
+
+#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
+
+motordir = [-1, -1, -1, -1, 1, 1, 1, 1]
+halfpi = 1.57079632679
+twopi = 4 * halfpi
+kneeangle = -2.1834
+
+dyn = p.getDynamicsInfo(quadruped, -1)
+mass = dyn[0]
+friction = dyn[1]
+localInertiaDiagonal = dyn[2]
+
+print("localInertiaDiagonal", localInertiaDiagonal)
+
+#this is a no-op, just to show the API
+p.changeDynamics(quadruped, -1, localInertiaDiagonal=localInertiaDiagonal)
+
+#for i in range (nJoints):
+# p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
+
+drawInertiaBox(quadruped, -1, [1, 0, 0])
+#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
+
+for i in range(nJoints):
+ drawInertiaBox(quadruped, i, [0, 1, 0])
+
+if (useMaximalCoordinates):
+ steps = 400
+ for aa in range(steps):
+ p.setJointMotorControl2(quadruped, motor_front_leftL_joint, p.POSITION_CONTROL,
+ motordir[0] * halfpi * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, motor_front_leftR_joint, p.POSITION_CONTROL,
+ motordir[1] * halfpi * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, motor_back_leftL_joint, p.POSITION_CONTROL,
+ motordir[2] * halfpi * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, motor_back_leftR_joint, p.POSITION_CONTROL,
+ motordir[3] * halfpi * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, motor_front_rightL_joint, p.POSITION_CONTROL,
+ motordir[4] * halfpi * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, motor_front_rightR_joint, p.POSITION_CONTROL,
+ motordir[5] * halfpi * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, motor_back_rightL_joint, p.POSITION_CONTROL,
+ motordir[6] * halfpi * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, motor_back_rightR_joint, p.POSITION_CONTROL,
+ motordir[7] * halfpi * float(aa) / steps)
+
+ p.setJointMotorControl2(quadruped, knee_front_leftL_joint, p.POSITION_CONTROL,
+ motordir[0] * (kneeangle + twopi) * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, knee_front_leftR_joint, p.POSITION_CONTROL,
+ motordir[1] * kneeangle * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, knee_back_leftL_joint, p.POSITION_CONTROL,
+ motordir[2] * kneeangle * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, knee_back_leftR_joint, p.POSITION_CONTROL,
+ motordir[3] * (kneeangle + twopi) * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, knee_front_rightL_joint, p.POSITION_CONTROL,
+ motordir[4] * (kneeangle) * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, knee_front_rightR_joint, p.POSITION_CONTROL,
+ motordir[5] * (kneeangle + twopi) * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, knee_back_rightL_joint, p.POSITION_CONTROL,
+ motordir[6] * (kneeangle + twopi) * float(aa) / steps)
+ p.setJointMotorControl2(quadruped, knee_back_rightR_joint, p.POSITION_CONTROL,
+ motordir[7] * kneeangle * float(aa) / steps)
+
+ p.stepSimulation()
+ #time.sleep(fixedTimeStep)
+else:
+
+ p.resetJointState(quadruped, motor_front_leftL_joint, motordir[0] * halfpi)
+ p.resetJointState(quadruped, knee_front_leftL_joint, motordir[0] * kneeangle)
+ p.resetJointState(quadruped, motor_front_leftR_joint, motordir[1] * halfpi)
+ p.resetJointState(quadruped, knee_front_leftR_joint, motordir[1] * kneeangle)
+
+ p.resetJointState(quadruped, motor_back_leftL_joint, motordir[2] * halfpi)
+ p.resetJointState(quadruped, knee_back_leftL_joint, motordir[2] * kneeangle)
+ p.resetJointState(quadruped, motor_back_leftR_joint, motordir[3] * halfpi)
+ p.resetJointState(quadruped, knee_back_leftR_joint, motordir[3] * kneeangle)
+
+ p.resetJointState(quadruped, motor_front_rightL_joint, motordir[4] * halfpi)
+ p.resetJointState(quadruped, knee_front_rightL_joint, motordir[4] * kneeangle)
+ p.resetJointState(quadruped, motor_front_rightR_joint, motordir[5] * halfpi)
+ p.resetJointState(quadruped, knee_front_rightR_joint, motordir[5] * kneeangle)
+
+ p.resetJointState(quadruped, motor_back_rightL_joint, motordir[6] * halfpi)
+ p.resetJointState(quadruped, knee_back_rightL_joint, motordir[6] * kneeangle)
+ p.resetJointState(quadruped, motor_back_rightR_joint, motordir[7] * halfpi)
+ p.resetJointState(quadruped, knee_back_rightR_joint, motordir[7] * kneeangle)
+
+#p.getNumJoints(1)
+
+if (toeConstraint):
+ cid = p.createConstraint(quadruped, knee_front_leftR_joint, quadruped, knee_front_leftL_joint,
+ p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
+ p.changeConstraint(cid, maxForce=maxKneeForce)
+ cid = p.createConstraint(quadruped, knee_front_rightR_joint, quadruped, knee_front_rightL_joint,
+ p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
+ p.changeConstraint(cid, maxForce=maxKneeForce)
+ cid = p.createConstraint(quadruped, knee_back_leftR_joint, quadruped, knee_back_leftL_joint,
+ p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
+ p.changeConstraint(cid, maxForce=maxKneeForce)
+ cid = p.createConstraint(quadruped, knee_back_rightR_joint, quadruped, knee_back_rightL_joint,
+ p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
+ p.changeConstraint(cid, maxForce=maxKneeForce)
+
+if (1):
+ p.setJointMotorControl(quadruped, knee_front_leftL_joint, p.VELOCITY_CONTROL, 0,
+ kneeFrictionForce)
+ p.setJointMotorControl(quadruped, knee_front_leftR_joint, p.VELOCITY_CONTROL, 0,
+ kneeFrictionForce)
+ p.setJointMotorControl(quadruped, knee_front_rightL_joint, p.VELOCITY_CONTROL, 0,
+ kneeFrictionForce)
+ p.setJointMotorControl(quadruped, knee_front_rightR_joint, p.VELOCITY_CONTROL, 0,
+ kneeFrictionForce)
+ p.setJointMotorControl(quadruped, knee_back_leftL_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
+ p.setJointMotorControl(quadruped, knee_back_leftR_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
+ p.setJointMotorControl(quadruped, knee_back_leftL_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
+ p.setJointMotorControl(quadruped, knee_back_leftR_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
+ p.setJointMotorControl(quadruped, knee_back_rightL_joint, p.VELOCITY_CONTROL, 0,
+ kneeFrictionForce)
+ p.setJointMotorControl(quadruped, knee_back_rightR_joint, p.VELOCITY_CONTROL, 0,
+ kneeFrictionForce)
+
+p.setGravity(0, 0, -10)
+
+legnumbering = [
+ motor_front_leftL_joint, motor_front_leftR_joint, motor_back_leftL_joint,
+ motor_back_leftR_joint, motor_front_rightL_joint, motor_front_rightR_joint,
+ motor_back_rightL_joint, motor_back_rightR_joint
+]
+
+for i in range(8):
+ print(legnumbering[i])
+#use the Minitaur leg numbering
+p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[0],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[0] * 1.57,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[1],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[1] * 1.57,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[2],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[2] * 1.57,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[3],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[3] * 1.57,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[4],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[4] * 1.57,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[5],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[5] * 1.57,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[6],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[6] * 1.57,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[7],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[7] * 1.57,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+#stand still
+p.setRealTimeSimulation(useRealTime)
+
+t = 0.0
+t_end = t + 5
+ref_time = time.time()
+while (t < t_end):
+ p.setGravity(0, 0, -10)
+ if (useRealTime):
+ t = time.time() - ref_time
+ else:
+ t = t + fixedTimeStep
+ if (useRealTime == 0):
+ p.stepSimulation()
+ time.sleep(fixedTimeStep)
+
+print("quadruped Id = ")
+print(quadruped)
+p.saveWorld("quadru.py")
+logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "quadrupedLog.bin", [quadruped])
+
+#jump
+t = 0.0
+t_end = t + 100
+i = 0
+ref_time = time.time()
+
+while (1):
+ if (useRealTime):
+ t = time.time() - ref_time
+ else:
+ t = t + fixedTimeStep
+ if (True):
+
+ target = math.sin(t * speed) * jump_amp + 1.57
+ p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[0],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[0] * target,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+ p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[1],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[1] * target,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+ p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[2],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[2] * target,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+ p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[3],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[3] * target,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+ p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[4],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[4] * target,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+ p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[5],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[5] * target,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+ p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[6],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[6] * target,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+ p.setJointMotorControl2(bodyIndex=quadruped,
+ jointIndex=legnumbering[7],
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=motordir[7] * target,
+ positionGain=kp,
+ velocityGain=kd,
+ force=maxForce)
+
+ if (useRealTime == 0):
+ p.stepSimulation()
+ time.sleep(fixedTimeStep)
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 5b8ca6e5b..b884a155b 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -336,9 +336,10 @@ static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObjec
struct b3ForwardDynamicsAnalyticsArgs analyticsData;
int numIslands = 0;
int i;
+ PyObject* pyAnalyticsData = PyTuple_New(numIslands);
+
numIslands = b3GetStatusForwardDynamicsAnalyticsData(statusHandle, &analyticsData);
- PyObject* pyAnalyticsData = PyTuple_New(numIslands);
for (i=0;i<numIslands;i++)
{
int numFields = 4;
View Lorry Controller Status