Merge pull request #990 from erwincoumans/master
improved minitaur.urdf file, see https://youtu.be/lv7lybtOzeo for a preview.
This commit is contained in:
erwincoumans
GitHub
@@ -2,7 +2,7 @@
|
||||
<robot name="cube.urdf">
|
||||
<link name="planeLink">
|
||||
<contact>
|
||||
<lateral_friction value="2"/>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
|
||||
BIN
data/quadruped/LOG00076.TXT
Normal file
BIN
data/quadruped/LOG00076.TXT
Normal file
Binary file not shown.
913
data/quadruped/minitaur.urdf
Normal file
913
data/quadruped/minitaur.urdf
Normal file
@@ -0,0 +1,913 @@
|
||||
<?xml version="0.0" ?>
|
||||
<!-- ======================================================================= -->
|
||||
<!--LICENSE: -->
|
||||
<!--Copyright (c) 2017, 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="quadruped">
|
||||
<link name="base_chassis_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".33 0.14 .07"/>
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.3 0.3 0.3 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0.10 0"/>
|
||||
<geometry>
|
||||
<box size=".17 0.10 .05"/>
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.3 0.3 0.3 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
|
||||
<geometry>
|
||||
<box size=".17 0.10 .05"/>
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.3 0.3 0.3 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".34 0.14 .07"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0.10 0"/>
|
||||
<geometry>
|
||||
<box size=".17 0.10 .05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
|
||||
<geometry>
|
||||
<box size=".17 0.10 .05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<mass value="3"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="chassis_right">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0.035 0"/>
|
||||
<geometry>
|
||||
<box size=".34 0.01 .04"/>
|
||||
</geometry>
|
||||
<material name="grey">
|
||||
<color rgba="0.65 0.65 0.75 1"/>
|
||||
</material> </visual>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
|
||||
<geometry>
|
||||
<box size=".34 0.01 .04"/>
|
||||
</geometry>
|
||||
<material name="grey">
|
||||
<color rgba="0.65 0.65 0.75 1"/>
|
||||
</material> </visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0.035 0"/>
|
||||
<geometry>
|
||||
<box size=".34 0.01 .04"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
|
||||
<geometry>
|
||||
<box size=".34 0.01 .04"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value=".1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</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.0872665 0 0" xyz="0.0 -0.10 0.0"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="chassis_left">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0.035 0"/>
|
||||
<geometry>
|
||||
<box size=".34 0.01 .04"/>
|
||||
</geometry>
|
||||
<material name="grey">
|
||||
<color rgba="0.65 0.65 0.75 1"/>
|
||||
</material> </visual>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
|
||||
<geometry>
|
||||
<box size=".34 0.01 .04"/>
|
||||
</geometry>
|
||||
<material name="grey">
|
||||
<color rgba="0.65 0.65 0.75 1"/>
|
||||
</material> </visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0.035 0"/>
|
||||
<geometry>
|
||||
<box size=".34 0.01 .04"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
|
||||
<geometry>
|
||||
<box size=".34 0.01 .04"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value=".1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</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.0872665 0 0" xyz="0.0 0.10 0.0"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
<link name="motor_front_rightR_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="tmotor3.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.026" radius="0.0434"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.25"/>
|
||||
<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.21 -0.025 0"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="motor_front_rightL_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="tmotor3.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.026" radius="0.0434"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.25"/>
|
||||
<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.21 0.04 0"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="motor_front_leftR_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="tmotor3.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.026" radius="0.0434"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.25"/>
|
||||
<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 3.141592" xyz="0.21 0.025 0"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="motor_front_leftL_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="tmotor3.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="0.25"/>
|
||||
<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 0" xyz="0.21 -0.04 0"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="motor_back_rightR_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="tmotor3.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.026" radius="0.0434"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.25"/>
|
||||
<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.21 -0.025 0"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="motor_back_rightL_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="tmotor3.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.026" radius="0.0434"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.25"/>
|
||||
<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.21 0.04 0"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="motor_back_leftR_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="tmotor3.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.026" radius="0.0434"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.25"/>
|
||||
<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 3.141592" xyz="-0.21 0.025 0"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="motor_back_leftL_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="tmotor3.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.026" radius="0.0434"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.25"/>
|
||||
<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 0" xyz="-0.21 -0.04 0"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties 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_link" 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>
|
||||
<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>
|
||||
<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_link" type="revolute">
|
||||
<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>
|
||||
<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>
|
||||
<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_link" type="revolute">
|
||||
<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_link" 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>
|
||||
<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>
|
||||
<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_link" type="revolute">
|
||||
<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="motor_front_leftL_link" 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>
|
||||
<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>
|
||||
<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_link" type="revolute">
|
||||
<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_rightR_link" 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>
|
||||
<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>
|
||||
<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_link" type="revolute">
|
||||
<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="motor_back_rightL_link" 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>
|
||||
<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>
|
||||
<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_link" type="revolute">
|
||||
<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_link" 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>
|
||||
<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>
|
||||
<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_link" type="revolute">
|
||||
<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="motor_back_leftL_link" 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>
|
||||
<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>
|
||||
<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_link" type="revolute">
|
||||
<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>
|
||||
|
||||
BIN
data/quadruped/t-motor.jpg
Normal file
BIN
data/quadruped/t-motor.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 153 KiB |
BIN
data/quadruped/tmotor.blend
Normal file
BIN
data/quadruped/tmotor.blend
Normal file
Binary file not shown.
19
data/quadruped/tmotor3.mtl
Normal file
19
data/quadruped/tmotor3.mtl
Normal file
@@ -0,0 +1,19 @@
|
||||
# Blender MTL File: 'tmotor.blend'
|
||||
# Material Count: 2
|
||||
|
||||
newmtl None
|
||||
Ns 0
|
||||
Ka 0.000000 0.000000 0.000000
|
||||
Kd 0.8 0.8 0.8
|
||||
Ks 0.8 0.8 0.8
|
||||
d 1
|
||||
illum 2
|
||||
map_Kd t-motor.jpg
|
||||
|
||||
newmtl None_NONE
|
||||
Ns 0
|
||||
Ka 0.000000 0.000000 0.000000
|
||||
Kd 0.8 0.8 0.8
|
||||
Ks 0.8 0.8 0.8
|
||||
d 1
|
||||
illum 2
|
||||
325
data/quadruped/tmotor3.obj
Normal file
325
data/quadruped/tmotor3.obj
Normal file
@@ -0,0 +1,325 @@
|
||||
# Blender v2.78 (sub 0) OBJ File: 'tmotor.blend'
|
||||
# www.blender.org
|
||||
mtllib tmotor3.mtl
|
||||
o Cylinder
|
||||
v 0.000000 0.043000 -0.006500
|
||||
v 0.000000 0.043000 0.006500
|
||||
v 0.008389 0.042174 -0.006500
|
||||
v 0.008389 0.042174 0.006500
|
||||
v 0.016455 0.039727 -0.006500
|
||||
v 0.016455 0.039727 0.006500
|
||||
v 0.023890 0.035753 -0.006500
|
||||
v 0.023890 0.035753 0.006500
|
||||
v 0.030406 0.030406 -0.006500
|
||||
v 0.030406 0.030406 0.006500
|
||||
v 0.035753 0.023890 -0.006500
|
||||
v 0.035753 0.023890 0.006500
|
||||
v 0.039727 0.016455 -0.006500
|
||||
v 0.039727 0.016455 0.006500
|
||||
v 0.042174 0.008389 -0.006500
|
||||
v 0.042174 0.008389 0.006500
|
||||
v 0.043000 0.000000 -0.006500
|
||||
v 0.043000 0.000000 0.006500
|
||||
v 0.042174 -0.008389 -0.006500
|
||||
v 0.042174 -0.008389 0.006500
|
||||
v 0.039727 -0.016455 -0.006500
|
||||
v 0.039727 -0.016455 0.006500
|
||||
v 0.035753 -0.023890 -0.006500
|
||||
v 0.035753 -0.023890 0.006500
|
||||
v 0.030406 -0.030406 -0.006500
|
||||
v 0.030406 -0.030406 0.006500
|
||||
v 0.023890 -0.035753 -0.006500
|
||||
v 0.023890 -0.035753 0.006500
|
||||
v 0.016455 -0.039727 -0.006500
|
||||
v 0.016455 -0.039727 0.006500
|
||||
v 0.008389 -0.042174 -0.006500
|
||||
v 0.008389 -0.042174 0.006500
|
||||
v -0.000000 -0.043000 -0.006500
|
||||
v -0.000000 -0.043000 0.006500
|
||||
v -0.008389 -0.042174 -0.006500
|
||||
v -0.008389 -0.042174 0.006500
|
||||
v -0.016455 -0.039727 -0.006500
|
||||
v -0.016455 -0.039727 0.006500
|
||||
v -0.023890 -0.035753 -0.006500
|
||||
v -0.023890 -0.035753 0.006500
|
||||
v -0.030406 -0.030406 -0.006500
|
||||
v -0.030406 -0.030406 0.006500
|
||||
v -0.035753 -0.023889 -0.006500
|
||||
v -0.035753 -0.023889 0.006500
|
||||
v -0.039727 -0.016455 -0.006500
|
||||
v -0.039727 -0.016455 0.006500
|
||||
v -0.042174 -0.008389 -0.006500
|
||||
v -0.042174 -0.008389 0.006500
|
||||
v -0.043000 0.000000 -0.006500
|
||||
v -0.043000 0.000000 0.006500
|
||||
v -0.042174 0.008389 -0.006500
|
||||
v -0.042174 0.008389 0.006500
|
||||
v -0.039727 0.016455 -0.006500
|
||||
v -0.039727 0.016455 0.006500
|
||||
v -0.035753 0.023890 -0.006500
|
||||
v -0.035753 0.023890 0.006500
|
||||
v -0.030406 0.030406 -0.006500
|
||||
v -0.030406 0.030406 0.006500
|
||||
v -0.023889 0.035753 -0.006500
|
||||
v -0.023889 0.035753 0.006500
|
||||
v -0.016455 0.039727 -0.006500
|
||||
v -0.016455 0.039727 0.006500
|
||||
v -0.008389 0.042174 -0.006500
|
||||
v -0.008389 0.042174 0.006500
|
||||
vt 0.6520 0.1657
|
||||
vt 0.8624 0.3762
|
||||
vt 0.6520 0.8843
|
||||
vt 0.5790 0.9064
|
||||
vt 0.3543 0.8843
|
||||
vt 0.5031 0.9139
|
||||
vt 0.4273 0.9064
|
||||
vt 0.2871 0.8484
|
||||
vt 0.2281 0.8000
|
||||
vt 0.1798 0.7411
|
||||
vt 0.1438 0.6738
|
||||
vt 0.1217 0.6009
|
||||
vt 0.1142 0.5250
|
||||
vt 0.1217 0.4491
|
||||
vt 0.1438 0.3762
|
||||
vt 0.1798 0.3089
|
||||
vt 0.2281 0.2500
|
||||
vt 0.2871 0.2016
|
||||
vt 0.3543 0.1657
|
||||
vt 0.4273 0.1436
|
||||
vt 0.5031 0.1361
|
||||
vt 0.5790 0.1436
|
||||
vt 0.7192 0.2016
|
||||
vt 0.7781 0.2500
|
||||
vt 0.8265 0.3089
|
||||
vt 0.8846 0.4491
|
||||
vt 0.8920 0.5250
|
||||
vt 0.8846 0.6009
|
||||
vt 0.8624 0.6738
|
||||
vt 0.8265 0.7411
|
||||
vt 0.7781 0.8000
|
||||
vt 0.7192 0.8484
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vn 0.0000 0.0000 1.0000
|
||||
vn 0.0980 0.9952 0.0000
|
||||
vn 0.2903 0.9569 0.0000
|
||||
vn 0.4714 0.8819 0.0000
|
||||
vn 0.6344 0.7730 0.0000
|
||||
vn 0.7730 0.6344 0.0000
|
||||
vn 0.8819 0.4714 0.0000
|
||||
vn 0.9569 0.2903 0.0000
|
||||
vn 0.9952 0.0980 0.0000
|
||||
vn 0.9952 -0.0980 0.0000
|
||||
vn 0.9569 -0.2903 0.0000
|
||||
vn 0.8819 -0.4714 0.0000
|
||||
vn 0.7730 -0.6344 0.0000
|
||||
vn 0.6344 -0.7730 0.0000
|
||||
vn 0.4714 -0.8819 0.0000
|
||||
vn 0.2903 -0.9569 0.0000
|
||||
vn 0.0980 -0.9952 0.0000
|
||||
vn -0.0980 -0.9952 0.0000
|
||||
vn -0.2903 -0.9569 0.0000
|
||||
vn -0.4714 -0.8819 0.0000
|
||||
vn -0.6344 -0.7730 0.0000
|
||||
vn -0.7730 -0.6344 0.0000
|
||||
vn -0.8819 -0.4714 0.0000
|
||||
vn -0.9569 -0.2903 0.0000
|
||||
vn -0.9952 -0.0980 0.0000
|
||||
vn -0.9952 0.0980 0.0000
|
||||
vn -0.9569 0.2903 0.0000
|
||||
vn -0.8819 0.4714 0.0000
|
||||
vn -0.7730 0.6344 0.0000
|
||||
vn -0.6344 0.7730 0.0000
|
||||
vn -0.4714 0.8819 0.0000
|
||||
vn -0.2903 0.9569 0.0000
|
||||
vn -0.0980 0.9952 0.0000
|
||||
vn 0.0000 0.0000 -1.0000
|
||||
usemtl None
|
||||
s off
|
||||
f 30/1/1 22/2/1 6/3/1
|
||||
f 6/3/1 4/4/1 62/5/1
|
||||
f 2/6/1 64/7/1 62/5/1
|
||||
f 62/5/1 60/8/1 58/9/1
|
||||
f 58/9/1 56/10/1 54/11/1
|
||||
f 54/11/1 52/12/1 50/13/1
|
||||
f 50/13/1 48/14/1 54/11/1
|
||||
f 46/15/1 44/16/1 42/17/1
|
||||
f 42/17/1 40/18/1 38/19/1
|
||||
f 38/19/1 36/20/1 34/21/1
|
||||
f 34/21/1 32/22/1 38/19/1
|
||||
f 30/1/1 28/23/1 26/24/1
|
||||
f 26/24/1 24/25/1 22/2/1
|
||||
f 22/2/1 20/26/1 18/27/1
|
||||
f 18/27/1 16/28/1 22/2/1
|
||||
f 14/29/1 12/30/1 10/31/1
|
||||
f 10/31/1 8/32/1 6/3/1
|
||||
f 4/4/1 2/6/1 62/5/1
|
||||
f 62/5/1 58/9/1 6/3/1
|
||||
f 54/11/1 48/14/1 46/15/1
|
||||
f 46/15/1 42/17/1 54/11/1
|
||||
f 38/19/1 32/22/1 30/1/1
|
||||
f 30/1/1 26/24/1 22/2/1
|
||||
f 22/2/1 16/28/1 14/29/1
|
||||
f 14/29/1 10/31/1 22/2/1
|
||||
f 6/3/1 58/9/1 54/11/1
|
||||
f 54/11/1 42/17/1 38/19/1
|
||||
f 38/19/1 30/1/1 6/3/1
|
||||
f 22/2/1 10/31/1 6/3/1
|
||||
f 6/3/1 54/11/1 38/19/1
|
||||
usemtl None
|
||||
f 2/33/2 3/34/2 1/35/2
|
||||
f 4/36/3 5/37/3 3/34/3
|
||||
f 6/38/4 7/39/4 5/37/4
|
||||
f 8/40/5 9/41/5 7/39/5
|
||||
f 10/42/6 11/43/6 9/41/6
|
||||
f 12/44/7 13/45/7 11/43/7
|
||||
f 14/46/8 15/47/8 13/45/8
|
||||
f 16/48/9 17/49/9 15/47/9
|
||||
f 18/50/10 19/51/10 17/49/10
|
||||
f 20/52/11 21/53/11 19/51/11
|
||||
f 22/54/12 23/55/12 21/53/12
|
||||
f 24/56/13 25/57/13 23/55/13
|
||||
f 26/58/14 27/59/14 25/57/14
|
||||
f 28/60/15 29/61/15 27/59/15
|
||||
f 30/62/16 31/63/16 29/61/16
|
||||
f 32/64/17 33/65/17 31/63/17
|
||||
f 34/66/18 35/67/18 33/65/18
|
||||
f 36/68/19 37/69/19 35/67/19
|
||||
f 38/70/20 39/71/20 37/69/20
|
||||
f 40/72/21 41/73/21 39/71/21
|
||||
f 42/74/22 43/75/22 41/73/22
|
||||
f 44/76/23 45/77/23 43/75/23
|
||||
f 46/78/24 47/79/24 45/77/24
|
||||
f 48/80/25 49/81/25 47/79/25
|
||||
f 50/82/26 51/83/26 49/81/26
|
||||
f 52/84/27 53/85/27 51/83/27
|
||||
f 54/86/28 55/87/28 53/85/28
|
||||
f 56/88/29 57/89/29 55/87/29
|
||||
f 58/90/30 59/91/30 57/89/30
|
||||
f 60/92/31 61/93/31 59/91/31
|
||||
f 62/94/32 63/95/32 61/93/32
|
||||
f 64/96/33 1/35/33 63/95/33
|
||||
f 31/63/34 55/87/34 63/95/34
|
||||
f 2/33/2 4/36/2 3/34/2
|
||||
f 4/36/3 6/38/3 5/37/3
|
||||
f 6/38/4 8/40/4 7/39/4
|
||||
f 8/40/5 10/42/5 9/41/5
|
||||
f 10/42/6 12/44/6 11/43/6
|
||||
f 12/44/7 14/46/7 13/45/7
|
||||
f 14/46/8 16/48/8 15/47/8
|
||||
f 16/48/9 18/50/9 17/49/9
|
||||
f 18/50/10 20/52/10 19/51/10
|
||||
f 20/52/11 22/54/11 21/53/11
|
||||
f 22/54/12 24/56/12 23/55/12
|
||||
f 24/56/13 26/58/13 25/57/13
|
||||
f 26/58/14 28/60/14 27/59/14
|
||||
f 28/60/15 30/62/15 29/61/15
|
||||
f 30/62/16 32/64/16 31/63/16
|
||||
f 32/64/17 34/66/17 33/65/17
|
||||
f 34/66/18 36/68/18 35/67/18
|
||||
f 36/68/19 38/70/19 37/69/19
|
||||
f 38/70/20 40/72/20 39/71/20
|
||||
f 40/72/21 42/74/21 41/73/21
|
||||
f 42/74/22 44/76/22 43/75/22
|
||||
f 44/76/23 46/78/23 45/77/23
|
||||
f 46/78/24 48/80/24 47/79/24
|
||||
f 48/80/25 50/82/25 49/81/25
|
||||
f 50/82/26 52/84/26 51/83/26
|
||||
f 52/84/27 54/86/27 53/85/27
|
||||
f 54/86/28 56/88/28 55/87/28
|
||||
f 56/88/29 58/90/29 57/89/29
|
||||
f 58/90/30 60/92/30 59/91/30
|
||||
f 60/92/31 62/94/31 61/93/31
|
||||
f 62/94/32 64/96/32 63/95/32
|
||||
f 64/96/33 2/33/33 1/35/33
|
||||
f 63/95/34 1/35/34 3/34/34
|
||||
f 3/34/34 5/37/34 7/39/34
|
||||
f 7/39/34 9/41/34 15/47/34
|
||||
f 11/43/34 13/45/34 15/47/34
|
||||
f 15/47/34 17/49/34 19/51/34
|
||||
f 19/51/34 21/53/34 23/55/34
|
||||
f 23/55/34 25/57/34 31/63/34
|
||||
f 27/59/34 29/61/34 31/63/34
|
||||
f 31/63/34 33/65/34 35/67/34
|
||||
f 35/67/34 37/69/34 31/63/34
|
||||
f 39/71/34 41/73/34 47/79/34
|
||||
f 43/75/34 45/77/34 47/79/34
|
||||
f 47/79/34 49/81/34 51/83/34
|
||||
f 51/83/34 53/85/34 55/87/34
|
||||
f 55/87/34 57/89/34 63/95/34
|
||||
f 59/91/34 61/93/34 63/95/34
|
||||
f 63/95/34 3/34/34 15/47/34
|
||||
f 9/41/34 11/43/34 15/47/34
|
||||
f 15/47/34 19/51/34 31/63/34
|
||||
f 25/57/34 27/59/34 31/63/34
|
||||
f 31/63/34 37/69/34 39/71/34
|
||||
f 41/73/34 43/75/34 47/79/34
|
||||
f 47/79/34 51/83/34 55/87/34
|
||||
f 57/89/34 59/91/34 63/95/34
|
||||
f 3/34/34 7/39/34 15/47/34
|
||||
f 19/51/34 23/55/34 31/63/34
|
||||
f 31/63/34 39/71/34 47/79/34
|
||||
f 47/79/34 55/87/34 31/63/34
|
||||
f 63/95/34 15/47/34 31/63/34
|
||||
@@ -2585,6 +2585,20 @@ int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHa
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
b3Assert(command);
|
||||
b3Assert(command->m_type == CMD_STATE_LOGGING);
|
||||
if (command->m_type == CMD_STATE_LOGGING)
|
||||
{
|
||||
command->m_updateFlags |= STATE_LOGGING_MAX_LOG_DOF;
|
||||
command->m_stateLoggingArguments.m_maxLogDof = maxLogDof;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
|
||||
@@ -356,9 +356,13 @@ void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3Keyboard
|
||||
b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient);
|
||||
int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
|
||||
int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
|
||||
int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof);
|
||||
|
||||
int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
|
||||
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);
|
||||
|
||||
|
||||
|
||||
void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
|
||||
double b3GetTimeOut(b3PhysicsClientHandle physClient);
|
||||
|
||||
|
||||
@@ -48,7 +48,7 @@ bool gResetSimulation = 0;
|
||||
int gVRTrackingObjectUniqueId = -1;
|
||||
btTransform gVRTrackingObjectTr = btTransform::getIdentity();
|
||||
|
||||
int gMaxNumCmdPer1ms = 10;//experiment: add some delay to avoid threads starving other threads
|
||||
int gMaxNumCmdPer1ms = -1;//experiment: add some delay to avoid threads starving other threads
|
||||
int gCreateObjectSimVR = -1;
|
||||
int gEnableKukaControl = 0;
|
||||
btVector3 gVRTeleportPos1(0,0,0);
|
||||
@@ -512,7 +512,8 @@ struct MinitaurStateLogger : public InternalStateLogger
|
||||
|
||||
MinitaurLogRecord logData;
|
||||
//'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo'
|
||||
btScalar motorDir[8] = {1, -1, 1, -1, -1, 1, -1, 1};
|
||||
btScalar motorDir[8] = {1, 1, 1, 1, 1, 1, 1, 1};
|
||||
|
||||
|
||||
btQuaternion orn = m_minitaurMultiBody->getBaseWorldTransform().getRotation();
|
||||
btMatrix3x3 mat(orn);
|
||||
@@ -568,12 +569,14 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
||||
btMultiBodyDynamicsWorld* m_dynamicsWorld;
|
||||
btAlignedObjectArray<int> m_bodyIdList;
|
||||
bool m_filterObjectUniqueId;
|
||||
|
||||
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld)
|
||||
int m_maxLogDof;
|
||||
|
||||
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof)
|
||||
:m_loggingTimeStamp(0),
|
||||
m_logFileHandle(0),
|
||||
m_dynamicsWorld(dynamicsWorld),
|
||||
m_filterObjectUniqueId(false)
|
||||
m_filterObjectUniqueId(false),
|
||||
m_maxLogDof(maxLogDof)
|
||||
{
|
||||
m_loggingType = STATE_LOGGING_GENERIC_ROBOT;
|
||||
|
||||
@@ -595,32 +598,24 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
||||
structNames.push_back("omegaY");
|
||||
structNames.push_back("omegaZ");
|
||||
structNames.push_back("qNum");
|
||||
structNames.push_back("q0");
|
||||
structNames.push_back("q1");
|
||||
structNames.push_back("q2");
|
||||
structNames.push_back("q3");
|
||||
structNames.push_back("q4");
|
||||
structNames.push_back("q5");
|
||||
structNames.push_back("q6");
|
||||
structNames.push_back("q7");
|
||||
structNames.push_back("q8");
|
||||
structNames.push_back("q9");
|
||||
structNames.push_back("q10");
|
||||
structNames.push_back("q11");
|
||||
structNames.push_back("u0");
|
||||
structNames.push_back("u1");
|
||||
structNames.push_back("u2");
|
||||
structNames.push_back("u3");
|
||||
structNames.push_back("u4");
|
||||
structNames.push_back("u5");
|
||||
structNames.push_back("u6");
|
||||
structNames.push_back("u7");
|
||||
structNames.push_back("u8");
|
||||
structNames.push_back("u9");
|
||||
structNames.push_back("u10");
|
||||
structNames.push_back("u11");
|
||||
|
||||
m_structTypes = "IfIfffffffffffffI";
|
||||
|
||||
for (int i=0;i<m_maxLogDof;i++)
|
||||
{
|
||||
m_structTypes.append("f");
|
||||
char jointName[256];
|
||||
sprintf(jointName,"q%d",i);
|
||||
structNames.push_back(jointName);
|
||||
}
|
||||
for (int i=0;i<m_maxLogDof;i++)
|
||||
{
|
||||
m_structTypes.append("f");
|
||||
char jointName[256];
|
||||
sprintf(jointName,"u%d",i);
|
||||
structNames.push_back(jointName);
|
||||
}
|
||||
|
||||
m_structTypes = "IfIfffffffffffffIffffffffffffffffffffffff";
|
||||
const char* fileNameC = fileName.c_str();
|
||||
|
||||
m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
|
||||
@@ -699,7 +694,7 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
||||
logData.m_values.push_back(q);
|
||||
}
|
||||
}
|
||||
for (int j = numDofs; j < 12; ++j)
|
||||
for (int j = numDofs; j < m_maxLogDof; ++j)
|
||||
{
|
||||
float q = 0.0;
|
||||
logData.m_values.push_back(q);
|
||||
@@ -713,7 +708,7 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
||||
logData.m_values.push_back(u);
|
||||
}
|
||||
}
|
||||
for (int j = numDofs; j < 12; ++j)
|
||||
for (int j = numDofs; j < m_maxLogDof; ++j)
|
||||
{
|
||||
float u = 0.0;
|
||||
logData.m_values.push_back(u);
|
||||
@@ -1860,14 +1855,20 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
|
||||
|
||||
int loggerUid = m_data->m_stateLoggersUniqueId++;
|
||||
GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld);
|
||||
int maxLogDof = 12;
|
||||
if ((clientCmd.m_updateFlags & STATE_LOGGING_MAX_LOG_DOF))
|
||||
{
|
||||
maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof;
|
||||
}
|
||||
GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld,maxLogDof);
|
||||
|
||||
if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0))
|
||||
{
|
||||
logger->m_filterObjectUniqueId = true;
|
||||
for (int i = 0; i < clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds; ++i)
|
||||
{
|
||||
logger->m_bodyIdList.push_back(clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i]);
|
||||
int objectUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i];
|
||||
logger->m_bodyIdList.push_back(objectUniqueId);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -313,12 +313,15 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
|
||||
b3Clock::usleep(0);
|
||||
}
|
||||
|
||||
if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms)
|
||||
if (gMaxNumCmdPer1ms>0)
|
||||
{
|
||||
BT_PROFILE("usleep(1000)");
|
||||
b3Clock::usleep(1000);
|
||||
numCmdSinceSleep1ms = 0;
|
||||
sleepClock.reset();
|
||||
if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms)
|
||||
{
|
||||
BT_PROFILE("usleep(10)");
|
||||
b3Clock::usleep(10);
|
||||
numCmdSinceSleep1ms = 0;
|
||||
sleepClock.reset();
|
||||
}
|
||||
}
|
||||
if (sleepClock.getTimeMilliseconds()>1)
|
||||
{
|
||||
|
||||
@@ -626,6 +626,7 @@ enum eStateLoggingEnums
|
||||
STATE_LOGGING_START_LOG=1,
|
||||
STATE_LOGGING_STOP_LOG=2,
|
||||
STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID=4,
|
||||
STATE_LOGGING_MAX_LOG_DOF=8
|
||||
};
|
||||
|
||||
struct VRCameraState
|
||||
@@ -644,6 +645,7 @@ struct StateLoggingRequest
|
||||
int m_numBodyUniqueIds;////only if ROBOT_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set
|
||||
int m_bodyUniqueIds[MAX_SDF_BODIES];
|
||||
int m_loggingUniqueId;
|
||||
int m_maxLogDof;
|
||||
};
|
||||
|
||||
struct StateLoggingResultArgs
|
||||
|
||||
@@ -16,8 +16,8 @@ def readLogFile(filename, verbose = True):
|
||||
print('Opened'),
|
||||
print(filename)
|
||||
|
||||
keys = f.readline().rstrip('\n').split(',')
|
||||
fmt = f.readline().rstrip('\n')
|
||||
keys = f.readline().decode('utf8').rstrip('\n').split(',')
|
||||
fmt = f.readline().decode('utf8').rstrip('\n')
|
||||
|
||||
# The byte number of one record
|
||||
sz = struct.calcsize(fmt)
|
||||
@@ -37,7 +37,7 @@ def readLogFile(filename, verbose = True):
|
||||
# Read data
|
||||
wholeFile = f.read()
|
||||
# split by alignment word
|
||||
chunks = wholeFile.split('\xaa\xbb')
|
||||
chunks = wholeFile.split(b'\xaa\xbb')
|
||||
log = list()
|
||||
for chunk in chunks:
|
||||
if len(chunk) == sz:
|
||||
|
||||
@@ -18,7 +18,7 @@ def current_position():
|
||||
def is_fallen():
|
||||
global minitaur
|
||||
orientation = minitaur.getBaseOrientation()
|
||||
rotMat = p.getMatrixFromQuaterion(orientation)
|
||||
rotMat = p.getMatrixFromQuaternion(orientation)
|
||||
localUp = rotMat[6:]
|
||||
return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0
|
||||
|
||||
|
||||
@@ -606,7 +606,7 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
|
||||
int numSubSteps = -1;
|
||||
int collisionFilterMode = -1;
|
||||
double contactBreakingThreshold = -1;
|
||||
int maxNumCmdPer1ms = -1;
|
||||
int maxNumCmdPer1ms = -2;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
|
||||
int physicsClientId = 0;
|
||||
@@ -658,7 +658,8 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
|
||||
{
|
||||
b3PhysicsParamSetContactBreakingThreshold(command,contactBreakingThreshold);
|
||||
}
|
||||
if (maxNumCmdPer1ms>=0)
|
||||
//-1 is disables the maxNumCmdPer1ms feature, allow it
|
||||
if (maxNumCmdPer1ms>=-1)
|
||||
{
|
||||
b3PhysicsParamSetMaxNumCommandsPer1ms(command,maxNumCmdPer1ms);
|
||||
}
|
||||
@@ -2624,12 +2625,13 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
|
||||
int loggingType = -1;
|
||||
char* fileName = 0;
|
||||
PyObject* objectUniqueIdsObj = 0;
|
||||
int maxLogDof=-1;
|
||||
|
||||
static char *kwlist[] = { "loggingType", "fileName", "objectUniqueIds", "physicsClientId", NULL };
|
||||
static char *kwlist[] = { "loggingType", "fileName", "objectUniqueIds", "maxLogDof", "physicsClientId", NULL };
|
||||
int physicsClientId = 0;
|
||||
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oi", kwlist,
|
||||
&loggingType, &fileName, &objectUniqueIdsObj,&physicsClientId))
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oii", kwlist,
|
||||
&loggingType, &fileName, &objectUniqueIdsObj,&maxLogDof, &physicsClientId))
|
||||
return NULL;
|
||||
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
@@ -2660,6 +2662,10 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
|
||||
}
|
||||
}
|
||||
|
||||
if (maxLogDof>0)
|
||||
{
|
||||
b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
|
||||
}
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
@@ -4,99 +4,124 @@ import math
|
||||
|
||||
useRealTime = 0
|
||||
fixedTimeStep = 0.001
|
||||
speed = 10
|
||||
amplitude = 0.8
|
||||
jump_amp = 0.5
|
||||
maxForce = 3.5
|
||||
kneeFrictionForce = 0.00
|
||||
kp = .05
|
||||
kd = .5
|
||||
|
||||
|
||||
physId = p.connect(p.SHARED_MEMORY)
|
||||
if (physId<0):
|
||||
p.connect(p.GUI)
|
||||
|
||||
p.loadURDF("plane.urdf")
|
||||
p.setGravity(0,0,-1)
|
||||
#p.resetSimulation()
|
||||
p.loadURDF("plane.urdf",0,0,0.1)
|
||||
#p.loadSDF("kitchens/1.sdf")
|
||||
p.setGravity(0,0,0)
|
||||
p.setTimeStep(fixedTimeStep)
|
||||
|
||||
p.setRealTimeSimulation(0)
|
||||
quadruped = p.loadURDF("quadruped/quadruped.urdf",1,-2,1)
|
||||
quadruped = p.loadURDF("quadruped/minitaur.urdf",[0,0,0.2],useFixedBase=False)
|
||||
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']
|
||||
hip_front_rightR_link = jointNameToId['hip_front_rightR_link']
|
||||
knee_front_rightR_link = jointNameToId['knee_front_rightR_link']
|
||||
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
|
||||
motor_front_rightL_link = jointNameToId['motor_front_rightL_link']
|
||||
knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
|
||||
motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
|
||||
hip_front_leftR_link = jointNameToId['hip_front_leftR_link']
|
||||
knee_front_leftR_link = jointNameToId['knee_front_leftR_link']
|
||||
motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
|
||||
motor_front_leftL_link = jointNameToId['motor_front_leftL_link']
|
||||
knee_front_leftL_link = jointNameToId['knee_front_leftL_link']
|
||||
motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint']
|
||||
hip_rightR_link = jointNameToId['hip_rightR_link']
|
||||
knee_back_rightR_link = jointNameToId['knee_back_rightR_link']
|
||||
motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
|
||||
motor_back_rightL_link = jointNameToId['motor_back_rightL_link']
|
||||
knee_back_rightL_link = jointNameToId['knee_back_rightL_link']
|
||||
motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint']
|
||||
hip_leftR_link = jointNameToId['hip_leftR_link']
|
||||
knee_back_leftR_link = jointNameToId['knee_back_leftR_link']
|
||||
motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
|
||||
motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
|
||||
knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
|
||||
|
||||
|
||||
#p.getNumJoints(1)
|
||||
#right front leg
|
||||
p.resetJointState(quadruped,0,1.57)
|
||||
p.resetJointState(quadruped,2,-2.2)
|
||||
p.resetJointState(quadruped,3,-1.57)
|
||||
p.resetJointState(quadruped,5,2.2)
|
||||
p.createConstraint(quadruped,2,quadruped,5,p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
|
||||
p.resetJointState(quadruped,motor_front_rightR_joint,1.57)
|
||||
p.resetJointState(quadruped,knee_front_rightR_link,-2.2)
|
||||
p.resetJointState(quadruped,motor_front_rightL_joint,1.57)
|
||||
p.resetJointState(quadruped,knee_front_rightL_link,-2.2)
|
||||
p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,1.57,1)
|
||||
p.setJointMotorControl(quadruped,1,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,2,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1)
|
||||
p.setJointMotorControl(quadruped,4,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,5,p.VELOCITY_CONTROL,0,0)
|
||||
p.resetJointState(quadruped,motor_front_leftR_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_front_leftR_link,2.2)
|
||||
p.resetJointState(quadruped,motor_front_leftL_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_front_leftL_link,2.2)
|
||||
p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
#left front leg
|
||||
p.resetJointState(quadruped,6,1.57)
|
||||
p.resetJointState(quadruped,8,-2.2)
|
||||
p.resetJointState(quadruped,9,-1.57)
|
||||
p.resetJointState(quadruped,11,2.2)
|
||||
p.createConstraint(quadruped,8,quadruped,11,p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
|
||||
p.resetJointState(quadruped,motor_back_rightR_joint,1.57)
|
||||
p.resetJointState(quadruped,knee_back_rightR_link,-2.2)
|
||||
p.resetJointState(quadruped,motor_back_rightL_joint,1.57)
|
||||
p.resetJointState(quadruped,knee_back_rightL_link,-2.2)
|
||||
p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,1.57,1)
|
||||
p.setJointMotorControl(quadruped,7,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,8,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-1.57,1)
|
||||
p.setJointMotorControl(quadruped,10,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,11,p.VELOCITY_CONTROL,0,0)
|
||||
|
||||
|
||||
#right back leg
|
||||
p.resetJointState(quadruped,12,1.57)
|
||||
p.resetJointState(quadruped,14,-2.2)
|
||||
p.resetJointState(quadruped,15,-1.57)
|
||||
p.resetJointState(quadruped,17,2.2)
|
||||
p.createConstraint(quadruped,14,quadruped,17,p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
|
||||
|
||||
p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,1.57,1)
|
||||
p.setJointMotorControl(quadruped,13,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,14,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,1)
|
||||
p.setJointMotorControl(quadruped,16,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,17,p.VELOCITY_CONTROL,0,0)
|
||||
|
||||
#left back leg
|
||||
p.resetJointState(quadruped,18,1.57)
|
||||
p.resetJointState(quadruped,20,-2.2)
|
||||
p.resetJointState(quadruped,21,-1.57)
|
||||
p.resetJointState(quadruped,23,2.2)
|
||||
p.createConstraint(quadruped,20,quadruped,23,p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
|
||||
|
||||
p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,1.57,1)
|
||||
p.setJointMotorControl(quadruped,19,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,20,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1)
|
||||
p.setJointMotorControl(quadruped,22,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0)
|
||||
p.resetJointState(quadruped,motor_back_leftR_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_back_leftR_link,2.2)
|
||||
p.resetJointState(quadruped,motor_back_leftL_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_back_leftL_link,2.2)
|
||||
p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.setJointMotorControl(quadruped,motor_back_leftR_joint,p.POSITION_CONTROL,-1.57,maxForce)
|
||||
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,motor_back_leftL_joint,p.POSITION_CONTROL,-1.57,maxForce)
|
||||
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
|
||||
|
||||
p_gain =1
|
||||
speed = 10
|
||||
amplitude = 1.3
|
||||
motordir=[-1,-1,-1,-1,1,1,1,1]
|
||||
legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint]
|
||||
|
||||
#stand still
|
||||
p.setRealTimeSimulation(useRealTime)
|
||||
#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)
|
||||
|
||||
|
||||
t=0.0
|
||||
ref_time = time.time()
|
||||
t_end = t + 4
|
||||
while t < t_end:
|
||||
if (useRealTime==0):
|
||||
t = t+fixedTimeStep
|
||||
p.stepSimulation()
|
||||
else:
|
||||
t = time.time()-ref_time
|
||||
p.setGravity(0,0,-1)
|
||||
p.stepSimulation()
|
||||
|
||||
|
||||
print("quadruped Id = ")
|
||||
print(quadruped)
|
||||
p.saveWorld("quadru.py")
|
||||
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.txt",[quadruped])
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
|
||||
jump_amp = 0.5
|
||||
|
||||
#stand still
|
||||
p.setRealTimeSimulation(1)
|
||||
|
||||
#jump
|
||||
t = 0.0
|
||||
@@ -110,55 +135,25 @@ while t < t_end:
|
||||
else:
|
||||
t = t+fixedTimeStep
|
||||
|
||||
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
|
||||
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()
|
||||
|
||||
|
||||
#hop forward
|
||||
t_end = 20
|
||||
i=0
|
||||
while t < t_end:
|
||||
if (useRealTime):
|
||||
t = time.time()-ref_time
|
||||
else:
|
||||
t = t+fixedTimeStep
|
||||
|
||||
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*speed)*amplitude+1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*speed)*amplitude+1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,math.sin(t*speed+3.14)*amplitude+1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,math.sin(t*speed+3.14)*amplitude+1.57,p_gain)
|
||||
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,p_gain)
|
||||
if (useRealTime==0):
|
||||
p.stepSimulation()
|
||||
|
||||
#walk
|
||||
t_end = 100
|
||||
i=0
|
||||
while t < t_end:
|
||||
if (useRealTime):
|
||||
t = time.time()-ref_time
|
||||
else:
|
||||
t = t+fixedTimeStep
|
||||
|
||||
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*3)*.3+1.57,1)
|
||||
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1)
|
||||
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*3+0.5*3.14)*.3+1.57,1)
|
||||
p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-1.57,1)
|
||||
p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,math.sin(t*3+3.14)*.3+1.57,1)
|
||||
p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,1)
|
||||
p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,math.sin(t*3+1.5*3.14)*.3+1.57,1)
|
||||
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1)
|
||||
|
||||
p.stepSimulation()
|
||||
p.setRealTimeSimulation(1)
|
||||
|
||||
126
examples/pybullet/quadruped_playback.py
Normal file
126
examples/pybullet/quadruped_playback.py
Normal file
@@ -0,0 +1,126 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
import math
|
||||
from datetime import datetime
|
||||
from numpy import *
|
||||
from pylab import *
|
||||
import struct
|
||||
import sys
|
||||
import os, fnmatch
|
||||
import argparse
|
||||
from time import sleep
|
||||
|
||||
def readLogFile(filename, verbose = True):
|
||||
f = open(filename, 'rb')
|
||||
|
||||
print('Opened'),
|
||||
print(filename)
|
||||
|
||||
keys = f.readline().decode('utf8').rstrip('\n').split(',')
|
||||
fmt = f.readline().decode('utf8').rstrip('\n')
|
||||
|
||||
# The byte number of one record
|
||||
sz = struct.calcsize(fmt)
|
||||
# The type number of one record
|
||||
ncols = len(fmt)
|
||||
|
||||
if verbose:
|
||||
print('Keys:'),
|
||||
print(keys)
|
||||
print('Format:'),
|
||||
print(fmt)
|
||||
print('Size:'),
|
||||
print(sz)
|
||||
print('Columns:'),
|
||||
print(ncols)
|
||||
|
||||
# Read data
|
||||
wholeFile = f.read()
|
||||
# split by alignment word
|
||||
chunks = wholeFile.split(b'\xaa\xbb')
|
||||
print ("num chunks")
|
||||
print (len(chunks))
|
||||
|
||||
log = list()
|
||||
for chunk in chunks:
|
||||
if len(chunk) == sz:
|
||||
values = struct.unpack(fmt, chunk)
|
||||
record = list()
|
||||
for i in range(ncols):
|
||||
record.append(values[i])
|
||||
log.append(record)
|
||||
|
||||
return log
|
||||
|
||||
clid = p.connect(p.SHARED_MEMORY)
|
||||
|
||||
log = readLogFile("LOG00076.TXT");
|
||||
|
||||
recordNum = len(log)
|
||||
print('record num:'),
|
||||
print(recordNum)
|
||||
itemNum = len(log[0])
|
||||
print('item num:'),
|
||||
print(itemNum)
|
||||
|
||||
useRealTime = 0
|
||||
fixedTimeStep = 0.001
|
||||
speed = 10
|
||||
amplitude = 0.8
|
||||
jump_amp = 0.5
|
||||
maxForce = 3.5
|
||||
kp = .05
|
||||
kd = .5
|
||||
|
||||
|
||||
|
||||
quadruped = 1
|
||||
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']
|
||||
hip_front_rightR_link = jointNameToId['hip_front_rightR_link']
|
||||
knee_front_rightR_link = jointNameToId['knee_front_rightR_link']
|
||||
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
|
||||
motor_front_rightL_link = jointNameToId['motor_front_rightL_link']
|
||||
knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
|
||||
motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
|
||||
hip_front_leftR_link = jointNameToId['hip_front_leftR_link']
|
||||
knee_front_leftR_link = jointNameToId['knee_front_leftR_link']
|
||||
motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
|
||||
motor_front_leftL_link = jointNameToId['motor_front_leftL_link']
|
||||
knee_front_leftL_link = jointNameToId['knee_front_leftL_link']
|
||||
motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint']
|
||||
hip_rightR_link = jointNameToId['hip_rightR_link']
|
||||
knee_back_rightR_link = jointNameToId['knee_back_rightR_link']
|
||||
motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
|
||||
motor_back_rightL_link = jointNameToId['motor_back_rightL_link']
|
||||
knee_back_rightL_link = jointNameToId['knee_back_rightL_link']
|
||||
motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint']
|
||||
hip_leftR_link = jointNameToId['hip_leftR_link']
|
||||
knee_back_leftR_link = jointNameToId['knee_back_leftR_link']
|
||||
motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
|
||||
motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
|
||||
knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
|
||||
|
||||
motorDir = [1, 1, 1, 1, 1, 1, 1, 1];
|
||||
legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint]
|
||||
|
||||
|
||||
for record in log:
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[0], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[0]*record[7], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[1], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[1]*record[8], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[2], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[2]*record[9], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[3], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[3]*record[10], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[4], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[4]*record[11], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[5], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[5]*record[12], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[6], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[6]*record[13], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[7], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[7]*record[14], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setGravity(0.000000,0.000000,-10.000000)
|
||||
p.stepSimulation()
|
||||
p.stepSimulation()
|
||||
sleep(0.01)
|
||||
|
||||
21
examples/pybullet/quadruped_setup_playback.py
Normal file
21
examples/pybullet/quadruped_setup_playback.py
Normal file
@@ -0,0 +1,21 @@
|
||||
import pybullet as p
|
||||
p.connect(p.SHARED_MEMORY)
|
||||
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,-.300000,0.000000,0.000000,0.000000,1.000000)]
|
||||
objects = [p.loadURDF("quadruped/minitaur.urdf", [-0.000046,-0.000068,0.200774],[-0.000701,0.000387,-0.000252,1.000000],useFixedBase=False)]
|
||||
ob = objects[0]
|
||||
jointPositions=[ 0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000, -2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193, 0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517 ]
|
||||
for ji in range (p.getNumJoints(ob)):
|
||||
p.resetJointState(ob,ji,jointPositions[ji])
|
||||
p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0)
|
||||
|
||||
cid0 = p.createConstraint(1,3,1,6,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid0,maxForce=500.000000)
|
||||
cid1 = p.createConstraint(1,16,1,19,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid1,maxForce=500.000000)
|
||||
cid2 = p.createConstraint(1,9,1,12,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid2,maxForce=500.000000)
|
||||
cid3 = p.createConstraint(1,22,1,25,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid3,maxForce=500.000000)
|
||||
p.setGravity(0.000000,0.000000,0.000000)
|
||||
p.stepSimulation()
|
||||
p.disconnect()
|
||||
Reference in New Issue
Block a user