diff options
Diffstat (limited to 'examples')
42 files changed, 9464 insertions, 0 deletions
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/minitaur_derpy.urdf b/examples/pybullet/gym/pybullet_data/quadruped/minitaur_derpy.urdf new file mode 100644 index 000000000..a5fbcf033 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/quadruped/minitaur_derpy.urdf @@ -0,0 +1,1073 @@ +<?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=".295 0.142 .078"/> + </geometry> + <material name="black"> + <color rgba="0.3 0.3 0.3 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".295 0.142 .078"/> + </geometry> + </collision> + <inertial> + <origin rpy="0.0 0 0" xyz="0.05 0 0"/> + <mass value="1.81246"/> + <inertia ixx="0.00474833" ixy="0.0" ixz="0.0" iyy="0.01386899" iyz="0.0" izz="0.01577876"/> + </inertial> + </link> + + <link name="chassis_right"> + <visual> + <origin rpy="0 0 0" xyz="0 0 0"/> + <geometry> + <box size=".156 0.091 .050"/> + </geometry> + <material name="black"> + <color rgba="0.3 0.3 0.3 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="0 0.0455 0"/> + <geometry> + <box size=".356 0.009 .050"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="0 -0.0455 0"/> + <geometry> + <box size=".356 0.009 .050"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="0.212 -0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="0.212 0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="-0.212 -0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="-0.212 0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + + + <collision> + <origin rpy="0 0 0" xyz="0 0.1165 0"/> + <geometry> + <box size=".156 0.091 .050"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="0 0.0455 0"/> + <geometry> + <box size=".356 0.009 .050"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="0 -0.0455 0"/> + <geometry> + <box size=".356 0.009 .050"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="0.212 -0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="0.212 0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="-0.212 -0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="-0.212 0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + </collision> + + <inertial> + <mass value="1.3668"/> + <inertia ixx="0.0038077" ixy="0.0" ixz="0.0" iyy="0.0328502" iyz="0.0" izz="0.03575585"/> + </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.1165 -0.011"/> + <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 0"/> + <geometry> + <box size=".156 0.091 .050"/> + </geometry> + <material name="black"> + <color rgba="0.3 0.3 0.3 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="0 0.0455 0"/> + <geometry> + <box size=".356 0.009 .050"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="0 -0.0455 0"/> + <geometry> + <box size=".356 0.009 .050"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="0.212 -0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="0.212 0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="-0.212 -0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="-0.212 0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + + + <collision> + <origin rpy="0 0 0" xyz="0 0.1165 0"/> + <geometry> + <box size=".156 0.091 .050"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="0 0.0455 0"/> + <geometry> + <box size=".356 0.009 .050"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="0 -0.0455 0"/> + <geometry> + <box size=".356 0.009 .050"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="0.212 -0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="0.212 0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="-0.212 -0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="-0.212 0.05915 0"/> + <geometry> + <box size=".068 0.0373 .060"/> + </geometry> + </collision> + + <inertial> + <mass value="1.3668"/> + <inertia ixx="0.0038077" ixy="0.0" ixz="0.0" iyy="0.0328502" iyz="0.0" izz="0.03575585"/> + </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.1165 -0.011"/> + <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.0165" radius="0.0425"/> + </geometry> + </collision> + <inertial> + <mass value="0.241"/> + <inertia ixx="0.00011397" ixy="0.0" ixz="0.0" iyy="0.00011397" iyz="0.0" izz="0.00021765"/> + </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.20268 -0.03275 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.0165" radius="0.0425"/> + </geometry> + </collision> + <inertial> + <mass value="0.241"/> + <inertia ixx="0.00011397" ixy="0.0" ixz="0.0" iyy="0.00011397" iyz="0.0" izz="0.00021765"/> + </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.20268 0.03275 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> + <collision> + <geometry> + <cylinder length="0.0165" radius="0.0425"/> + </geometry> + </collision> + <inertial> + <mass value="0.241"/> + <inertia ixx="0.00011397" ixy="0.0" ixz="0.0" iyy="0.00011397" iyz="0.0" izz="0.00021765"/> + </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.20268 0.03275 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.0165" radius="0.0425"/> + </geometry> + </collision> + <inertial> + <mass value="0.241"/> + <inertia ixx="0.00011397" ixy="0.0" ixz="0.0" iyy="0.00011397" iyz="0.0" izz="0.00021765"/> + </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.20268 -0.03275 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.0165" radius="0.0425"/> + </geometry> + </collision> + <inertial> + <mass value="0.241"/> + <inertia ixx="0.00011397" ixy="0.0" ixz="0.0" iyy="0.00011397" iyz="0.0" izz="0.00021765"/> + </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.20268 -0.03275 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.0165" radius="0.0425"/> + </geometry> + </collision> + <inertial> + <mass value="0.241"/> + <inertia ixx="0.00011397" ixy="0.0" ixz="0.0" iyy="0.00011397" iyz="0.0" izz="0.00021765"/> + </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.20268 0.03275 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.0165" radius="0.0425"/> + </geometry> + </collision> + <inertial> + <mass value="0.241"/> + <inertia ixx="0.00011397" ixy="0.0" ixz="0.0" iyy="0.00011397" iyz="0.0" izz="0.00021765"/> + </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.20268 0.03275 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.0165" radius="0.0425"/> + </geometry> + </collision> + <inertial> + <mass value="0.241"/> + <inertia ixx="0.00011397" ixy="0.0" ixz="0.0" iyy="0.00011397" iyz="0.0" izz="0.00021765"/> + </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.20268 -0.03275 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=".039 0.0064 .129"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 0.0064 .129"/> + </geometry> + </collision> + <inertial> + <mass value="0.026"/> + <inertia ixx="0.000038770" ixy="0.0" ixz="0.0" iyy="0.000042198" iyz="0.0" + izz="0.0000036030"/> + </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.045 -0.01725"/> + <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 0.120"/> + <geometry> + <box size="0.017 0.0064 0.240"/> + </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 0.120"/> + <geometry> + <box size="0.017 0.0064 0.240"/> + </geometry> + </collision> + <inertial> + <origin rpy="0.0 0 0" xyz="0 0 0.120"/> + <mass value="0.066"/> + <inertia ixx="0.00032459" ixy="0.0" ixz="0.0" iyy="0.00032637" iyz="0.0" izz="0.000002178"/> + </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 .056"/> + <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=".039 0.0064 .129"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 0.0064 .129"/> + </geometry> + </collision> + <inertial> + <mass value="0.026"/> + <inertia ixx="0.000038770" ixy="0.0" ixz="0.0" iyy="0.000042198" iyz="0.0" + izz="0.0000036030"/> + </inertial> + </link> + <joint name="hip_front_rightL_joint" 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.045 -0.01725"/> + <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 .108"/> + <geometry> + <box size="0.017 0.0064 0.216"/> + </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 .108"/> + <geometry> + <box size="0.017 0.0064 0.216"/> + </geometry> + </collision> + <inertial> + <origin rpy="0.0 0 0" xyz="0 0 .1"/> + <mass value="0.055"/> + <inertia ixx="0.00023166" ixy="0.0" ixz="0.0" iyy="0.00023325" iyz="0.0" izz="0.00000198"/> + </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 .056"/> + <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=".039 0.0064 .129"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 0.0064 .129"/> + </geometry> + </collision> + <inertial> + <mass value="0.026"/> + <inertia ixx="0.000038770" ixy="0.0" ixz="0.0" iyy="0.000042198" iyz="0.0" + izz="0.0000036030"/> + </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.045 -0.01725"/> + <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" xyz="0 0 .108"/> + <geometry> + <box size="0.017 0.0064 0.216"/> + </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 .108"/> + <geometry> + <box size="0.017 0.0064 0.216"/> + </geometry> + </collision> + <inertial> + <origin rpy="0.0 0 0" xyz="0 0 .1"/> + <mass value="0.055"/> + <inertia ixx="0.00023166" ixy="0.0" ixz="0.0" iyy="0.00023325" iyz="0.0" izz="0.00000198"/> + </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 .056"/> + <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=".039 0.0064 .129"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 0.0064 .129"/> + </geometry> + </collision> + <inertial> + <mass value="0.026"/> + <inertia ixx="0.000038770" ixy="0.0" ixz="0.0" iyy="0.000042198" iyz="0.0" + izz="0.0000036030"/> + </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.045 -0.01725"/> + <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" xyz="0 0 0.120"/> + <geometry> + <box size="0.017 0.0064 0.240"/> + </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 0.120"/> + <geometry> + <box size="0.017 0.0064 0.240"/> + </geometry> + </collision> + <inertial> + <origin rpy="0.0 0 0" xyz="0 0 0.120"/> + <mass value="0.066"/> + <inertia ixx="0.00032459" ixy="0.0" ixz="0.0" iyy="0.00032637" iyz="0.0" izz="0.000002178"/> + </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 .056"/> + <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=".039 0.0064 .129"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 0.0064 .129"/> + </geometry> + </collision> + <inertial> + <mass value="0.026"/> + <inertia ixx="0.000038770" ixy="0.0" ixz="0.0" iyy="0.000042198" iyz="0.0" + izz="0.0000036030"/> + </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.045 -0.01725"/> + <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 0.120"/> + <geometry> + <box size="0.017 0.0064 0.240"/> + </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 0.120"/> + <geometry> + <box size="0.017 0.0064 0.240"/> + </geometry> + </collision> + <inertial> + <origin rpy="0.0 0 0" xyz="0 0 0.120"/> + <mass value="0.066"/> + <inertia ixx="0.00032459" ixy="0.0" ixz="0.0" iyy="0.00032637" iyz="0.0" izz="0.000002178"/> + </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 .056"/> + <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=".039 0.0064 .129"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 0.0064 .129"/> + </geometry> + </collision> + <inertial> + <mass value="0.026"/> + <inertia ixx="0.000038770" ixy="0.0" ixz="0.0" iyy="0.000042198" iyz="0.0" + izz="0.0000036030"/> + </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.045 -0.01725"/> + <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 .108"/> + <geometry> + <box size="0.017 0.0064 0.216"/> + </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 .108"/> + <geometry> + <box size="0.017 0.0064 0.216"/> + </geometry> + </collision> + <inertial> + <origin rpy="0.0 0 0" xyz="0 0 .1"/> + <mass value="0.055"/> + <inertia ixx="0.00023166" ixy="0.0" ixz="0.0" iyy="0.00023325" iyz="0.0" izz="0.00000198"/> + </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 .056"/> + <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=".039 0.0064 .129"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 0.0064 .129"/> + </geometry> + </collision> + <inertial> + <mass value="0.026"/> + <inertia ixx="0.000038770" ixy="0.0" ixz="0.0" iyy="0.000042198" iyz="0.0" + izz="0.0000036030"/> + </inertial> + </link> + <joint name="hip_back_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.045 -0.01725"/> + <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 .108"/> + <geometry> + <box size="0.017 0.0064 0.216"/> + </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 .108"/> + <geometry> + <box size="0.017 0.0064 0.216"/> + </geometry> + </collision> + <inertial> + <origin rpy="0.0 0 0" xyz="0 0 .1"/> + <mass value="0.055"/> + <inertia ixx="0.00023166" ixy="0.0" ixz="0.0" iyy="0.00023325" iyz="0.0" izz="0.00000198"/> + </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 .056"/> + <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=".039 0.0064 .129"/> + </geometry> + <material name="grey"> + <color rgba="0.65 0.65 0.75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 0.0064 .129"/> + </geometry> + </collision> + <inertial> + <mass value="0.026"/> + <inertia ixx="0.000038770" ixy="0.0" ixz="0.0" iyy="0.000042198" iyz="0.0" + izz="0.0000036030"/> + </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.045 -0.01725"/> + <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 0.120"/> + <geometry> + <box size="0.017 0.0064 0.240"/> + </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 0.120"/> + <geometry> + <box size="0.017 0.0064 0.240"/> + </geometry> + </collision> + <inertial> + <origin rpy="0.0 0 0" xyz="0 0 0.120"/> + <mass value="0.066"/> + <inertia ixx="0.00032459" ixy="0.0" ixz="0.0" iyy="0.00032637" iyz="0.0" izz="0.000002178"/> + </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 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping="0.0" friction="0.0"/> + </joint> + +</robot> diff --git a/examples/pybullet/gym/pybullet_data/quadruped/minitaur_rainbow_dash.urdf b/examples/pybullet/gym/pybullet_data/quadruped/minitaur_rainbow_dash.urdf new file mode 100644 index 000000000..744f086a0 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/quadruped/minitaur_rainbow_dash.urdf @@ -0,0 +1,914 @@ +<?xml version="1.0"?> +<robot name="quadruped"> + <link name="base_chassis_link"> + <visual> + <geometry> + <box size=".3 .13 .087"/> + </geometry> + <material name="black"> + <color rgba=".3 .3 .3 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz=".10 0 .08"/> + <geometry> + <box size=".08 .08 .08"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".3 .13 .087"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz=".10 0 .08"/> + <geometry> + <box size=".08 .08 .08"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz=".05 0 0"/> + <mass value="3.353"/> + <inertia ixx=".006837" ixy=".0" ixz=".0" iyy=".027262" iyz=".0" izz=".029870"/> + </inertial> + </link> + <link name="chassis_right"> + <visual> + <origin rpy="0 0 0" xyz=".2375 -.054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz=".2375 .054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="-.2375 -.054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="-.2375 .054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz=".2375 -.054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz=".2375 .054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="-.2375 -.054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="-.2375 .054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + <inertial> + <mass value="1.32"/> + <inertia ixx=".004147" ixy=".0" ixz=".0" iyy=".040913" iyz=".0" izz=".041840"/> + </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 0 0" xyz="0 -.1265 -.0185"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="chassis_left"> + <visual> + <origin rpy="0 0 0" xyz=".2375 -.054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz=".2375 .054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="-.2375 -.054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <visual> + <origin rpy="0 0 0" xyz="-.2375 .054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz=".2375 -.054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz=".2375 .054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="-.2375 -.054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="-.2375 .054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + <inertial> + <mass value="1.32"/> + <inertia ixx=".004147" ixy=".0" ixz=".0" iyy=".040913" iyz=".0" izz=".041840"/> + </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 0 0" xyz="0 .1265 -.0185"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".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=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".072"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </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=".2375 -.0275 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".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=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".072"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </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=".2375 .0275 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".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> + <collision> + <geometry> + <cylinder length=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".072"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </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=".2375 .0275 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".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=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".072"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </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=".2375 -.0275 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".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=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".072"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </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="-.2375 -.0275 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".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=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".072"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </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="-.2375 .0275 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".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=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".072"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </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="-.2375 .0275 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".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=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".072"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </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="-.2375 -.0275 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_front_rightR_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_front_rightR_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".3"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <mass value=".086"/> + <inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_front_rightL_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </inertial> + </link> + <joint name="hip_front_rightL_joint" 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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_front_rightL_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".3"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .1"/> + <mass value=".072"/> + <inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_front_leftR_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_front_leftR_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".3"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .1"/> + <mass value=".072"/> + <inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_front_leftL_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_front_leftL_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".3"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <mass value=".086"/> + <inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_back_rightR_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_back_rightR_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".3"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <mass value=".086"/> + <inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_back_rightL_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_back_rightL_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".3"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .1"/> + <mass value=".072"/> + <inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_back_leftR_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </inertial> + </link> + <joint name="hip_back_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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_back_leftR_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".3"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .1"/> + <mass value=".072"/> + <inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_back_leftL_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_back_leftL_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".3"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <mass value=".086"/> + <inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> +</robot> diff --git a/examples/pybullet/gym/pybullet_data/quadruped/minitaur_rainbow_dash_v1.urdf b/examples/pybullet/gym/pybullet_data/quadruped/minitaur_rainbow_dash_v1.urdf new file mode 100644 index 000000000..c69a224dc --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/quadruped/minitaur_rainbow_dash_v1.urdf @@ -0,0 +1,980 @@ +<?xml version="1.0"?> +<robot name="quadruped"> + + <link name="base_chassis_link"> + <visual> + <geometry> + <box size=".3 .13 .087"/> + </geometry> + <material name="black"> + <color rgba=".3 .3 .3 1"/> + </material> + </visual> + + <collision> + <geometry> + <box size=".3 .13 .087"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz=".05 0 0"/> + <mass value="3.353"/> + <inertia ixx=".006837" ixy=".0" ixz=".0" iyy=".027262" iyz=".0" izz=".029870"/> + </inertial> + </link> + + + <link name="chassis_right"> + + <visual> + <origin rpy="0 0 0" xyz=".2375 .054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="blue"> + <color rgba="0 0 1 1"/> + </material> + </visual> + + <visual> + <origin rpy="0 0 0" xyz="-.2375 .054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="yellow"> + <color rgba="1 1 0 1"/> + </material> + </visual> + + <collision> + <origin rpy="0 0 0" xyz=".2375 .054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + <collision> + <origin rpy="0 0 0" xyz="-.2375 .054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + <inertial> + <mass value="0.322"/> + <inertia ixx=".004147" ixy=".0" ixz=".0" iyy=".040913" iyz=".0" izz=".041840"/> + </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 0 0" xyz="0 -.1265 -.0185"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="chassis_left"> + <visual> + <origin rpy="0 0 0" xyz=".2375 -.054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="green"> + <color rgba="0 1 0 1"/> + </material> + </visual> + + <visual> + <origin rpy="0 0 0" xyz="-.2375 -.054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="green"/> + </visual> + + <collision> + <origin rpy="0 0 0" xyz=".2375 -.054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + + <collision> + <origin rpy="0 0 0" xyz="-.2375 -.054 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + + <inertial> + <mass value="0.322"/> + <inertia ixx=".004147" ixy=".0" ixz=".0" iyy=".040913" iyz=".0" izz=".041840"/> + </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 0 0" xyz="0 .1265 -.0185"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="motor_front_rightR_bracket_link"> + <visual> + <origin rpy="0 0 0" xyz="0 -0.02 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="red"> + <color rgba="1 0 0 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 -0.02 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + + <inertial> + <origin rpy="0 0 0" xyz="0 -0.03 0"/> + <mass value=".16"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </inertial> + </link> + <joint name="motor_front_rightR_bracket_joint" type="prismatic"> + <axis xyz="0 0 1"/> + <parent link="base_chassis_link"/> + <child link="motor_front_rightR_bracket_link"/> + <origin rpy="0 0 0" xyz=".2375 -0.154 -.0185"/> + <limit effort="100" lower="-0.02" upper="0.02" velocity="100"/> + <joint_properties damping=".0" friction=".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=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".241"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </inertial> + </link> + <joint name="motor_front_rightR_joint" type="continuous"> + <axis xyz="0 0 1"/> + <parent link="motor_front_rightR_bracket_link"/> + <child link="motor_front_rightR_link"/> + <origin rpy="1.57075 0 0" xyz="0 0 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".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=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".241"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </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=".2375 .0275 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + + <link name="motor_front_leftL_bracket_link"> + <visual> + <origin rpy="0 0 0" xyz="0 0.02 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="red"> + <color rgba="1 0 0 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0.02 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + + <inertial> + <origin rpy="0 0 0" xyz="0 0.02 0"/> + <mass value=".16"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </inertial> + </link> + <joint name="motor_front_leftL_bracket_joint" type="prismatic"> + <axis xyz="0 0 1"/> + <parent link="base_chassis_link"/> + <child link="motor_front_leftL_bracket_link"/> + <origin rpy="0 0 0" xyz=".2375 0.154 -.0185"/> + <limit effort="100" lower="-0.02" upper="0.02" velocity="100"/> + <joint_properties damping=".0" friction=".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> + <collision> + <geometry> + <cylinder length=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".241"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </inertial> + </link> + <joint name="motor_front_leftL_joint" type="continuous"> + <axis xyz="0 0 1"/> + <parent link="motor_front_leftL_bracket_link"/> + <child link="motor_front_leftL_link"/> + <origin rpy="1.57075 0 3.141592" xyz="0 0 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".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=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".241"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </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=".2375 -.0275 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + + <link name="motor_back_rightR_bracket_link"> + <visual> + <origin rpy="0 0 0" xyz="0 -0.02 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="red"> + <color rgba="1 0 0 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 -0.02 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + + <inertial> + <origin rpy="0 0 0" xyz="0 -0.03 0"/> + <mass value=".16"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </inertial> + </link> + <joint name="motor_back_rightR_bracket_joint" type="prismatic"> + <axis xyz="0 0 1"/> + <parent link="base_chassis_link"/> + <child link="motor_back_rightR_bracket_link"/> + <origin rpy="0 0 0" xyz="-.2375 -0.154 -.0185"/> + <limit effort="100" lower="-0.02" upper="0.02" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + + <link name="motor_back_rightR_link"> + <visual> + <geometry> + <mesh filename="tmotor3.obj" scale="1 1 1"/> + </geometry> + <material name="white1"> + <color rgba="1 0 0 1"/> + </material> + </visual> + <collision> + <geometry> + <cylinder length=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".241"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </inertial> + </link> + <joint name="motor_back_rightR_joint" type="continuous"> + <axis xyz="0 0 1"/> + <parent link="motor_back_rightR_bracket_link"/> + <child link="motor_back_rightR_link"/> + <origin rpy="1.57075 0 0" xyz="0 0 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".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=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".241"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </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="-.2375 .0275 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + + <link name="motor_back_leftL_bracket_link"> + <visual> + <origin rpy="0 0 0" xyz="0 0.02 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + <material name="red"> + <color rgba="1 0 0 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0.02 0"/> + <geometry> + <box size=".068 .032 .050"/> + </geometry> + </collision> + + <inertial> + <origin rpy="0 0 0" xyz="0 0.02 0"/> + <mass value=".16"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </inertial> + </link> + <joint name="motor_back_leftL_bracket_joint" type="prismatic"> + <axis xyz="0 0 1"/> + <parent link="base_chassis_link"/> + <child link="motor_back_leftL_bracket_link"/> + <origin rpy="0 0 0" xyz="-.2375 0.154 -.0185"/> + <limit effort="100" lower="-0.02" upper="0.02" velocity="100"/> + <joint_properties damping=".0" friction=".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=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".241"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </inertial> + </link> + <joint name="motor_back_leftL_joint" type="continuous"> + <axis xyz="0 0 1"/> + <parent link="motor_back_leftL_bracket_link"/> + <child link="motor_back_leftL_link"/> + <origin rpy="1.57075 0 3.141592" xyz="0 0 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".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=".021" radius=".0425"/> + </geometry> + </collision> + <inertial> + <mass value=".241"/> + <inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/> + </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="-.2375 -.0275 0"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_front_rightR_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_front_rightR_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".05"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <mass value=".086"/> + <inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_front_rightL_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </inertial> + </link> + <joint name="hip_front_rightL_joint" 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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_front_rightL_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".05"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .1"/> + <mass value=".072"/> + <inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_front_leftR_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_front_leftR_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".05"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .1"/> + <mass value=".072"/> + <inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_front_leftL_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_front_leftL_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".05"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <mass value=".086"/> + <inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_back_rightR_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_back_rightR_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".05"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <mass value=".086"/> + <inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_back_rightL_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_back_rightL_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".05"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .1"/> + <mass value=".072"/> + <inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_back_leftR_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </inertial> + </link> + <joint name="hip_back_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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_back_leftR_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".05"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .108"/> + <geometry> + <box size=".017 .009 .216"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .1"/> + <mass value=".072"/> + <inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="upper_leg_back_leftL_link"> + <visual> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <geometry> + <box size=".039 .008 .129"/> + </geometry> + </collision> + <inertial> + <mass value=".034"/> + <inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/> + </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 .045 -.0145"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> + <link name="lower_leg_back_leftL_link"> + <contact> + <friction_anchor/> + <stiffness value="3000.0"/> + <damping value="100.0"/> + <spinning_friction value=".05"/> + <lateral_friction value="1"/> + </contact> + <visual> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + <material name="grey"> + <color rgba=".65 .65 .75 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <geometry> + <box size=".017 .009 .240"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 0" xyz="0 0 .120"/> + <mass value=".086"/> + <inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/> + </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 .0085 .056"/> + <limit effort="100" velocity="100"/> + <joint_properties damping=".0" friction=".0"/> + </joint> +</robot> diff --git a/examples/pybullet/gym/pybullet_data/quadruped/minitaur_single_motor.urdf b/examples/pybullet/gym/pybullet_data/quadruped/minitaur_single_motor.urdf new file mode 100644 index 000000000..7911c6555 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/quadruped/minitaur_single_motor.urdf @@ -0,0 +1,203 @@ +<?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.10 .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=".33 0.10 .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> + +</robot> + diff --git a/examples/pybullet/gym/pybullet_data/sphere_with_restitution.urdf b/examples/pybullet/gym/pybullet_data/sphere_with_restitution.urdf new file mode 100644 index 000000000..63a6682a5 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/sphere_with_restitution.urdf @@ -0,0 +1,33 @@ +<?xml version="0.0" ?> +<robot name="urdf_robot"> + <link name="baseLink"> + <contact> + <restitution value="0.5" /> + <rolling_friction value="0.001"/> + <spinning_friction value="0.001"/> + + </contact> + + <inertial> + <origin rpy="0 0 0" xyz="0 0 0"/> + <mass value="10.0"/> + <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/> + </inertial> + <visual> + <origin rpy="0 0 0" xyz="0 0 0"/> + <geometry> + <mesh filename="textured_sphere_smooth.obj" scale="0.5 0.5 0.5"/> + </geometry> + <material name="white"> + <color rgba="1 1 1 1"/> + </material> + </visual> + <collision> + <origin rpy="0 0 0" xyz="0 0 0"/> + <geometry> + <sphere radius="0.5"/> + </geometry> + </collision> + </link> +</robot> + diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/bullet_client.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/bullet_client.py new file mode 100644 index 000000000..8fc1fcd5c --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/bullet_client.py @@ -0,0 +1,50 @@ +"""A wrapper for pybullet to manage different clients.""" +from __future__ import absolute_import +from __future__ import division +import functools +import inspect +import pybullet + + +class BulletClient(object): + """A wrapper for pybullet to manage different clients.""" + + def __init__(self, connection_mode=None): + """Creates a Bullet client and connects to a simulation. + + Args: + connection_mode: + `None` connects to an existing simulation or, if fails, creates a + new headless simulation, + `pybullet.GUI` creates a new simulation with a GUI, + `pybullet.DIRECT` creates a headless simulation, + `pybullet.SHARED_MEMORY` connects to an existing simulation. + """ + self._shapes = {} + + if connection_mode is None: + self._client = pybullet.connect(pybullet.SHARED_MEMORY) + if self._client >= 0: + return + else: + connection_mode = pybullet.DIRECT + self._client = pybullet.connect(connection_mode) + + def __del__(self): + """Clean up connection if not already done.""" + try: + pybullet.disconnect(physicsClientId=self._client) + except pybullet.error: + pass + + def __getattr__(self, name): + """Inject the client id into Bullet functions.""" + attribute = getattr(pybullet, name) + if inspect.isbuiltin(attribute): + if name not in [ + "invertTransform", "multiplyTransforms", "getMatrixFromQuaternion", + "getEulerFromQuaternion", "computeViewMatrixFromYawPitchRoll", + "computeProjectionMatrixFOV", "getQuaternionFromEuler", + ]: # A temporary hack for now. + attribute = functools.partial(attribute, physicsClientId=self._client) + return attribute diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py new file mode 100644 index 000000000..8ab49b103 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py @@ -0,0 +1,35 @@ +"""Abstract base class for environment randomizer.""" + +import abc + + +class EnvRandomizerBase(object): + """Abstract base class for environment randomizer. + + Randomizes physical parameters of the objects in the simulation and adds + perturbations to the stepping of the simulation. + """ + + __metaclass__ = abc.ABCMeta + + @abc.abstractmethod + def randomize_env(self, env): + """Randomize the simulated_objects in the environment. + + Will be called at when env is reset. The physical parameters will be fixed + for that episode and be randomized again in the next environment.reset(). + + Args: + env: The Minitaur gym environment to be randomized. + """ + pass + + def randomize_step(self, env): + """Randomize simulation steps. + + Will be called at every timestep. May add random forces/torques to Minitaur. + + Args: + env: The Minitaur gym environment to be randomized. + """ + pass diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_alternating_legs_env_randomizer.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_alternating_legs_env_randomizer.py new file mode 100644 index 000000000..8dcb1f688 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_alternating_legs_env_randomizer.py @@ -0,0 +1,59 @@ +"""Randomize the minitaur_gym_alternating_leg_env when reset() is called. + +The randomization include swing_offset, extension_offset of all legs that mimics +bent legs, desired_pitch from user input, battery voltage and motor damping. +""" + +import numpy as np +import tensorflow as tf +from pybullet_envs.minitaur.minitaur.envs import env_randomizer_base + +# Absolute range. +NUM_LEGS = 4 +BATTERY_VOLTAGE_RANGE = (14.8, 16.8) +MOTOR_VISCOUS_DAMPING_RANGE = (0, 0.01) + + +class MinitaurAlternatingLegsEnvRandomizer( + env_randomizer_base.EnvRandomizerBase): + """A randomizer that changes the minitaur_gym_alternating_leg_env.""" + + def __init__(self, + perturb_swing_bound=0.1, + perturb_extension_bound=0.1, + perturb_desired_pitch_bound=0.01): + super(MinitaurAlternatingLegsEnvRandomizer, self).__init__() + self.perturb_swing_bound = perturb_swing_bound + self.perturb_extension_bound = perturb_extension_bound + self.perturb_desired_pitch_bound = perturb_desired_pitch_bound + + def randomize_env(self, env): + perturb_magnitude = np.random.uniform( + low=-self.perturb_swing_bound, + high=self.perturb_swing_bound, + size=NUM_LEGS) + env.set_swing_offset(perturb_magnitude) + tf.logging.info("swing_offset: {}".format(perturb_magnitude)) + + perturb_magnitude = np.random.uniform( + low=-self.perturb_extension_bound, + high=self.perturb_extension_bound, + size=NUM_LEGS) + env.set_extension_offset(perturb_magnitude) + tf.logging.info("extension_offset: {}".format(perturb_magnitude)) + + perturb_magnitude = np.random.uniform( + low=-self.perturb_desired_pitch_bound, + high=self.perturb_desired_pitch_bound) + env.set_desired_pitch(perturb_magnitude) + tf.logging.info("desired_pitch: {}".format(perturb_magnitude)) + + randomized_battery_voltage = np.random.uniform(BATTERY_VOLTAGE_RANGE[0], + BATTERY_VOLTAGE_RANGE[1]) + env.minitaur.SetBatteryVoltage(randomized_battery_voltage) + tf.logging.info("battery_voltage: {}".format(randomized_battery_voltage)) + + randomized_motor_damping = np.random.uniform(MOTOR_VISCOUS_DAMPING_RANGE[0], + MOTOR_VISCOUS_DAMPING_RANGE[1]) + env.minitaur.SetMotorViscousDamping(randomized_motor_damping) + tf.logging.info("motor_damping: {}".format(randomized_motor_damping)) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_env_randomizer.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_env_randomizer.py new file mode 100644 index 000000000..c0ce35faf --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_env_randomizer.py @@ -0,0 +1,69 @@ +"""Randomize the minitaur_gym_env when reset() is called.""" +import random + +import numpy as np +from pybullet_envs.minitaur.envs import env_randomizer_base + +# Relative range. +MINITAUR_BASE_MASS_ERROR_RANGE = (-0.2, 0.2) # 0.2 means 20% +MINITAUR_LEG_MASS_ERROR_RANGE = (-0.2, 0.2) # 0.2 means 20% +# Absolute range. +BATTERY_VOLTAGE_RANGE = (14.8, 16.8) # Unit: Volt +MOTOR_VISCOUS_DAMPING_RANGE = (0, 0.01) # Unit: N*m*s/rad (torque/angular vel) +MINITAUR_LEG_FRICTION = (0.8, 1.5) # Unit: dimensionless + + +class MinitaurEnvRandomizer(env_randomizer_base.EnvRandomizerBase): + """A randomizer that change the minitaur_gym_env during every reset.""" + + def __init__(self, + minitaur_base_mass_err_range=MINITAUR_BASE_MASS_ERROR_RANGE, + minitaur_leg_mass_err_range=MINITAUR_LEG_MASS_ERROR_RANGE, + battery_voltage_range=BATTERY_VOLTAGE_RANGE, + motor_viscous_damping_range=MOTOR_VISCOUS_DAMPING_RANGE): + self._minitaur_base_mass_err_range = minitaur_base_mass_err_range + self._minitaur_leg_mass_err_range = minitaur_leg_mass_err_range + self._battery_voltage_range = battery_voltage_range + self._motor_viscous_damping_range = motor_viscous_damping_range + + def randomize_env(self, env): + self._randomize_minitaur(env.minitaur) + + def _randomize_minitaur(self, minitaur): + """Randomize various physical properties of minitaur. + + It randomizes the mass/inertia of the base, mass/inertia of the legs, + friction coefficient of the feet, the battery voltage and the motor damping + at each reset() of the environment. + + Args: + minitaur: the Minitaur instance in minitaur_gym_env environment. + """ + base_mass = minitaur.GetBaseMassesFromURDF() + randomized_base_mass = random.uniform( + np.array(base_mass) * (1.0 + self._minitaur_base_mass_err_range[0]), + np.array(base_mass) * (1.0 + self._minitaur_base_mass_err_range[1])) + minitaur.SetBaseMasses(randomized_base_mass) + + leg_masses = minitaur.GetLegMassesFromURDF() + leg_masses_lower_bound = np.array(leg_masses) * ( + 1.0 + self._minitaur_leg_mass_err_range[0]) + leg_masses_upper_bound = np.array(leg_masses) * ( + 1.0 + self._minitaur_leg_mass_err_range[1]) + randomized_leg_masses = [ + np.random.uniform(leg_masses_lower_bound[i], leg_masses_upper_bound[i]) + for i in xrange(len(leg_masses)) + ] + minitaur.SetLegMasses(randomized_leg_masses) + + randomized_battery_voltage = random.uniform(BATTERY_VOLTAGE_RANGE[0], + BATTERY_VOLTAGE_RANGE[1]) + minitaur.SetBatteryVoltage(randomized_battery_voltage) + + randomized_motor_damping = random.uniform(MOTOR_VISCOUS_DAMPING_RANGE[0], + MOTOR_VISCOUS_DAMPING_RANGE[1]) + minitaur.SetMotorViscousDamping(randomized_motor_damping) + + randomized_foot_friction = random.uniform(MINITAUR_LEG_FRICTION[0], + MINITAUR_LEG_FRICTION[1]) + minitaur.SetFootFriction(randomized_foot_friction) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_env_randomizer_config.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_env_randomizer_config.py new file mode 100644 index 000000000..53d5ad122 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_env_randomizer_config.py @@ -0,0 +1,25 @@ +"""A config file for parameters and their ranges in dynamics randomization.""" + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + + +PARAM_RANGE = { + # The following ranges are in percentage. e.g. 0.8 means 80%. + "mass": [0.8, 1.2], + "inertia": [0.5, 1.5], + "motor strength": [0.8, 1.2], + # The following ranges are the physical values, in SI unit. + "motor friction": [0, 0.05], # Viscous damping (Nm s/rad). + "control step": [0.003, 0.02], # Time inteval (s). + "latency": [0.0, 0.04], # Time inteval (s). + "lateral friction": [0.5, 1.25], # Friction coefficient (dimensionless). + "battery": [14.0, 16.8], # Voltage (V). + "joint friction": [0, 0.05], # Coulomb friction torque (Nm). +} + + +def all_params(): + """Randomize all the physical parameters.""" + return PARAM_RANGE diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_env_randomizer_from_config.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_env_randomizer_from_config.py new file mode 100644 index 000000000..d49cfafad --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_env_randomizer_from_config.py @@ -0,0 +1,139 @@ +"""An environment randomizer that randomizes physical parameters from config.""" + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +import functools +import random + +import numpy as np +import tensorflow as tf +from pybullet_envs.minitaur.envs import env_randomizer_base +from pybullet_envs.minitaur.envs.env_randomizers import minitaur_env_randomizer_config + +SIMULATION_TIME_STEP = 0.001 + + +class MinitaurEnvRandomizerFromConfig(env_randomizer_base.EnvRandomizerBase): + """A randomizer that change the minitaur_gym_env during every reset.""" + + def __init__(self, config=None): + if config is None: + config = "all_params" + try: + config = getattr(minitaur_env_randomizer_config, config) + except AttributeError: + raise ValueError("Config {} is not found.".format(config)) + self._randomization_param_dict = config() + tf.logging.info("Randomization config is: {}".format( + self._randomization_param_dict)) + + def randomize_env(self, env): + """Randomize various physical properties of the environment. + + It randomizes the physical parameters according to the input configuration. + + Args: + env: A minitaur gym environment. + """ + self._randomization_function_dict = self._build_randomization_function_dict( + env) + for param_name, random_range in self._randomization_param_dict.iteritems(): + self._randomization_function_dict[param_name]( + lower_bound=random_range[0], upper_bound=random_range[1]) + + def _build_randomization_function_dict(self, env): + func_dict = {} + func_dict["mass"] = functools.partial( + self._randomize_masses, minitaur=env.minitaur) + func_dict["inertia"] = functools.partial( + self._randomize_inertia, minitaur=env.minitaur) + func_dict["latency"] = functools.partial( + self._randomize_latency, minitaur=env.minitaur) + func_dict["joint friction"] = functools.partial( + self._randomize_joint_friction, minitaur=env.minitaur) + func_dict["motor friction"] = functools.partial( + self._randomize_motor_friction, minitaur=env.minitaur) + func_dict["restitution"] = functools.partial( + self._randomize_contact_restitution, minitaur=env.minitaur) + func_dict["lateral friction"] = functools.partial( + self._randomize_contact_friction, minitaur=env.minitaur) + func_dict["battery"] = functools.partial( + self._randomize_battery_level, minitaur=env.minitaur) + func_dict["motor strength"] = functools.partial( + self._randomize_motor_strength, minitaur=env.minitaur) + # Settinmg control step needs access to the environment. + func_dict["control step"] = functools.partial( + self._randomize_control_step, env=env) + return func_dict + + def _randomize_control_step(self, env, lower_bound, upper_bound): + randomized_control_step = random.uniform(lower_bound, upper_bound) + env.set_time_step(randomized_control_step) + tf.logging.info("control step is: {}".format(randomized_control_step)) + + def _randomize_masses(self, minitaur, lower_bound, upper_bound): + base_mass = minitaur.GetBaseMassesFromURDF() + random_base_ratio = random.uniform(lower_bound, upper_bound) + randomized_base_mass = random_base_ratio * np.array(base_mass) + minitaur.SetBaseMasses(randomized_base_mass) + tf.logging.info("base mass is: {}".format(randomized_base_mass)) + + leg_masses = minitaur.GetLegMassesFromURDF() + random_leg_ratio = random.uniform(lower_bound, upper_bound) + randomized_leg_masses = random_leg_ratio * np.array(leg_masses) + minitaur.SetLegMasses(randomized_leg_masses) + tf.logging.info("leg mass is: {}".format(randomized_leg_masses)) + + def _randomize_inertia(self, minitaur, lower_bound, upper_bound): + base_inertia = minitaur.GetBaseInertiasFromURDF() + random_base_ratio = random.uniform(lower_bound, upper_bound) + randomized_base_inertia = random_base_ratio * np.array(base_inertia) + minitaur.SetBaseInertias(randomized_base_inertia) + tf.logging.info("base inertia is: {}".format(randomized_base_inertia)) + leg_inertia = minitaur.GetLegInertiasFromURDF() + random_leg_ratio = random.uniform(lower_bound, upper_bound) + randomized_leg_inertia = random_leg_ratio * np.array(leg_inertia) + minitaur.SetLegInertias(randomized_leg_inertia) + tf.logging.info("leg inertia is: {}".format(randomized_leg_inertia)) + + def _randomize_latency(self, minitaur, lower_bound, upper_bound): + randomized_latency = random.uniform(lower_bound, upper_bound) + minitaur.SetControlLatency(randomized_latency) + tf.logging.info("control latency is: {}".format(randomized_latency)) + + def _randomize_joint_friction(self, minitaur, lower_bound, upper_bound): + num_knee_joints = minitaur.GetNumKneeJoints() + randomized_joint_frictions = np.random.uniform( + [lower_bound] * num_knee_joints, [upper_bound] * num_knee_joints) + minitaur.SetJointFriction(randomized_joint_frictions) + tf.logging.info("joint friction is: {}".format(randomized_joint_frictions)) + + def _randomize_motor_friction(self, minitaur, lower_bound, upper_bound): + randomized_motor_damping = random.uniform(lower_bound, upper_bound) + minitaur.SetMotorViscousDamping(randomized_motor_damping) + tf.logging.info("motor friction is: {}".format(randomized_motor_damping)) + + def _randomize_contact_restitution(self, minitaur, lower_bound, upper_bound): + randomized_restitution = random.uniform(lower_bound, upper_bound) + minitaur.SetFootRestitution(randomized_restitution) + tf.logging.info("foot restitution is: {}".format(randomized_restitution)) + + def _randomize_contact_friction(self, minitaur, lower_bound, upper_bound): + randomized_foot_friction = random.uniform(lower_bound, upper_bound) + minitaur.SetFootFriction(randomized_foot_friction) + tf.logging.info("foot friction is: {}".format(randomized_foot_friction)) + + def _randomize_battery_level(self, minitaur, lower_bound, upper_bound): + randomized_battery_voltage = random.uniform(lower_bound, upper_bound) + minitaur.SetBatteryVoltage(randomized_battery_voltage) + tf.logging.info("battery voltage is: {}".format(randomized_battery_voltage)) + + def _randomize_motor_strength(self, minitaur, lower_bound, upper_bound): + randomized_motor_strength_ratios = np.random.uniform( + [lower_bound] * minitaur.num_motors, + [upper_bound] * minitaur.num_motors) + minitaur.SetMotorStrengthRatios(randomized_motor_strength_ratios) + tf.logging.info( + "motor strength is: {}".format(randomized_motor_strength_ratios)) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py new file mode 100644 index 000000000..0ebe4ed0d --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py @@ -0,0 +1,91 @@ +"""Adds random forces to the base of Minitaur during the simulation steps.""" + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +import math +import numpy as np +from pybullet_envs.minitaur.envs import env_randomizer_base + +_PERTURBATION_START_STEP = 100 +_PERTURBATION_INTERVAL_STEPS = 200 +_PERTURBATION_DURATION_STEPS = 10 +_HORIZONTAL_FORCE_UPPER_BOUND = 120 +_HORIZONTAL_FORCE_LOWER_BOUND = 240 +_VERTICAL_FORCE_UPPER_BOUND = 300 +_VERTICAL_FORCE_LOWER_BOUND = 500 + + +class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase): + """Applies a random impulse to the base of Minitaur.""" + + def __init__( + self, + perturbation_start_step=_PERTURBATION_START_STEP, + perturbation_interval_steps=_PERTURBATION_INTERVAL_STEPS, + perturbation_duration_steps=_PERTURBATION_DURATION_STEPS, + horizontal_force_bound=None, + vertical_force_bound=None, + ): + """Initializes the randomizer. + + Args: + perturbation_start_step: No perturbation force before the env has advanced + this amount of steps. + perturbation_interval_steps: The step interval between applying + perturbation forces. + perturbation_duration_steps: The duration of the perturbation force. + horizontal_force_bound: The lower and upper bound of the applied force + magnitude when projected in the horizontal plane. + vertical_force_bound: The z component (abs value) bound of the applied + perturbation force. + """ + self._perturbation_start_step = perturbation_start_step + self._perturbation_interval_steps = perturbation_interval_steps + self._perturbation_duration_steps = perturbation_duration_steps + self._horizontal_force_bound = ( + horizontal_force_bound if horizontal_force_bound else + [_HORIZONTAL_FORCE_LOWER_BOUND, _HORIZONTAL_FORCE_UPPER_BOUND]) + self._vertical_force_bound = ( + vertical_force_bound if vertical_force_bound else + [_VERTICAL_FORCE_LOWER_BOUND, _VERTICAL_FORCE_UPPER_BOUND]) + + def randomize_env(self, env): + """Randomizes the simulation environment. + + Args: + env: The Minitaur gym environment to be randomized. + """ + pass + + def randomize_step(self, env): + """Randomizes simulation steps. + + Will be called at every timestep. May add random forces/torques to Minitaur. + + Args: + env: The Minitaur gym environment to be randomized. + """ + base_link_ids = env.minitaur.chassis_link_ids + if env.env_step_counter % self._perturbation_interval_steps == 0: + self._applied_link_id = base_link_ids[np.random.randint( + 0, len(base_link_ids))] + horizontal_force_magnitude = np.random.uniform( + self._horizontal_force_bound[0], self._horizontal_force_bound[1]) + theta = np.random.uniform(0, 2 * math.pi) + vertical_force_magnitude = np.random.uniform( + self._vertical_force_bound[0], self._vertical_force_bound[1]) + self._applied_force = horizontal_force_magnitude * np.array( + [math.cos(theta), math.sin(theta), 0]) + np.array( + [0, 0, -vertical_force_magnitude]) + + if (env.env_step_counter % self._perturbation_interval_steps < + self._perturbation_duration_steps) and (env.env_step_counter >= + self._perturbation_start_step): + env.pybullet_client.applyExternalForce( + objectUniqueId=env.minitaur.quadruped, + linkIndex=self._applied_link_id, + forceObj=self._applied_force, + posObj=[0.0, 0.0, 0.0], + flags=env.pybullet_client.LINK_FRAME) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_terrain_randomizer.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_terrain_randomizer.py new file mode 100644 index 000000000..91cb52302 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_terrain_randomizer.py @@ -0,0 +1,295 @@ +"""Generates a random terrain at Minitaur gym environment reset.""" + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +import itertools +import math +import enum +import numpy as np + +from pybullet_envs.minitaur.envs import env_randomizer_base + +_GRID_LENGTH = 15 +_GRID_WIDTH = 10 +_MAX_SAMPLE_SIZE = 30 +_MIN_BLOCK_DISTANCE = 0.7 +_MAX_BLOCK_LENGTH = _MIN_BLOCK_DISTANCE +_MIN_BLOCK_LENGTH = _MAX_BLOCK_LENGTH / 2 +_MAX_BLOCK_HEIGHT = 0.05 +_MIN_BLOCK_HEIGHT = _MAX_BLOCK_HEIGHT / 2 + + +class PoissonDisc2D(object): + """Generates 2D points using Poisson disk sampling method. + + Implements the algorithm described in: + http://www.cs.ubc.ca/~rbridson/docs/bridson-siggraph07-poissondisk.pdf + Unlike the uniform sampling method that creates small clusters of points, + Poisson disk method enforces the minimum distance between points and is more + suitable for generating a spatial distribution of non-overlapping objects. + """ + + def __init__(self, grid_length, grid_width, min_radius, max_sample_size): + """Initializes the algorithm. + + Args: + grid_length: The length of the bounding square in which points are + sampled. + grid_width: The width of the bounding square in which points are + sampled. + min_radius: The minimum distance between any pair of points. + max_sample_size: The maximum number of sample points around a active site. + See details in the algorithm description. + """ + self._cell_length = min_radius / math.sqrt(2) + self._grid_length = grid_length + self._grid_width = grid_width + self._grid_size_x = int(grid_length / self._cell_length) + 1 + self._grid_size_y = int(grid_width / self._cell_length) + 1 + self._min_radius = min_radius + self._max_sample_size = max_sample_size + + # Flattern the 2D grid as an 1D array. The grid is used for fast nearest + # point searching. + self._grid = [None] * self._grid_size_x * self._grid_size_y + + # Generate the first sample point and set it as an active site. + first_sample = np.array( + np.random.random_sample(2)) * [grid_length, grid_width] + self._active_list = [first_sample] + + # Also store the sample point in the grid. + self._grid[self._point_to_index_1d(first_sample)] = first_sample + + def _point_to_index_1d(self, point): + """Computes the index of a point in the grid array. + + Args: + point: A 2D point described by its coordinates (x, y). + + Returns: + The index of the point within the self._grid array. + """ + return self._index_2d_to_1d(self._point_to_index_2d(point)) + + def _point_to_index_2d(self, point): + """Computes the 2D index (aka cell ID) of a point in the grid. + + Args: + point: A 2D point (list) described by its coordinates (x, y). + + Returns: + x_index: The x index of the cell the point belongs to. + y_index: The y index of the cell the point belongs to. + """ + x_index = int(point[0] / self._cell_length) + y_index = int(point[1] / self._cell_length) + return x_index, y_index + + def _index_2d_to_1d(self, index2d): + """Converts the 2D index to the 1D position in the grid array. + + Args: + index2d: The 2D index of a point (aka the cell ID) in the grid. + + Returns: + The 1D position of the cell within the self._grid array. + """ + return index2d[0] + index2d[1] * self._grid_size_x + + def _is_in_grid(self, point): + """Checks if the point is inside the grid boundary. + + Args: + point: A 2D point (list) described by its coordinates (x, y). + + Returns: + Whether the point is inside the grid. + """ + return (0 <= point[0] < self._grid_length) and (0 <= point[1] < + self._grid_width) + + def _is_in_range(self, index2d): + """Checks if the cell ID is within the grid. + + Args: + index2d: The 2D index of a point (aka the cell ID) in the grid. + + Returns: + Whether the cell (2D index) is inside the grid. + """ + + return (0 <= index2d[0] < self._grid_size_x) and (0 <= index2d[1] < + self._grid_size_y) + + def _is_close_to_existing_points(self, point): + """Checks if the point is close to any already sampled (and stored) points. + + Args: + point: A 2D point (list) described by its coordinates (x, y). + + Returns: + True iff the distance of the point to any existing points is smaller than + the min_radius + """ + px, py = self._point_to_index_2d(point) + # Now we can check nearby cells for existing points + for neighbor_cell in itertools.product( + xrange(px - 1, px + 2), xrange(py - 1, py + 2)): + + if not self._is_in_range(neighbor_cell): + continue + + maybe_a_point = self._grid[self._index_2d_to_1d(neighbor_cell)] + if maybe_a_point is not None and np.linalg.norm( + maybe_a_point - point) < self._min_radius: + return True + + return False + + def sample(self): + """Samples new points around some existing point. + + Removes the sampling base point and also stores the new jksampled points if + they are far enough from all existing points. + """ + active_point = self._active_list.pop() + for _ in xrange(self._max_sample_size): + # Generate random points near the current active_point between the radius + random_radius = np.random.uniform(self._min_radius, 2 * self._min_radius) + random_angle = np.random.uniform(0, 2 * math.pi) + + # The sampled 2D points near the active point + sample = random_radius * np.array( + [np.cos(random_angle), np.sin(random_angle)]) + active_point + + if not self._is_in_grid(sample): + continue + + if self._is_close_to_existing_points(sample): + continue + + self._active_list.append(sample) + self._grid[self._point_to_index_1d(sample)] = sample + + def generate(self): + """Generates the Poisson disc distribution of 2D points. + + Although the while loop looks scary, the algorithm is in fact O(N), where N + is the number of cells within the grid. When we sample around a base point + (in some base cell), new points will not be pushed into the base cell + because of the minimum distance constraint. Once the current base point is + removed, all future searches cannot start from within the same base cell. + + Returns: + All sampled points. The points are inside the quare [0, grid_length] x [0, + grid_width] + """ + + while self._active_list: + self.sample() + + all_sites = [] + for p in self._grid: + if p is not None: + all_sites.append(p) + + return all_sites + + +class TerrainType(enum.Enum): + """The randomzied terrain types we can use in the gym env.""" + RANDOM_BLOCKS = 1 + TRIANGLE_MESH = 2 + + +class MinitaurTerrainRandomizer(env_randomizer_base.EnvRandomizerBase): + """Generates an uneven terrain in the gym env.""" + + def __init__( + self, + terrain_type=TerrainType.TRIANGLE_MESH, + mesh_filename="robotics/reinforcement_learning/minitaur/envs/testdata/" + "triangle_mesh_terrain/terrain9735.obj", + mesh_scale=None): + """Initializes the randomizer. + + Args: + terrain_type: Whether to generate random blocks or load a triangle mesh. + mesh_filename: The mesh file to be used. The mesh will only be loaded if + terrain_type is set to TerrainType.TRIANGLE_MESH. + mesh_scale: the scaling factor for the triangles in the mesh file. + """ + self._terrain_type = terrain_type + self._mesh_filename = mesh_filename + self._mesh_scale = mesh_scale if mesh_scale else [1.0, 1.0, 0.3] + + def randomize_env(self, env): + """Generate a random terrain for the current env. + + Args: + env: A minitaur gym environment. + """ + + if self._terrain_type is TerrainType.TRIANGLE_MESH: + self._load_triangle_mesh(env) + if self._terrain_type is TerrainType.RANDOM_BLOCKS: + self._generate_convex_blocks(env) + + def _load_triangle_mesh(self, env): + """Represents the random terrain using a triangle mesh. + + It is possible for Minitaur leg to stuck at the common edge of two triangle + pieces. To prevent this from happening, we recommend using hard contacts + (or high stiffness values) for Minitaur foot in sim. + + Args: + env: A minitaur gym environment. + """ + env.pybullet_client.removeBody(env.ground_id) + terrain_collision_shape_id = env.pybullet_client.createCollisionShape( + shapeType=env.pybullet_client.GEOM_MESH, + fileName=self._mesh_filename, + flags=1, + meshScale=self._mesh_scale) + env.ground_id = env.pybullet_client.createMultiBody( + baseMass=0, + baseCollisionShapeIndex=terrain_collision_shape_id, + basePosition=[0, 0, 0]) + + def _generate_convex_blocks(self, env): + """Adds random convex blocks to the flat ground. + + We use the Possion disk algorithm to add some random blocks on the ground. + Possion disk algorithm sets the minimum distance between two sampling + points, thus voiding the clustering effect in uniform N-D distribution. + + Args: + env: A minitaur gym environment. + + """ + + poisson_disc = PoissonDisc2D(_GRID_LENGTH, _GRID_WIDTH, _MIN_BLOCK_DISTANCE, + _MAX_SAMPLE_SIZE) + + block_centers = poisson_disc.generate() + + for center in block_centers: + # We want the blocks to be in front of the robot. + shifted_center = np.array(center) - [2, _GRID_WIDTH / 2] + + # Do not place blocks near the point [0, 0], where the robot will start. + if abs(shifted_center[0]) < 1.0 and abs(shifted_center[1]) < 1.0: + continue + half_length = np.random.uniform(_MIN_BLOCK_LENGTH, _MAX_BLOCK_LENGTH) / ( + 2 * math.sqrt(2)) + half_height = np.random.uniform(_MIN_BLOCK_HEIGHT, _MAX_BLOCK_HEIGHT) / 2 + box_id = env.pybullet_client.createCollisionShape( + env.pybullet_client.GEOM_BOX, + halfExtents=[half_length, half_length, half_height]) + env.pybullet_client.createMultiBody( + baseMass=0, + baseCollisionShapeIndex=box_id, + basePosition=[shifted_center[0], shifted_center[1], half_height]) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur.py new file mode 100644 index 000000000..cd5749d22 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur.py @@ -0,0 +1,996 @@ +"""This file implements the functionalities of a minitaur using pybullet. + +""" +import collections +import copy +import math +import re + +import numpy as np +import motor + +INIT_POSITION = [0, 0, .2] +INIT_RACK_POSITION = [0, 0, 1] +INIT_ORIENTATION = [0, 0, 0, 1] +KNEE_CONSTRAINT_POINT_RIGHT = [0, 0.005, 0.2] +KNEE_CONSTRAINT_POINT_LEFT = [0, 0.01, 0.2] +OVERHEAT_SHUTDOWN_TORQUE = 2.45 +OVERHEAT_SHUTDOWN_TIME = 1.0 +LEG_POSITION = ["front_left", "back_left", "front_right", "back_right"] +MOTOR_NAMES = [ + "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" +] +_CHASSIS_NAME_PATTERN = re.compile(r"chassis\D*center") +_MOTOR_NAME_PATTERN = re.compile(r"motor\D*joint") +_KNEE_NAME_PATTERN = re.compile(r"knee\D*") +SENSOR_NOISE_STDDEV = (0.0, 0.0, 0.0, 0.0, 0.0) +TWO_PI = 2 * math.pi + + +def MapToMinusPiToPi(angles): + """Maps a list of angles to [-pi, pi]. + + Args: + angles: A list of angles in rad. + Returns: + A list of angle mapped to [-pi, pi]. + """ + mapped_angles = copy.deepcopy(angles) + for i in range(len(angles)): + mapped_angles[i] = math.fmod(angles[i], TWO_PI) + if mapped_angles[i] >= math.pi: + mapped_angles[i] -= TWO_PI + elif mapped_angles[i] < -math.pi: + mapped_angles[i] += TWO_PI + return mapped_angles + + +class Minitaur(object): + """The minitaur class that simulates a quadruped robot from Ghost Robotics. + + """ + + def __init__(self, + pybullet_client, + urdf_root="", + time_step=0.01, + action_repeat=1, + self_collision_enabled=False, + motor_velocity_limit=np.inf, + pd_control_enabled=False, + accurate_motor_model_enabled=False, + remove_default_joint_damping=False, + motor_kp=1.0, + motor_kd=0.02, + pd_latency=0.0, + control_latency=0.0, + observation_noise_stdev=SENSOR_NOISE_STDDEV, + torque_control_enabled=False, + motor_overheat_protection=False, + on_rack=False): + """Constructs a minitaur and reset it to the initial states. + + Args: + pybullet_client: The instance of BulletClient to manage different + simulations. + urdf_root: The path to the urdf folder. + time_step: The time step of the simulation. + action_repeat: The number of ApplyAction() for each control step. + self_collision_enabled: Whether to enable self collision. + motor_velocity_limit: The upper limit of the motor velocity. + pd_control_enabled: Whether to use PD control for the motors. + accurate_motor_model_enabled: Whether to use the accurate DC motor model. + remove_default_joint_damping: Whether to remove the default joint damping. + motor_kp: proportional gain for the accurate motor model. + motor_kd: derivative gain for the accurate motor model. + pd_latency: The latency of the observations (in seconds) used to calculate + PD control. On the real hardware, it is the latency between the + microcontroller and the motor controller. + control_latency: The latency of the observations (in second) used to + calculate action. On the real hardware, it is the latency from the motor + controller, the microcontroller to the host (Nvidia TX2). + observation_noise_stdev: The standard deviation of a Gaussian noise model + for the sensor. It should be an array for separate sensors in the + following order [motor_angle, motor_velocity, motor_torque, + base_roll_pitch_yaw, base_angular_velocity] + torque_control_enabled: Whether to use the torque control, if set to + False, pose control will be used. + motor_overheat_protection: Whether to shutdown the motor that has exerted + large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time + (OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in minitaur.py for more + details. + on_rack: Whether to place the minitaur on rack. This is only used to debug + the walking gait. In this mode, the minitaur's base is hanged midair so + that its walking gait is clearer to visualize. + """ + self.num_motors = 8 + self.num_legs = int(self.num_motors / 2) + self._pybullet_client = pybullet_client + self._action_repeat = action_repeat + self._urdf_root = urdf_root + self._self_collision_enabled = self_collision_enabled + self._motor_velocity_limit = motor_velocity_limit + self._pd_control_enabled = pd_control_enabled + self._motor_direction = [-1, -1, -1, -1, 1, 1, 1, 1] + self._observed_motor_torques = np.zeros(self.num_motors) + self._applied_motor_torques = np.zeros(self.num_motors) + self._max_force = 3.5 + self._pd_latency = pd_latency + self._control_latency = control_latency + self._observation_noise_stdev = observation_noise_stdev + self._accurate_motor_model_enabled = accurate_motor_model_enabled + self._remove_default_joint_damping = remove_default_joint_damping + self._observation_history = collections.deque(maxlen=100) + self._control_observation = [] + self._chassis_link_ids = [-1] + self._leg_link_ids = [] + self._motor_link_ids = [] + self._foot_link_ids = [] + self._torque_control_enabled = torque_control_enabled + self._motor_overheat_protection = motor_overheat_protection + self._on_rack = on_rack + if self._accurate_motor_model_enabled: + self._kp = motor_kp + self._kd = motor_kd + self._motor_model = motor.MotorModel( + torque_control_enabled=self._torque_control_enabled, + kp=self._kp, + kd=self._kd) + elif self._pd_control_enabled: + self._kp = 8 + self._kd = 0.3 + else: + self._kp = 1 + self._kd = 1 + self.time_step = time_step + self._step_counter = 0 + # reset_time=-1.0 means skipping the reset motion. + # See Reset for more details. + self.Reset(reset_time=-1.0) + + def GetTimeSinceReset(self): + return self._step_counter * self.time_step + + def Step(self, action): + for _ in range(self._action_repeat): + self.ApplyAction(action) + self._pybullet_client.stepSimulation() + self.ReceiveObservation() + self._step_counter += 1 + + def Terminate(self): + pass + + def _RecordMassInfoFromURDF(self): + self._base_mass_urdf = [] + for chassis_id in self._chassis_link_ids: + self._base_mass_urdf.append( + self._pybullet_client.getDynamicsInfo(self.quadruped, chassis_id)[0]) + self._leg_masses_urdf = [] + for leg_id in self._leg_link_ids: + self._leg_masses_urdf.append( + self._pybullet_client.getDynamicsInfo(self.quadruped, leg_id)[0]) + for motor_id in self._motor_link_ids: + self._leg_masses_urdf.append( + self._pybullet_client.getDynamicsInfo(self.quadruped, motor_id)[0]) + + def _RecordInertiaInfoFromURDF(self): + """Record the inertia of each body from URDF file.""" + self._link_urdf = [] + num_bodies = self._pybullet_client.getNumJoints(self.quadruped) + for body_id in range(-1, num_bodies): # -1 is for the base link. + inertia = self._pybullet_client.getDynamicsInfo(self.quadruped, + body_id)[2] + self._link_urdf.append(inertia) + # We need to use id+1 to index self._link_urdf because it has the base + # (index = -1) at the first element. + self._base_inertia_urdf = [ + self._link_urdf[chassis_id + 1] for chassis_id in self._chassis_link_ids + ] + self._leg_inertia_urdf = [ + self._link_urdf[leg_id + 1] for leg_id in self._leg_link_ids + ] + self._leg_inertia_urdf.extend( + [self._link_urdf[motor_id + 1] for motor_id in self._motor_link_ids]) + + def _BuildJointNameToIdDict(self): + num_joints = self._pybullet_client.getNumJoints(self.quadruped) + self._joint_name_to_id = {} + for i in range(num_joints): + joint_info = self._pybullet_client.getJointInfo(self.quadruped, i) + self._joint_name_to_id[joint_info[1].decode("UTF-8")] = joint_info[0] + + def _BuildUrdfIds(self): + """Build the link Ids from its name in the URDF file.""" + num_joints = self._pybullet_client.getNumJoints(self.quadruped) + self._chassis_link_ids = [-1] + # the self._leg_link_ids include both the upper and lower links of the leg. + self._leg_link_ids = [] + self._motor_link_ids = [] + self._foot_link_ids = [] + for i in range(num_joints): + joint_info = self._pybullet_client.getJointInfo(self.quadruped, i) + joint_name = joint_info[1].decode("UTF-8") + joint_id = self._joint_name_to_id[joint_name] + if _CHASSIS_NAME_PATTERN.match(joint_name): + self._chassis_link_ids.append(joint_id) + elif _MOTOR_NAME_PATTERN.match(joint_name): + self._motor_link_ids.append(joint_id) + elif _KNEE_NAME_PATTERN.match(joint_name): + self._foot_link_ids.append(joint_id) + else: + self._leg_link_ids.append(joint_id) + self._leg_link_ids.extend(self._foot_link_ids) + self._chassis_link_ids.sort() + self._motor_link_ids.sort() + self._foot_link_ids.sort() + self._leg_link_ids.sort() + + def _RemoveDefaultJointDamping(self): + num_joints = self._pybullet_client.getNumJoints(self.quadruped) + for i in range(num_joints): + joint_info = self._pybullet_client.getJointInfo(self.quadruped, i) + self._pybullet_client.changeDynamics( + joint_info[0], -1, linearDamping=0, angularDamping=0) + + def _BuildMotorIdList(self): + self._motor_id_list = [ + self._joint_name_to_id[motor_name] for motor_name in MOTOR_NAMES + ] + + def IsObservationValid(self): + """Whether the observation is valid for the current time step. + + In simulation, observations are always valid. In real hardware, it may not + be valid from time to time when communication error happens between the + Nvidia TX2 and the microcontroller. + + Returns: + Whether the observation is valid for the current time step. + """ + return True + + def Reset(self, reload_urdf=True, default_motor_angles=None, reset_time=3.0): + """Reset the minitaur to its initial states. + + Args: + reload_urdf: Whether to reload the urdf file. If not, Reset() just place + the minitaur back to its starting position. + default_motor_angles: The default motor angles. If it is None, minitaur + will hold a default pose (motor angle math.pi / 2) for 100 steps. In + torque control mode, the phase of holding the default pose is skipped. + reset_time: The duration (in seconds) to hold the default motor angles. If + reset_time <= 0 or in torque control mode, the phase of holding the + default pose is skipped. + """ + if self._on_rack: + init_position = INIT_RACK_POSITION + else: + init_position = INIT_POSITION + if reload_urdf: + if self._self_collision_enabled: + self.quadruped = self._pybullet_client.loadURDF( + "%s/quadruped/minitaur.urdf" % self._urdf_root, + init_position, + useFixedBase=self._on_rack, + flags=self._pybullet_client.URDF_USE_SELF_COLLISION) + else: + self.quadruped = self._pybullet_client.loadURDF( + "%s/quadruped/minitaur.urdf" % self._urdf_root, + init_position, + useFixedBase=self._on_rack) + self._BuildJointNameToIdDict() + self._BuildUrdfIds() + if self._remove_default_joint_damping: + self._RemoveDefaultJointDamping() + self._BuildMotorIdList() + self._RecordMassInfoFromURDF() + self._RecordInertiaInfoFromURDF() + self.ResetPose(add_constraint=True) + else: + self._pybullet_client.resetBasePositionAndOrientation( + self.quadruped, init_position, INIT_ORIENTATION) + self._pybullet_client.resetBaseVelocity(self.quadruped, [0, 0, 0], + [0, 0, 0]) + self.ResetPose(add_constraint=False) + self._overheat_counter = np.zeros(self.num_motors) + self._motor_enabled_list = [True] * self.num_motors + self._step_counter = 0 + + # Perform reset motion within reset_duration if in position control mode. + # Nothing is performed if in torque control mode for now. + # TODO(jietan): Add reset motion when the torque control is fully supported. + self._observation_history.clear() + if not self._torque_control_enabled and reset_time > 0.0: + self.ReceiveObservation() + for _ in range(100): + self.ApplyAction([math.pi / 2] * self.num_motors) + self._pybullet_client.stepSimulation() + self.ReceiveObservation() + if default_motor_angles is not None: + num_steps_to_reset = int(reset_time / self.time_step) + for _ in range(num_steps_to_reset): + self.ApplyAction(default_motor_angles) + self._pybullet_client.stepSimulation() + self.ReceiveObservation() + self.ReceiveObservation() + + def _SetMotorTorqueById(self, motor_id, torque): + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=motor_id, + controlMode=self._pybullet_client.TORQUE_CONTROL, + force=torque) + + def _SetDesiredMotorAngleById(self, motor_id, desired_angle): + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=motor_id, + controlMode=self._pybullet_client.POSITION_CONTROL, + targetPosition=desired_angle, + positionGain=self._kp, + velocityGain=self._kd, + force=self._max_force) + + def _SetDesiredMotorAngleByName(self, motor_name, desired_angle): + self._SetDesiredMotorAngleById(self._joint_name_to_id[motor_name], + desired_angle) + + def ResetPose(self, add_constraint): + """Reset the pose of the minitaur. + + Args: + add_constraint: Whether to add a constraint at the joints of two feet. + """ + for i in range(self.num_legs): + self._ResetPoseForLeg(i, add_constraint) + + def _ResetPoseForLeg(self, leg_id, add_constraint): + """Reset the initial pose for the leg. + + Args: + leg_id: It should be 0, 1, 2, or 3, which represents the leg at + front_left, back_left, front_right and back_right. + add_constraint: Whether to add a constraint at the joints of two feet. + """ + knee_friction_force = 0 + half_pi = math.pi / 2.0 + knee_angle = -2.1834 + + leg_position = LEG_POSITION[leg_id] + self._pybullet_client.resetJointState( + self.quadruped, + self._joint_name_to_id["motor_" + leg_position + "L_joint"], + self._motor_direction[2 * leg_id] * half_pi, + targetVelocity=0) + self._pybullet_client.resetJointState( + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "L_link"], + self._motor_direction[2 * leg_id] * knee_angle, + targetVelocity=0) + self._pybullet_client.resetJointState( + self.quadruped, + self._joint_name_to_id["motor_" + leg_position + "R_joint"], + self._motor_direction[2 * leg_id + 1] * half_pi, + targetVelocity=0) + self._pybullet_client.resetJointState( + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "R_link"], + self._motor_direction[2 * leg_id + 1] * knee_angle, + targetVelocity=0) + if add_constraint: + self._pybullet_client.createConstraint( + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "R_link"], + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "L_link"], + self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0], + KNEE_CONSTRAINT_POINT_RIGHT, KNEE_CONSTRAINT_POINT_LEFT) + + if self._accurate_motor_model_enabled or self._pd_control_enabled: + # Disable the default motor in pybullet. + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=( + self._joint_name_to_id["motor_" + leg_position + "L_joint"]), + controlMode=self._pybullet_client.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=( + self._joint_name_to_id["motor_" + leg_position + "R_joint"]), + controlMode=self._pybullet_client.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) + + else: + self._SetDesiredMotorAngleByName( + "motor_" + leg_position + "L_joint", + self._motor_direction[2 * leg_id] * half_pi) + self._SetDesiredMotorAngleByName( + "motor_" + leg_position + "R_joint", + self._motor_direction[2 * leg_id + 1] * half_pi) + + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=(self._joint_name_to_id["knee_" + leg_position + "L_link"]), + controlMode=self._pybullet_client.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=(self._joint_name_to_id["knee_" + leg_position + "R_link"]), + controlMode=self._pybullet_client.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) + + def GetBasePosition(self): + """Get the position of minitaur's base. + + Returns: + The position of minitaur's base. + """ + position, _ = ( + self._pybullet_client.getBasePositionAndOrientation(self.quadruped)) + return position + + def GetTrueBaseRollPitchYaw(self): + """Get minitaur's base orientation in euler angle in the world frame. + + Returns: + A tuple (roll, pitch, yaw) of the base in world frame. + """ + orientation = self.GetTrueBaseOrientation() + roll_pitch_yaw = self._pybullet_client.getEulerFromQuaternion(orientation) + return np.asarray(roll_pitch_yaw) + + def GetBaseRollPitchYaw(self): + """Get minitaur's base orientation in euler angle in the world frame. + + This function mimicks the noisy sensor reading and adds latency. + Returns: + A tuple (roll, pitch, yaw) of the base in world frame polluted by noise + and latency. + """ + delayed_orientation = np.array( + self._control_observation[3 * self.num_motors:3 * self.num_motors + 4]) + delayed_roll_pitch_yaw = self._pybullet_client.getEulerFromQuaternion( + delayed_orientation) + roll_pitch_yaw = self._AddSensorNoise( + np.array(delayed_roll_pitch_yaw), self._observation_noise_stdev[3]) + return roll_pitch_yaw + + def GetTrueMotorAngles(self): + """Gets the eight motor angles at the current moment, mapped to [-pi, pi]. + + Returns: + Motor angles, mapped to [-pi, pi]. + """ + motor_angles = [ + self._pybullet_client.getJointState(self.quadruped, motor_id)[0] + for motor_id in self._motor_id_list + ] + motor_angles = np.multiply(motor_angles, self._motor_direction) + return motor_angles + + def GetMotorAngles(self): + """Gets the eight motor angles. + + This function mimicks the noisy sensor reading and adds latency. The motor + angles that are delayed, noise polluted, and mapped to [-pi, pi]. + + Returns: + Motor angles polluted by noise and latency, mapped to [-pi, pi]. + """ + motor_angles = self._AddSensorNoise( + np.array(self._control_observation[0:self.num_motors]), + self._observation_noise_stdev[0]) + return MapToMinusPiToPi(motor_angles) + + def GetTrueMotorVelocities(self): + """Get the velocity of all eight motors. + + Returns: + Velocities of all eight motors. + """ + motor_velocities = [ + self._pybullet_client.getJointState(self.quadruped, motor_id)[1] + for motor_id in self._motor_id_list + ] + motor_velocities = np.multiply(motor_velocities, self._motor_direction) + return motor_velocities + + def GetMotorVelocities(self): + """Get the velocity of all eight motors. + + This function mimicks the noisy sensor reading and adds latency. + Returns: + Velocities of all eight motors polluted by noise and latency. + """ + return self._AddSensorNoise( + np.array( + self._control_observation[self.num_motors:2 * self.num_motors]), + self._observation_noise_stdev[1]) + + def GetTrueMotorTorques(self): + """Get the amount of torque the motors are exerting. + + Returns: + Motor torques of all eight motors. + """ + if self._accurate_motor_model_enabled or self._pd_control_enabled: + return self._observed_motor_torques + else: + motor_torques = [ + self._pybullet_client.getJointState(self.quadruped, motor_id)[3] + for motor_id in self._motor_id_list + ] + motor_torques = np.multiply(motor_torques, self._motor_direction) + return motor_torques + + def GetMotorTorques(self): + """Get the amount of torque the motors are exerting. + + This function mimicks the noisy sensor reading and adds latency. + Returns: + Motor torques of all eight motors polluted by noise and latency. + """ + return self._AddSensorNoise( + np.array( + self._control_observation[2 * self.num_motors:3 * self.num_motors]), + self._observation_noise_stdev[2]) + + def GetTrueBaseOrientation(self): + """Get the orientation of minitaur's base, represented as quaternion. + + Returns: + The orientation of minitaur's base. + """ + _, orientation = ( + self._pybullet_client.getBasePositionAndOrientation(self.quadruped)) + return orientation + + def GetBaseOrientation(self): + """Get the orientation of minitaur's base, represented as quaternion. + + This function mimicks the noisy sensor reading and adds latency. + Returns: + The orientation of minitaur's base polluted by noise and latency. + """ + return self._pybullet_client.getQuaternionFromEuler( + self.GetBaseRollPitchYaw()) + + def GetTrueBaseRollPitchYawRate(self): + """Get the rate of orientation change of the minitaur's base in euler angle. + + Returns: + rate of (roll, pitch, yaw) change of the minitaur's base. + """ + vel = self._pybullet_client.getBaseVelocity(self.quadruped) + return np.asarray([vel[1][0], vel[1][1], vel[1][2]]) + + def GetBaseRollPitchYawRate(self): + """Get the rate of orientation change of the minitaur's base in euler angle. + + This function mimicks the noisy sensor reading and adds latency. + Returns: + rate of (roll, pitch, yaw) change of the minitaur's base polluted by noise + and latency. + """ + return self._AddSensorNoise( + np.array(self._control_observation[3 * self.num_motors + 4: + 3 * self.num_motors + 7]), + self._observation_noise_stdev[4]) + + def GetActionDimension(self): + """Get the length of the action list. + + Returns: + The length of the action list. + """ + return self.num_motors + + def ApplyAction(self, motor_commands, motor_kps=None, motor_kds=None): + """Set the desired motor angles to the motors of the minitaur. + + The desired motor angles are clipped based on the maximum allowed velocity. + If the pd_control_enabled is True, a torque is calculated according to + the difference between current and desired joint angle, as well as the joint + velocity. This torque is exerted to the motor. For more information about + PD control, please refer to: https://en.wikipedia.org/wiki/PID_controller. + + Args: + motor_commands: The eight desired motor angles. + motor_kps: Proportional gains for the motor model. If not provided, it + uses the default kp of the minitaur for all the motors. + motor_kds: Derivative gains for the motor model. If not provided, it + uses the default kd of the minitaur for all the motors. + """ + if self._motor_velocity_limit < np.inf: + current_motor_angle = self.GetTrueMotorAngles() + motor_commands_max = ( + current_motor_angle + self.time_step * self._motor_velocity_limit) + motor_commands_min = ( + current_motor_angle - self.time_step * self._motor_velocity_limit) + motor_commands = np.clip(motor_commands, motor_commands_min, + motor_commands_max) + # Set the kp and kd for all the motors if not provided as an argument. + if motor_kps is None: + motor_kps = np.full(8, self._kp) + if motor_kds is None: + motor_kds = np.full(8, self._kd) + + if self._accurate_motor_model_enabled or self._pd_control_enabled: + q, qdot = self._GetPDObservation() + qdot_true = self.GetTrueMotorVelocities() + if self._accurate_motor_model_enabled: + actual_torque, observed_torque = self._motor_model.convert_to_torque( + motor_commands, q, qdot, qdot_true, motor_kps, motor_kds) + if self._motor_overheat_protection: + for i in range(self.num_motors): + if abs(actual_torque[i]) > OVERHEAT_SHUTDOWN_TORQUE: + self._overheat_counter[i] += 1 + else: + self._overheat_counter[i] = 0 + if (self._overheat_counter[i] > + OVERHEAT_SHUTDOWN_TIME / self.time_step): + self._motor_enabled_list[i] = False + + # The torque is already in the observation space because we use + # GetMotorAngles and GetMotorVelocities. + self._observed_motor_torques = observed_torque + + # Transform into the motor space when applying the torque. + self._applied_motor_torque = np.multiply(actual_torque, + self._motor_direction) + + for motor_id, motor_torque, motor_enabled in zip( + self._motor_id_list, self._applied_motor_torque, + self._motor_enabled_list): + if motor_enabled: + self._SetMotorTorqueById(motor_id, motor_torque) + else: + self._SetMotorTorqueById(motor_id, 0) + else: + torque_commands = -1 * motor_kps * ( + q - motor_commands) - motor_kds * qdot + + # The torque is already in the observation space because we use + # GetMotorAngles and GetMotorVelocities. + self._observed_motor_torques = torque_commands + + # Transform into the motor space when applying the torque. + self._applied_motor_torques = np.multiply(self._observed_motor_torques, + self._motor_direction) + + for motor_id, motor_torque in zip(self._motor_id_list, + self._applied_motor_torques): + self._SetMotorTorqueById(motor_id, motor_torque) + else: + motor_commands_with_direction = np.multiply(motor_commands, + self._motor_direction) + for motor_id, motor_command_with_direction in zip( + self._motor_id_list, motor_commands_with_direction): + self._SetDesiredMotorAngleById(motor_id, motor_command_with_direction) + + def ConvertFromLegModel(self, actions): + """Convert the actions that use leg model to the real motor actions. + + Args: + actions: The theta, phi of the leg model. + Returns: + The eight desired motor angles that can be used in ApplyActions(). + """ + motor_angle = copy.deepcopy(actions) + scale_for_singularity = 1 + offset_for_singularity = 1.5 + half_num_motors = int(self.num_motors / 2) + quater_pi = math.pi / 4 + for i in range(self.num_motors): + action_idx = int(i // 2) + forward_backward_component = (-scale_for_singularity * quater_pi * ( + actions[action_idx + half_num_motors] + offset_for_singularity)) + extension_component = (-1)**i * quater_pi * actions[action_idx] + if i >= half_num_motors: + extension_component = -extension_component + motor_angle[i] = ( + math.pi + forward_backward_component + extension_component) + return motor_angle + + def GetBaseMassesFromURDF(self): + """Get the mass of the base from the URDF file.""" + return self._base_mass_urdf + + def GetBaseInertiasFromURDF(self): + """Get the inertia of the base from the URDF file.""" + return self._base_inertia_urdf + + def GetLegMassesFromURDF(self): + """Get the mass of the legs from the URDF file.""" + return self._leg_masses_urdf + + def GetLegInertiasFromURDF(self): + """Get the inertia of the legs from the URDF file.""" + return self._leg_inertia_urdf + + def SetBaseMasses(self, base_mass): + """Set the mass of minitaur's base. + + Args: + base_mass: A list of masses of each body link in CHASIS_LINK_IDS. The + length of this list should be the same as the length of CHASIS_LINK_IDS. + Raises: + ValueError: It is raised when the length of base_mass is not the same as + the length of self._chassis_link_ids. + """ + if len(base_mass) != len(self._chassis_link_ids): + raise ValueError( + "The length of base_mass {} and self._chassis_link_ids {} are not " + "the same.".format(len(base_mass), len(self._chassis_link_ids))) + for chassis_id, chassis_mass in zip(self._chassis_link_ids, base_mass): + self._pybullet_client.changeDynamics( + self.quadruped, chassis_id, mass=chassis_mass) + + def SetLegMasses(self, leg_masses): + """Set the mass of the legs. + + A leg includes leg_link and motor. 4 legs contain 16 links (4 links each) + and 8 motors. First 16 numbers correspond to link masses, last 8 correspond + to motor masses (24 total). + + Args: + leg_masses: The leg and motor masses for all the leg links and motors. + + Raises: + ValueError: It is raised when the length of masses is not equal to number + of links + motors. + """ + if len(leg_masses) != len(self._leg_link_ids) + len(self._motor_link_ids): + raise ValueError("The number of values passed to SetLegMasses are " + "different than number of leg links and motors.") + for leg_id, leg_mass in zip(self._leg_link_ids, leg_masses): + self._pybullet_client.changeDynamics( + self.quadruped, leg_id, mass=leg_mass) + motor_masses = leg_masses[len(self._leg_link_ids):] + for link_id, motor_mass in zip(self._motor_link_ids, motor_masses): + self._pybullet_client.changeDynamics( + self.quadruped, link_id, mass=motor_mass) + + def SetBaseInertias(self, base_inertias): + """Set the inertias of minitaur's base. + + Args: + base_inertias: A list of inertias of each body link in CHASIS_LINK_IDS. + The length of this list should be the same as the length of + CHASIS_LINK_IDS. + Raises: + ValueError: It is raised when the length of base_inertias is not the same + as the length of self._chassis_link_ids and base_inertias contains + negative values. + """ + if len(base_inertias) != len(self._chassis_link_ids): + raise ValueError( + "The length of base_inertias {} and self._chassis_link_ids {} are " + "not the same.".format( + len(base_inertias), len(self._chassis_link_ids))) + for chassis_id, chassis_inertia in zip(self._chassis_link_ids, + base_inertias): + for inertia_value in chassis_inertia: + if (np.asarray(inertia_value) < 0).any(): + raise ValueError("Values in inertia matrix should be non-negative.") + self._pybullet_client.changeDynamics( + self.quadruped, chassis_id, localInertiaDiagonal=chassis_inertia) + + def SetLegInertias(self, leg_inertias): + """Set the inertias of the legs. + + A leg includes leg_link and motor. 4 legs contain 16 links (4 links each) + and 8 motors. First 16 numbers correspond to link inertia, last 8 correspond + to motor inertia (24 total). + + Args: + leg_inertias: The leg and motor inertias for all the leg links and motors. + + Raises: + ValueError: It is raised when the length of inertias is not equal to + the number of links + motors or leg_inertias contains negative values. + """ + + if len(leg_inertias) != len(self._leg_link_ids) + len(self._motor_link_ids): + raise ValueError("The number of values passed to SetLegMasses are " + "different than number of leg links and motors.") + for leg_id, leg_inertia in zip(self._leg_link_ids, leg_inertias): + for inertia_value in leg_inertias: + if (np.asarray(inertia_value) < 0).any(): + raise ValueError("Values in inertia matrix should be non-negative.") + self._pybullet_client.changeDynamics( + self.quadruped, leg_id, localInertiaDiagonal=leg_inertia) + + motor_inertias = leg_inertias[len(self._leg_link_ids):] + for link_id, motor_inertia in zip(self._motor_link_ids, motor_inertias): + for inertia_value in motor_inertias: + if (np.asarray(inertia_value) < 0).any(): + raise ValueError("Values in inertia matrix should be non-negative.") + self._pybullet_client.changeDynamics( + self.quadruped, link_id, localInertiaDiagonal=motor_inertia) + + def SetFootFriction(self, foot_friction): + """Set the lateral friction of the feet. + + Args: + foot_friction: The lateral friction coefficient of the foot. This value is + shared by all four feet. + """ + for link_id in self._foot_link_ids: + self._pybullet_client.changeDynamics( + self.quadruped, link_id, lateralFriction=foot_friction) + + # TODO(b/73748980): Add more API's to set other contact parameters. + def SetFootRestitution(self, foot_restitution): + """Set the coefficient of restitution at the feet. + + Args: + foot_restitution: The coefficient of restitution (bounciness) of the feet. + This value is shared by all four feet. + """ + for link_id in self._foot_link_ids: + self._pybullet_client.changeDynamics( + self.quadruped, link_id, restitution=foot_restitution) + + def SetJointFriction(self, joint_frictions): + for knee_joint_id, friction in zip(self._foot_link_ids, joint_frictions): + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=knee_joint_id, + controlMode=self._pybullet_client.VELOCITY_CONTROL, + targetVelocity=0, + force=friction) + + def GetNumKneeJoints(self): + return len(self._foot_link_ids) + + def SetBatteryVoltage(self, voltage): + if self._accurate_motor_model_enabled: + self._motor_model.set_voltage(voltage) + + def SetMotorViscousDamping(self, viscous_damping): + if self._accurate_motor_model_enabled: + self._motor_model.set_viscous_damping(viscous_damping) + + def GetTrueObservation(self): + observation = [] + observation.extend(self.GetTrueMotorAngles()) + observation.extend(self.GetTrueMotorVelocities()) + observation.extend(self.GetTrueMotorTorques()) + observation.extend(self.GetTrueBaseOrientation()) + observation.extend(self.GetTrueBaseRollPitchYawRate()) + return observation + + def ReceiveObservation(self): + """Receive the observation from sensors. + + This function is called once per step. The observations are only updated + when this function is called. + """ + self._observation_history.appendleft(self.GetTrueObservation()) + self._control_observation = self._GetControlObservation() + + def _GetDelayedObservation(self, latency): + """Get observation that is delayed by the amount specified in latency. + + Args: + latency: The latency (in seconds) of the delayed observation. + Returns: + observation: The observation which was actually latency seconds ago. + """ + if latency <= 0 or len(self._observation_history) == 1: + observation = self._observation_history[0] + else: + n_steps_ago = int(latency / self.time_step) + if n_steps_ago + 1 >= len(self._observation_history): + return self._observation_history[-1] + remaining_latency = latency - n_steps_ago * self.time_step + blend_alpha = remaining_latency / self.time_step + observation = ( + (1.0 - blend_alpha) * np.array(self._observation_history[n_steps_ago]) + + blend_alpha * np.array(self._observation_history[n_steps_ago + 1])) + return observation + + def _GetPDObservation(self): + pd_delayed_observation = self._GetDelayedObservation(self._pd_latency) + q = pd_delayed_observation[0:self.num_motors] + qdot = pd_delayed_observation[self.num_motors:2 * self.num_motors] + return (np.array(q), np.array(qdot)) + + def _GetControlObservation(self): + control_delayed_observation = self._GetDelayedObservation( + self._control_latency) + return control_delayed_observation + + def _AddSensorNoise(self, sensor_values, noise_stdev): + if noise_stdev <= 0: + return sensor_values + observation = sensor_values + np.random.normal( + scale=noise_stdev, size=sensor_values.shape) + return observation + + def SetControlLatency(self, latency): + """Set the latency of the control loop. + + It measures the duration between sending an action from Nvidia TX2 and + receiving the observation from microcontroller. + + Args: + latency: The latency (in seconds) of the control loop. + """ + self._control_latency = latency + + def GetControlLatency(self): + """Get the control latency. + + Returns: + The latency (in seconds) between when the motor command is sent and when + the sensor measurements are reported back to the controller. + """ + return self._control_latency + + def SetMotorGains(self, kp, kd): + """Set the gains of all motors. + + These gains are PD gains for motor positional control. kp is the + proportional gain and kd is the derivative gain. + + Args: + kp: proportional gain of the motors. + kd: derivative gain of the motors. + """ + self._kp = kp + self._kd = kd + if self._accurate_motor_model_enabled: + self._motor_model.set_motor_gains(kp, kd) + + def GetMotorGains(self): + """Get the gains of the motor. + + Returns: + The proportional gain. + The derivative gain. + """ + return self._kp, self._kd + + def SetMotorStrengthRatio(self, ratio): + """Set the strength of all motors relative to the default value. + + Args: + ratio: The relative strength. A scalar range from 0.0 to 1.0. + """ + if self._accurate_motor_model_enabled: + self._motor_model.set_strength_ratios([ratio] * self.num_motors) + + def SetMotorStrengthRatios(self, ratios): + """Set the strength of each motor relative to the default value. + + Args: + ratios: The relative strength. A numpy array ranging from 0.0 to 1.0. + """ + if self._accurate_motor_model_enabled: + self._motor_model.set_strength_ratios(ratios) + + def SetTimeSteps(self, action_repeat, simulation_step): + """Set the time steps of the control and simulation. + + Args: + action_repeat: The number of simulation steps that the same action is + repeated. + simulation_step: The simulation time step. + """ + self.time_step = simulation_step + self._action_repeat = action_repeat + + @property + def chassis_link_ids(self): + return self._chassis_link_ids diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_alternating_legs_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_alternating_legs_env.py new file mode 100644 index 000000000..82d64b184 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_alternating_legs_env.py @@ -0,0 +1,244 @@ +"""This file implements the gym environment of minitaur alternating legs. + +""" +import math + +from gym import spaces +import numpy as np +import minitaur_gym_env + +INIT_EXTENSION_POS = 2.6 +INIT_SWING_POS = 0.0 +DESIRED_PITCH = 0 +NUM_LEGS = 4 +NUM_MOTORS = 2 * NUM_LEGS +STEP_PERIOD = 1.0 / 3.0 # Three steps per second. +STEP_AMPLITUDE = 0.75 + + +class MinitaurAlternatingLegsEnv(minitaur_gym_env.MinitaurGymEnv): + """The gym environment for the minitaur. + + It simulates the locomotion of a minitaur, a quadruped robot. The state space + include the angles, velocities and torques for all the motors and the action + space is the desired motor angle for each motor. The reward function is based + on how far the minitaur walks in 1000 steps and penalizes the energy + expenditure. + + """ + metadata = { + "render.modes": ["human", "rgb_array"], + "video.frames_per_second": 66 + } + + def __init__(self, + urdf_version=None, + control_time_step=0.006, + action_repeat=6, + control_latency=0, + pd_latency=0, + on_rack=False, + motor_kp=1.0, + motor_kd=0.02, + remove_default_joint_damping=False, + render=False, + num_steps_to_log=1000, + env_randomizer=None, + log_path=None): + """Initialize the minitaur alternating legs gym environment. + + Args: + urdf_version: [DEFAULT_URDF_VERSION, DERPY_V0_URDF_VERSION] are allowable + versions. If None, DEFAULT_URDF_VERSION is used. Refer to + minitaur_gym_env for more details. + control_time_step: The time step between two successive control signals. + action_repeat: The number of simulation steps that an action is repeated. + control_latency: The latency between get_observation() and the actual + observation. See minituar.py for more details. + pd_latency: The latency used to get motor angles/velocities used to + compute PD controllers. See minitaur.py for more details. + on_rack: Whether to place the minitaur on rack. This is only used to debug + the walking gait. In this mode, the minitaur's base is hung midair so + that its walking gait is clearer to visualize. + motor_kp: The P gain of the motor. + motor_kd: The D gain of the motor. + remove_default_joint_damping: Whether to remove the default joint damping. + render: Whether to render the simulation. + num_steps_to_log: The max number of control steps in one episode. If the + number of steps is over num_steps_to_log, the environment will still + be running, but only first num_steps_to_log will be recorded in logging. + env_randomizer: An instance (or a list) of EnvRanzomier(s) that can + randomize the environment during when env.reset() is called and add + perturbations when env._step() is called. + log_path: The path to write out logs. For the details of logging, refer to + minitaur_logging.proto. + """ + # _swing_offset and _extension_offset is to mimick the bent legs. + self._swing_offset = np.zeros(NUM_LEGS) + self._extension_offset = np.zeros(NUM_LEGS) + super(MinitaurAlternatingLegsEnv, self).__init__( + urdf_version=urdf_version, + accurate_motor_model_enabled=True, + motor_overheat_protection=True, + hard_reset=False, + motor_kp=motor_kp, + motor_kd=motor_kd, + remove_default_joint_damping=remove_default_joint_damping, + control_latency=control_latency, + pd_latency=pd_latency, + on_rack=on_rack, + render=render, + num_steps_to_log=num_steps_to_log, + env_randomizer=env_randomizer, + log_path=log_path, + control_time_step=control_time_step, + action_repeat=action_repeat) + + action_dim = 8 + action_high = np.array([0.1] * action_dim) + self.action_space = spaces.Box(-action_high, action_high) + + self._cam_dist = 1.0 + self._cam_yaw = 30 + self._cam_pitch = -30 + + def _reset(self): + self.desired_pitch = DESIRED_PITCH + # In this environment, the actions are + # [swing leg 1, swing leg 2, swing leg 3, swing leg 4, + # extension leg 1, extension leg 2, extension leg 3, extension leg 4] + init_pose = [ + INIT_SWING_POS + self._swing_offset[0], + INIT_SWING_POS + self._swing_offset[1], + INIT_SWING_POS + self._swing_offset[2], + INIT_SWING_POS + self._swing_offset[3], + INIT_EXTENSION_POS + self._extension_offset[0], + INIT_EXTENSION_POS + self._extension_offset[1], + INIT_EXTENSION_POS + self._extension_offset[2], + INIT_EXTENSION_POS + self._extension_offset[3] + ] + initial_motor_angles = self._convert_from_leg_model(init_pose) + super(MinitaurAlternatingLegsEnv, self)._reset( + initial_motor_angles=initial_motor_angles, reset_duration=0.5) + return self._get_observation() + + def _convert_from_leg_model(self, leg_pose): + motor_pose = np.zeros(NUM_MOTORS) + for i in range(NUM_LEGS): + motor_pose[2 * i] = leg_pose[NUM_LEGS + i] - (-1)**(i / 2) * leg_pose[i] + motor_pose[2 * i + + 1] = leg_pose[NUM_LEGS + i] + (-1)**(i / 2) * leg_pose[i] + return motor_pose + + def _signal(self, t): + initial_pose = np.array([ + INIT_SWING_POS, INIT_SWING_POS, INIT_SWING_POS, INIT_SWING_POS, + INIT_EXTENSION_POS, INIT_EXTENSION_POS, INIT_EXTENSION_POS, + INIT_EXTENSION_POS + ]) + amplitude = STEP_AMPLITUDE + period = STEP_PERIOD + extension = amplitude * (-1.0 + math.cos(2 * math.pi / period * t)) + ith_leg = int(t / period) % 2 + first_leg = np.array([0, 0, 0, 0, 0, extension, extension, 0]) + second_leg = np.array([0, 0, 0, 0, extension, 0, 0, extension]) + if ith_leg: + signal = initial_pose + second_leg + else: + signal = initial_pose + first_leg + return signal + + def _transform_action_to_motor_command(self, action): + # Add swing_offset and extension_offset to mimick the bent legs. + action[0:4] += self._swing_offset + action[4:8] += self._extension_offset + action += self._signal(self.minitaur.GetTimeSinceReset()) + action = self._convert_from_leg_model(action) + return action + + def is_fallen(self): + """Decide whether the minitaur has fallen. + + If the up directions between the base and the world is large (the dot + product is smaller than 0.85), the minitaur is considered fallen. + + Returns: + Boolean value that indicates whether the minitaur has fallen. + """ + orientation = self.minitaur.GetBaseOrientation() + rot_mat = self._pybullet_client.getMatrixFromQuaternion(orientation) + local_up = rot_mat[6:] + return np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.85 + + def _reward(self): + return 1.0 + + def _get_true_observation(self): + """Get the true observations of this environment. + + It includes the roll, the error between current pitch and desired pitch, + roll dot and pitch dot of the base. + + Returns: + The observation list. + """ + observation = [] + roll, pitch, _ = self.minitaur.GetTrueBaseRollPitchYaw() + roll_rate, pitch_rate, _ = self.minitaur.GetTrueBaseRollPitchYawRate() + observation.extend([roll, pitch, roll_rate, pitch_rate]) + observation[1] -= self.desired_pitch # observation[1] is the pitch + self._true_observation = np.array(observation) + return self._true_observation + + def _get_observation(self): + observation = [] + roll, pitch, _ = self.minitaur.GetBaseRollPitchYaw() + roll_rate, pitch_rate, _ = self.minitaur.GetBaseRollPitchYawRate() + observation.extend([roll, pitch, roll_rate, pitch_rate]) + observation[1] -= self.desired_pitch # observation[1] is the pitch + self._observation = np.array(observation) + return self._observation + + def _get_observation_upper_bound(self): + """Get the upper bound of the observation. + + Returns: + The upper bound of an observation. See GetObservation() for the details + of each element of an observation. + """ + upper_bound = np.zeros(self._get_observation_dimension()) + upper_bound[0:2] = 2 * math.pi # Roll, pitch, yaw of the base. + upper_bound[2:4] = 2 * math.pi / self._time_step # Roll, pitch, yaw rate. + return upper_bound + + def _get_observation_lower_bound(self): + lower_bound = -self._get_observation_upper_bound() + return lower_bound + + def set_swing_offset(self, value): + """Set the swing offset of each leg. + + It is to mimic the bent leg. + + Args: + value: A list of four values. + """ + self._swing_offset = value + + def set_extension_offset(self, value): + """Set the extension offset of each leg. + + It is to mimic the bent leg. + + Args: + value: A list of four values. + """ + self._extension_offset = value + + def set_desired_pitch(self, value): + """Set the desired pitch of the base, which is a user input. + + Args: + value: A scalar. + """ + self.desired_pitch = value diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_alternating_legs_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_alternating_legs_env_example.py new file mode 100644 index 000000000..aa467133c --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_alternating_legs_env_example.py @@ -0,0 +1,109 @@ +"""An example to run the minitaur environment of alternating legs. + +""" +import time + + +import numpy as np +import tensorflow as tf +import minitaur_alternating_legs_env +import minitaur_gym_env +from env_randomizers import minitaur_alternating_legs_env_randomizer as randomizer_lib + +#FLAGS = flags.FLAGS +#flags.DEFINE_string("log_path", None, "The directory to write the log file.") + + +def hand_tuned_agent(observation, timestamp): + """A hand tuned controller structure with vizier optimized parameters. + + Args: + observation: The observation of the environment. It includes the roll, pith + the speed of roll and pitch changes. + timestamp: The simulated time since the simulation reset. + Returns: + Delta desired motor angles to be added to the reference motion of + alternating legs for balance. + """ + roll = observation[0] + pitch = observation[1] + roll_dot = observation[2] + pitch_dot = observation[3] + + # The following gains are hand-tuned. These gains are + # designed according to traditional robotics techniques. These are linear + # feedback balance conroller. The idea is that when the base is tilting, + # the legs in the air should swing more towards the falling direction to catch + # up falling. At the same time, the legs in the air should extend more to + # touch ground earlier. + roll_gain = 1.0 + pitch_gain = 1.0 + roll_dot_gain = 0.1 + pitch_dot_gain = 0.1 + + roll_compensation = roll_gain * roll + roll_dot_gain * roll_dot + pitch_compensation = pitch_gain * pitch + pitch_dot_gain * pitch_dot + + first_leg = [ + 0, -pitch_compensation, -pitch_compensation, 0, 0, + -pitch_compensation - roll_compensation, + pitch_compensation + roll_compensation, 0 + ] + second_leg = [ + -pitch_compensation, 0, 0, -pitch_compensation, + pitch_compensation - roll_compensation, 0, 0, + -pitch_compensation + roll_compensation + ] + if (timestamp // minitaur_alternating_legs_env.STEP_PERIOD) % 2: + return second_leg + else: + return first_leg + + +def hand_tuned_balance_example(log_path=None): + """An example that the minitaur balances while alternating its legs. + + Args: + log_path: The directory that the log files are written to. If log_path is + None, no logs will be written. + """ + steps = 1000 + episodes = 5 + + randomizer = randomizer_lib.MinitaurAlternatingLegsEnvRandomizer() + environment = minitaur_alternating_legs_env.MinitaurAlternatingLegsEnv( + urdf_version=minitaur_gym_env.DERPY_V0_URDF_VERSION, + render=True, + num_steps_to_log=steps, + pd_latency=0.002, + control_latency=0.02, + remove_default_joint_damping=True, + on_rack=False, + env_randomizer=randomizer, + log_path=log_path) + np.random.seed(100) + avg_reward = 0 + for i in xrange(episodes): + sum_reward = 0 + observation = environment.reset() + for _ in xrange(steps): + # Sleep to prevent serial buffer overflow on microcontroller. + time.sleep(0.002) + action = hand_tuned_agent(observation, + environment.minitaur.GetTimeSinceReset()) + observation, reward, done, _ = environment.step(action) + sum_reward += reward + if done: + break + tf.logging.info("reward {}: {}".format(i, sum_reward)) + avg_reward += sum_reward + tf.logging.info("avg_reward: {}\n\n\n".format(avg_reward / episodes)) + + +def main(unused_argv): + hand_tuned_balance_example(log_path=FLAGS.log_path) + + +if __name__ == "__main__": + tf.logging.set_verbosity(tf.logging.INFO) + tf.app.run() diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_ball_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_ball_gym_env.py new file mode 100644 index 000000000..46522b076 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_ball_gym_env.py @@ -0,0 +1,147 @@ +"""This file implements the gym environment of minitaur. + +""" +import math +import random + +from gym import spaces +import numpy as np +import minitaur_gym_env +import pybullet_data + +GOAL_DISTANCE_THRESHOLD = 0.8 +GOAL_REWARD = 1000.0 +REWARD_SCALING = 1e-3 +INIT_BALL_ANGLE = math.pi / 3 +INIT_BALL_DISTANCE = 5.0 +ACTION_EPS = 0.01 + + +class MinitaurBallGymEnv(minitaur_gym_env.MinitaurGymEnv): + """The gym environment for the minitaur and a ball. + + It simulates a minitaur (a quadruped robot) and a ball. The state space + includes the angle and distance of the ball relative to minitaur's base. + The action space is a steering command. The reward function is based + on how far the ball is relative to the minitaur's base. + + """ + + def __init__(self, + urdf_root=pybullet_data.getDataPath(), + self_collision_enabled=True, + pd_control_enabled=False, + leg_model_enabled=True, + on_rack=False, + render=False): + """Initialize the minitaur and ball gym environment. + + Args: + urdf_root: The path to the urdf data folder. + self_collision_enabled: Whether to enable self collision in the sim. + pd_control_enabled: Whether to use PD controller for each motor. + leg_model_enabled: Whether to use a leg motor to reparameterize the action + space. + on_rack: Whether to place the minitaur on rack. This is only used to debug + the walking gait. In this mode, the minitaur's base is hanged midair so + that its walking gait is clearer to visualize. + render: Whether to render the simulation. + """ + super(MinitaurBallGymEnv, self).__init__( + urdf_root=urdf_root, + self_collision_enabled=self_collision_enabled, + pd_control_enabled=pd_control_enabled, + leg_model_enabled=leg_model_enabled, + on_rack=on_rack, + render=render) + self._cam_dist = 2.0 + self._cam_yaw = -70 + self._cam_pitch = -30 + self.action_space = spaces.Box(np.array([-1]), np.array([1])) + self.observation_space = spaces.Box(np.array([-math.pi, 0]), + np.array([math.pi, 100])) + + def _reset(self): + self._ball_id = 0 + super(MinitaurBallGymEnv, self)._reset() + self._init_ball_theta = random.uniform(-INIT_BALL_ANGLE, INIT_BALL_ANGLE) + self._init_ball_distance = INIT_BALL_DISTANCE + self._ball_pos = [self._init_ball_distance * + math.cos(self._init_ball_theta), + self._init_ball_distance * + math.sin(self._init_ball_theta), 1] + self._ball_id = self._pybullet_client.loadURDF( + "%s/sphere_with_restitution.urdf" % self._urdf_root, self._ball_pos) + return self._get_observation() + + def _get_observation(self): + world_translation_minitaur, world_rotation_minitaur = ( + self._pybullet_client.getBasePositionAndOrientation( + self.minitaur.quadruped)) + world_translation_ball, world_rotation_ball = ( + self._pybullet_client.getBasePositionAndOrientation(self._ball_id)) + minitaur_translation_world, minitaur_rotation_world = ( + self._pybullet_client.invertTransform(world_translation_minitaur, + world_rotation_minitaur)) + minitaur_translation_ball, _ = ( + self._pybullet_client.multiplyTransforms(minitaur_translation_world, + minitaur_rotation_world, + world_translation_ball, + world_rotation_ball)) + distance = math.sqrt(minitaur_translation_ball[0]**2 + + minitaur_translation_ball[1]**2) + angle = math.atan2(minitaur_translation_ball[0], + minitaur_translation_ball[1]) + self._observation = [angle - math.pi / 2, distance] + return self._observation + + def _transform_action_to_motor_command(self, action): + if self._leg_model_enabled: + for i, action_component in enumerate(action): + if not (-self._action_bound - ACTION_EPS <= + action_component <= self._action_bound + ACTION_EPS): + raise ValueError("{}th action {} out of bounds.".format + (i, action_component)) + action = self._apply_steering_to_locomotion(action) + action = self.minitaur.ConvertFromLegModel(action) + return action + + def _apply_steering_to_locomotion(self, action): + # A hardcoded feedforward walking controller based on sine functions. + amplitude_swing = 0.5 + amplitude_extension = 0.5 + speed = 200 + steering_amplitude = 0.5 * action[0] + t = self.minitaur.GetTimeSinceReset() + a1 = math.sin(t * speed) * (amplitude_swing + steering_amplitude) + a2 = math.sin(t * speed + math.pi) * (amplitude_swing - steering_amplitude) + a3 = math.sin(t * speed) * amplitude_extension + a4 = math.sin(t * speed + math.pi) * amplitude_extension + action = [a1, a2, a2, a1, a3, a4, a4, a3] + return action + + def _distance_to_ball(self): + world_translation_minitaur, _ = ( + self._pybullet_client.getBasePositionAndOrientation( + self.minitaur.quadruped)) + world_translation_ball, _ = ( + self._pybullet_client.getBasePositionAndOrientation( + self._ball_id)) + distance = math.sqrt( + (world_translation_ball[0] - world_translation_minitaur[0])**2 + + (world_translation_ball[1] - world_translation_minitaur[1])**2) + return distance + + def _goal_state(self): + return self._observation[1] < GOAL_DISTANCE_THRESHOLD + + def _reward(self): + reward = -self._observation[1] + if self._goal_state(): + reward += GOAL_REWARD + return reward * REWARD_SCALING + + def _termination(self): + if self._goal_state(): + return True + return False diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_ball_gym_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_ball_gym_env_example.py new file mode 100644 index 000000000..b52b945ad --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_ball_gym_env_example.py @@ -0,0 +1,34 @@ +"""An example to run the gym environment that a minitaur follows a ball. + +""" + +import math + +import tensorflow as tf +import minitaur_ball_gym_env + + +def FollowBallManualPolicy(): + """An example of a minitaur following a ball.""" + env = minitaur_ball_gym_env.MinitaurBallGymEnv(render=True, + pd_control_enabled=True, + on_rack=False) + observation = env.reset() + sum_reward = 0 + steps = 100000 + for _ in range(steps): + action = [math.tanh(observation[0] * 4)] + observation, reward, done, _ = env.step(action) + sum_reward += reward + if done: + tf.logging.info("Return is {}".format(sum_reward)) + observation = env.reset() + sum_reward = 0 + + +def main(): + FollowBallManualPolicy() + + +if __name__ == '__main__': + main()
\ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_derpy.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_derpy.py new file mode 100644 index 000000000..f7bf10dac --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_derpy.py @@ -0,0 +1,175 @@ +"""This file implements the functionalities of a minitaur derpy using pybullet. + +It is the result of first pass system identification for the derpy robot. The + + +""" +import math + +import numpy as np +import minitaur + +KNEE_CONSTRAINT_POINT_LONG = [0, 0.0055, 0.088] +KNEE_CONSTRAINT_POINT_SHORT = [0, 0.0055, 0.100] + + +class MinitaurDerpy(minitaur.Minitaur): + """The minitaur class that simulates a quadruped robot from Ghost Robotics. + + """ + + def Reset(self, reload_urdf=True, default_motor_angles=None, reset_time=3.0): + """Reset the minitaur to its initial states. + + Args: + reload_urdf: Whether to reload the urdf file. If not, Reset() just place + the minitaur back to its starting position. + default_motor_angles: The default motor angles. If it is None, minitaur + will hold a default pose (motor angle math.pi / 2) for 100 steps. In + torque control mode, the phase of holding the default pose is skipped. + reset_time: The duration (in seconds) to hold the default motor angles. If + reset_time <= 0 or in torque control mode, the phase of holding the + default pose is skipped. + """ + if self._on_rack: + init_position = minitaur.INIT_RACK_POSITION + else: + init_position = minitaur.INIT_POSITION + if reload_urdf: + if self._self_collision_enabled: + self.quadruped = self._pybullet_client.loadURDF( + "%s/quadruped/minitaur_derpy.urdf" % self._urdf_root, + init_position, + useFixedBase=self._on_rack, + flags=( + self._pybullet_client.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)) + else: + self.quadruped = self._pybullet_client.loadURDF( + "%s/quadruped/minitaur_derpy.urdf" % self._urdf_root, + init_position, + useFixedBase=self._on_rack) + self._BuildJointNameToIdDict() + self._BuildUrdfIds() + if self._remove_default_joint_damping: + self._RemoveDefaultJointDamping() + self._BuildMotorIdList() + self._RecordMassInfoFromURDF() + self._RecordInertiaInfoFromURDF() + self.ResetPose(add_constraint=True) + else: + self._pybullet_client.resetBasePositionAndOrientation( + self.quadruped, init_position, minitaur.INIT_ORIENTATION) + self._pybullet_client.resetBaseVelocity(self.quadruped, [0, 0, 0], + [0, 0, 0]) + self.ResetPose(add_constraint=False) + + self._overheat_counter = np.zeros(self.num_motors) + self._motor_enabled_list = [True] * self.num_motors + self._step_counter = 0 + + # Perform reset motion within reset_duration if in position control mode. + # Nothing is performed if in torque control mode for now. + # TODO(jietan): Add reset motion when the torque control is fully supported. + self._observation_history.clear() + if not self._torque_control_enabled and reset_time > 0.0: + self.ReceiveObservation() + for _ in range(100): + self.ApplyAction([math.pi / 2] * self.num_motors) + self._pybullet_client.stepSimulation() + self.ReceiveObservation() + if default_motor_angles is not None: + num_steps_to_reset = int(reset_time / self.time_step) + for _ in range(num_steps_to_reset): + self.ApplyAction(default_motor_angles) + self._pybullet_client.stepSimulation() + self.ReceiveObservation() + self.ReceiveObservation() + + def _ResetPoseForLeg(self, leg_id, add_constraint): + """Reset the initial pose for the leg. + + Args: + leg_id: It should be 0, 1, 2, or 3, which represents the leg at + front_left, back_left, front_right and back_right. + add_constraint: Whether to add a constraint at the joints of two feet. + """ + knee_friction_force = 0 + half_pi = math.pi / 2.0 + knee_angle = -2.1834 + + leg_position = minitaur.LEG_POSITION[leg_id] + self._pybullet_client.resetJointState( + self.quadruped, + self._joint_name_to_id["motor_" + leg_position + "L_joint"], + self._motor_direction[2 * leg_id] * half_pi, + targetVelocity=0) + self._pybullet_client.resetJointState( + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "L_joint"], + self._motor_direction[2 * leg_id] * knee_angle, + targetVelocity=0) + self._pybullet_client.resetJointState( + self.quadruped, + self._joint_name_to_id["motor_" + leg_position + "R_joint"], + self._motor_direction[2 * leg_id + 1] * half_pi, + targetVelocity=0) + self._pybullet_client.resetJointState( + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "R_joint"], + self._motor_direction[2 * leg_id + 1] * knee_angle, + targetVelocity=0) + if add_constraint: + if leg_id < 2: + self._pybullet_client.createConstraint( + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "R_joint"], + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "L_joint"], + self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0], + KNEE_CONSTRAINT_POINT_SHORT, KNEE_CONSTRAINT_POINT_LONG) + else: + self._pybullet_client.createConstraint( + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "R_joint"], + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "L_joint"], + self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0], + KNEE_CONSTRAINT_POINT_LONG, KNEE_CONSTRAINT_POINT_SHORT) + + if self._accurate_motor_model_enabled or self._pd_control_enabled: + # Disable the default motor in pybullet. + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=( + self._joint_name_to_id["motor_" + leg_position + "L_joint"]), + controlMode=self._pybullet_client.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=( + self._joint_name_to_id["motor_" + leg_position + "R_joint"]), + controlMode=self._pybullet_client.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) + + else: + self._SetDesiredMotorAngleByName( + "motor_" + leg_position + "L_joint", + self._motor_direction[2 * leg_id] * half_pi) + self._SetDesiredMotorAngleByName( + "motor_" + leg_position + "R_joint", + self._motor_direction[2 * leg_id + 1] * half_pi) + + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=(self._joint_name_to_id["knee_" + leg_position + "L_joint"]), + controlMode=self._pybullet_client.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=(self._joint_name_to_id["knee_" + leg_position + "R_joint"]), + controlMode=self._pybullet_client.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_four_leg_stand_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_four_leg_stand_env.py new file mode 100644 index 000000000..12a792206 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_four_leg_stand_env.py @@ -0,0 +1,359 @@ +"""This file implements the gym environment of minitaur standing with four legs. + +""" +import math +import operator +import random +import time +from gym import spaces +import numpy as np +from pybullet_envs.minitaur.envs import minitaur_gym_env +from pybullet_envs.minitaur.envs import minitaur_logging + +INIT_EXTENSION_POS = 2.0 +INIT_SWING_POS = 0.0 +DESIRED_PITCH = 0 +NUM_LEGS = 4 +NUM_MOTORS = 2 * NUM_LEGS +STEP_PERIOD = 1.0 / 3.0 # Three steps per second. +STEP_AMPLITUDE = 0.75 +PERTURBATION_TOTAL_STEP = 100 +MOVING_FLOOR_TOTAL_STEP = 100 +DERPY_V0_URDF_VERSION = minitaur_gym_env.DERPY_V0_URDF_VERSION +RAINBOW_DASH_V0_URDF_VERSION = minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION + + +class MinitaurFourLegStandEnv(minitaur_gym_env.MinitaurGymEnv): + """The gym environment for the minitaur. + + It simulates the a minitaur standing with four legs. The state space + include the orientation of the torso, and the action space is the desired + motor angle for each motor. The reward function is based on how close the + action to zero and the height of the robot base. It prefers a similar pose to + the signal while keeping balance. + """ + metadata = { + "render.modes": ["human", "rgb_array"], + "video.frames_per_second": 166 + } + + def __init__(self, + urdf_version=None, + hard_reset=True, + remove_default_joint_damping=True, + control_latency=0.0, + pd_latency=0.0, + on_rack=False, + motor_kp=1.0, + motor_kd=0.02, + render=False, + env_randomizer=None, + use_angular_velocity_in_observation=False, + use_motor_angle_in_observation=False, + control_time_step=0.006, + action_repeat=6, + log_path=None): + """Initialize the minitaur alternating legs gym environment. + + Args: + urdf_version: [DEFAULT_URDF_VERSION, DERPY_V0_URDF_VERSION, + RAINBOW_DASH_V0_URDF_VERSION] are allowable + versions. If None, DEFAULT_URDF_VERSION is used. DERPY_V0_URDF_VERSION + is the result of first pass system identification for derpy. + We will have a different URDF and related Minitaur class each time we + perform system identification. While the majority of the code of the + class remains the same, some code changes (e.g. the constraint location + might change). __init__() will choose the right Minitaur class from + different minitaur modules based on urdf_version. + hard_reset: Whether to wipe the simulation and load everything when reset + is called. If set to false, reset just place the minitaur back to start + position and set its pose to initial configuration. + remove_default_joint_damping: Whether to remove the default joint damping. + control_latency: It is the delay in the controller between when an + observation is made at some point, and when that reading is reported + back to the Neural Network. + pd_latency: latency of the PD controller loop. PD calculates PWM based on + the motor angle and velocity. The latency measures the time between when + the motor angle and velocity are observed on the microcontroller and + when the true state happens on the motor. It is typically (0.001- + 0.002s). + on_rack: Whether to place the minitaur on rack. This is only used to debug + the walking gait. In this mode, the minitaur's base is hung midair so + that its walking gait is clearer to visualize. + motor_kp: The P gain of the motor. + motor_kd: The D gain of the motor. + render: Whether to render the simulation. + env_randomizer: An instance (or a list) of EnvRanzomier(s) that can + randomize the environment during when env.reset() is called and add + perturbations when env._step() is called. + use_angular_velocity_in_observation: Whether to include roll_dot and + pitch_dot of the base in the observation. + use_motor_angle_in_observation: Whether to include motor angles in the + observation. + control_time_step: The time step between two successive control signals. + action_repeat: The number of simulation steps before actions are applied. + log_path: The path to write out logs. For the details of logging, refer to + minitaur_logging.proto. + """ + # _swing_offset and _extension_offset is to mimic the motor zero-calibration + # errors. + self._swing_offset = np.zeros(NUM_LEGS) + self._extension_offset = np.zeros(NUM_LEGS) + self._use_angular_velocity_in_observation = use_motor_angle_in_observation + self._use_motor_angle_in_observation = use_motor_angle_in_observation + super(MinitaurFourLegStandEnv, self).__init__( + urdf_version=urdf_version, + control_time_step=control_time_step, + action_repeat=action_repeat, + remove_default_joint_damping=remove_default_joint_damping, + accurate_motor_model_enabled=True, + motor_overheat_protection=True, + hard_reset=hard_reset, + motor_kp=motor_kp, + motor_kd=motor_kd, + control_latency=control_latency, + pd_latency=pd_latency, + on_rack=on_rack, + render=render, + env_randomizer=env_randomizer, + log_path=log_path) + + action_dim = 4 + action_low = np.array([-1.0] * action_dim) + action_high = -action_low + self.action_space = spaces.Box(action_low, action_high) + + self._cam_dist = 1.0 + self._cam_yaw = 30 + self._cam_pitch = -30 + self._perturbation_magnitude = 0.0 + self._sign = 1.0 + self._cur_ori = [0, 0, 0, 1] + self._goal_ori = [0, 0, 0, 1] + + def _reset(self): + self.desired_pitch = DESIRED_PITCH + # In this environment, the actions are + # [swing leg 1, swing leg 2, swing leg 3, swing leg 4, + # extension leg 1, extension leg 2, extension leg 3, extension leg 4] + init_pose = [ + INIT_SWING_POS + self._swing_offset[0], + INIT_SWING_POS + self._swing_offset[1], + INIT_SWING_POS + self._swing_offset[2], + INIT_SWING_POS + self._swing_offset[3], + INIT_EXTENSION_POS + self._extension_offset[0], + INIT_EXTENSION_POS + self._extension_offset[1], + INIT_EXTENSION_POS + self._extension_offset[2], + INIT_EXTENSION_POS + self._extension_offset[3] + ] + initial_motor_angles = self._convert_from_leg_model(init_pose) + self._pybullet_client.resetBasePositionAndOrientation( + 0, [0, 0, 0], [0, 0, 0, 1]) + super(MinitaurFourLegStandEnv, self)._reset( + initial_motor_angles=initial_motor_angles, reset_duration=0.5) + return self._get_observation() + + def _step(self, action): + """Step forward the simulation, given the action. + + Args: + action: A list of desired motor angles for eight motors. + + Returns: + observations: Roll, pitch of the base, and roll, pitch rate. + reward: The reward for the current state-action pair. + done: Whether the episode has ended. + info: A dictionary that stores diagnostic information. + + Raises: + ValueError: The action dimension is not the same as the number of motors. + ValueError: The magnitude of actions is out of bounds. + """ + if self._is_render: + # Sleep, otherwise the computation takes less time than real time, + # which will make the visualization like a fast-forward video. + time_spent = time.time() - self._last_frame_time + self._last_frame_time = time.time() + time_to_sleep = self.control_time_step - time_spent + if time_to_sleep > 0: + time.sleep(time_to_sleep) + base_pos = self.minitaur.GetBasePosition() + # Keep the previous orientation of the camera set by the user. + [yaw, pitch, + dist] = self._pybullet_client.getDebugVisualizerCamera()[8:11] + self._pybullet_client.resetDebugVisualizerCamera(dist, yaw, pitch, + base_pos) + action = self._transform_action_to_motor_command(action) + t = self._env_step_counter % MOVING_FLOOR_TOTAL_STEP + if t == 0: + self._seed() + orientation_x = random.uniform(-0.2, 0.2) + self._seed() + orientation_y = random.uniform(-0.2, 0.2) + _, self._cur_ori = self._pybullet_client.getBasePositionAndOrientation(0) + self._goal_ori = self._pybullet_client.getQuaternionFromEuler( + [orientation_x, orientation_y, 0]) + t = float(float(t) / float(MOVING_FLOOR_TOTAL_STEP)) + ori = map(operator.add, [x * (1.0 - t) for x in self._cur_ori], + [x * t for x in self._goal_ori]) + self._pybullet_client.resetBasePositionAndOrientation(0, [0, 0, 0], ori) + if self._env_step_counter % PERTURBATION_TOTAL_STEP == 0: + self._perturbation_magnitude = random.uniform(0.0, 0.0) + if self._sign < 0.5: + self._sign = 1.0 + else: + self._sign = 0.0 + self._pybullet_client.applyExternalForce( + objectUniqueId=1, + linkIndex=-1, + forceObj=[self._sign * self._perturbation_magnitude, 0.0, 0.0], + posObj=[0.0, 0.0, 0.0], + flags=self._pybullet_client.LINK_FRAME) + self.minitaur.Step(action) + self._env_step_counter += 1 + done = self._termination() + obs = self._get_true_observation() + reward = self._reward(action, obs) + if self._log_path is not None: + minitaur_logging.update_episode_proto(self._episode_proto, self.minitaur, + action, self._env_step_counter) + if done: + self.minitaur.Terminate() + return np.array(self._get_observation()), reward, done, {} + + def _convert_from_leg_model(self, leg_pose): + motor_pose = np.zeros(NUM_MOTORS) + for i in range(NUM_LEGS): + motor_pose[2 * i] = leg_pose[NUM_LEGS + i] - (-1)**(i / 2) * leg_pose[i] + motor_pose[2 * i + + 1] = leg_pose[NUM_LEGS + i] + (-1)**(i / 2) * leg_pose[i] + return motor_pose + + def _signal(self, t): + initial_pose = np.array([ + INIT_SWING_POS, INIT_SWING_POS, INIT_SWING_POS, INIT_SWING_POS, + INIT_EXTENSION_POS, INIT_EXTENSION_POS, INIT_EXTENSION_POS, + INIT_EXTENSION_POS + ]) + signal = initial_pose + return signal + + def _transform_action_to_motor_command(self, action): + # Add swing_offset and extension_offset to mimick the motor zero-calibration + # errors. + real_action = np.array([0.0] * 8) + real_action[4:8] = action + self._extension_offset + real_action[0:4] = self._swing_offset + real_action += self._signal(self.minitaur.GetTimeSinceReset()) + real_action = self._convert_from_leg_model(real_action) + return real_action + + def is_fallen(self): + """Decide whether the minitaur has fallen. + + # TODO(yunfeibai): choose the fallen option for force perturbation and + moving floor, and update the comments. + + If the up directions between the base and the world is large (the dot + product is smaller than 0.85), or the robot base is lower than 0.24, the + minitaur is considered fallen. + + Returns: + Boolean value that indicates whether the minitaur has fallen. + """ + orientation = self.minitaur.GetBaseOrientation() + rot_mat = self._pybullet_client.getMatrixFromQuaternion(orientation) + local_up = rot_mat[6:] + _, _, height = self.minitaur.GetBasePosition() + local_global_up_dot_product = np.dot( + np.asarray([0, 0, 1]), np.asarray(local_up)) + return local_global_up_dot_product < 0.85 or height < 0.15 + + def _reward(self, action, obs): + roll, pitch, _ = self.minitaur.GetBaseRollPitchYaw() + return 1.0 / (0.001 + math.fabs(roll) + math.fabs(pitch)) + + def _get_observation(self): + """Get the true observations of this environment. + + It includes the roll, pitch, roll dot, pitch dot of the base, and the motor + angles. + + Returns: + The observation list. + """ + roll, pitch, _ = self.minitaur.GetBaseRollPitchYaw() + observation = [roll, pitch] + if self._use_angular_velocity_in_observation: + roll_rate, pitch_rate, _ = self.minitaur.GetBaseRollPitchYawRate() + observation.extend([roll_rate, pitch_rate]) + if self._use_motor_angle_in_observation: + observation.extend(self.minitaur.GetMotorAngles().tolist()) + self._observation = np.array(observation) + return self._observation + + def _get_true_observation(self): + """Get the true observations of this environment. + + It includes the roll, pitch, roll dot, pitch dot of the base, and the motor + angles. + + Returns: + The observation list. + """ + roll, pitch, _ = self.minitaur.GetBaseRollPitchYaw() + observation = [roll, pitch] + if self._use_angular_velocity_in_observation: + roll_rate, pitch_rate, _ = self.minitaur.GetBaseRollPitchYawRate() + observation.extend([roll_rate, pitch_rate]) + if self._use_motor_angle_in_observation: + observation.extend(self.minitaur.GetMotorAngles().tolist()) + + self._observation = np.array(observation) + return self._observation + + def _get_observation_upper_bound(self): + """Get the upper bound of the observation. + + Returns: + The upper bound of an observation. See GetObservation() for the details + of each element of an observation. + """ + upper_bound = [2 * math.pi] * 2 # Roll, pitch the base. + if self._use_angular_velocity_in_observation: + upper_bound.extend([2 * math.pi / self._time_step] * 2) + if self._use_motor_angle_in_observation: + upper_bound.extend([2 * math.pi] * 8) + return np.array(upper_bound) + + def _get_observation_lower_bound(self): + lower_bound = -self._get_observation_upper_bound() + return lower_bound + + def set_swing_offset(self, value): + """Set the swing offset of each leg. + + It is to mimic the motor zero-calibration errors. + + Args: + value: A list of four values. + """ + self._swing_offset = value + + def set_extension_offset(self, value): + """Set the extension offset of each leg. + + It is to mimic the motor zero-calibration errors. + + Args: + value: A list of four values. + """ + self._extension_offset = value + + def set_desired_pitch(self, value): + """Set the desired pitch of the base, which is a user input. + + Args: + value: A scalar. + """ + self.desired_pitch = value diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_four_leg_stand_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_four_leg_stand_env_example.py new file mode 100644 index 000000000..4893c30f7 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_four_leg_stand_env_example.py @@ -0,0 +1,69 @@ +"""An example to run the minitaur environment of standing with four legs. + +""" +import numpy as np +import tensorflow as tf +from pybullet_envs.minitaur.envs import minitaur_four_leg_stand_env + + +FLAGS = tf.flags.FLAGS +tf.flags.DEFINE_string("log_path", None, "The directory to write the log file.") +NUM_LEGS = 4 +kroll = 3.0 +kpitch = 3.0 + + +def feed_forward_only_control_example(log_path=None): + """An example of hand-tuned controller for minitaur standing with four legs. + + Args: + log_path: The directory that the log files are written to. If log_path is + None, no logs will be written. + """ + steps = 1000 + episodes = 1 + + environment = minitaur_four_leg_stand_env.MinitaurFourLegStandEnv( + on_rack=False, + log_path=log_path, + urdf_version=minitaur_four_leg_stand_env.RAINBOW_DASH_V0_URDF_VERSION, + remove_default_joint_damping=True, + hard_reset=True, + motor_kp=1.0, + motor_kd=0.015, + control_latency=0.015, + pd_latency=0.003, + control_time_step=0.006, + action_repeat=6, + env_randomizer=None, + render=True) + + np.random.seed(100) + avg_reward = 0 + for i in xrange(episodes): + sum_reward = 0 + observation = environment.reset() + for _ in xrange(steps): + action = [0] * 4 + uroll = kroll * observation[0] + upitch = kpitch * observation[1] + action[0] = upitch - uroll + action[1] = -upitch - uroll + action[2] = upitch + uroll + action[3] = -upitch + uroll + observation, reward, done, _ = environment.step(action) + sum_reward += reward + if done: + break + tf.logging.info("reward {}: {}".format(i, sum_reward)) + avg_reward += sum_reward + tf.logging.info("avg_reward: {}\n\n\n".format(avg_reward / episodes)) + + +def main(unused_argv): + feed_forward_only_control_example(log_path=FLAGS.log_path) + + +if __name__ == "__main__": + tf.logging.set_verbosity(tf.logging.INFO) + tf.app.run() diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py new file mode 100644 index 000000000..189accae2 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py @@ -0,0 +1,601 @@ +"""This file implements the gym environment of minitaur. + +""" +import math +import time + +import gym +from gym import spaces +from gym.utils import seeding +import numpy as np +import pybullet +import bullet_client +import pybullet_data +import minitaur +import minitaur_derpy +import minitaur_logging +import minitaur_logging_pb2 +import minitaur_rainbow_dash +import motor + +NUM_MOTORS = 8 +MOTOR_ANGLE_OBSERVATION_INDEX = 0 +MOTOR_VELOCITY_OBSERVATION_INDEX = MOTOR_ANGLE_OBSERVATION_INDEX + NUM_MOTORS +MOTOR_TORQUE_OBSERVATION_INDEX = MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS +BASE_ORIENTATION_OBSERVATION_INDEX = MOTOR_TORQUE_OBSERVATION_INDEX + NUM_MOTORS +ACTION_EPS = 0.01 +OBSERVATION_EPS = 0.01 +RENDER_HEIGHT = 360 +RENDER_WIDTH = 480 +SENSOR_NOISE_STDDEV = minitaur.SENSOR_NOISE_STDDEV +DEFAULT_URDF_VERSION = "default" +DERPY_V0_URDF_VERSION = "derpy_v0" +RAINBOW_DASH_V0_URDF_VERSION = "rainbow_dash_v0" +NUM_SIMULATION_ITERATION_STEPS = 300 + +MINIATUR_URDF_VERSION_MAP = { + DEFAULT_URDF_VERSION: minitaur.Minitaur, + DERPY_V0_URDF_VERSION: minitaur_derpy.MinitaurDerpy, + RAINBOW_DASH_V0_URDF_VERSION: minitaur_rainbow_dash.MinitaurRainbowDash, +} + + +def convert_to_list(obj): + try: + iter(obj) + return obj + except TypeError: + return [obj] + + +class MinitaurGymEnv(gym.Env): + """The gym environment for the minitaur. + + It simulates the locomotion of a minitaur, a quadruped robot. The state space + include the angles, velocities and torques for all the motors and the action + space is the desired motor angle for each motor. The reward function is based + on how far the minitaur walks in 1000 steps and penalizes the energy + expenditure. + + """ + metadata = { + "render.modes": ["human", "rgb_array"], + "video.frames_per_second": 100 + } + + def __init__(self, + urdf_root=pybullet_data.getDataPath(), + urdf_version=None, + distance_weight=1.0, + energy_weight=0.005, + shake_weight=0.0, + drift_weight=0.0, + distance_limit=float("inf"), + observation_noise_stdev=SENSOR_NOISE_STDDEV, + self_collision_enabled=True, + motor_velocity_limit=np.inf, + pd_control_enabled=False, + leg_model_enabled=True, + accurate_motor_model_enabled=False, + remove_default_joint_damping=False, + motor_kp=1.0, + motor_kd=0.02, + control_latency=0.0, + pd_latency=0.0, + torque_control_enabled=False, + motor_overheat_protection=False, + hard_reset=True, + on_rack=False, + render=False, + num_steps_to_log=1000, + action_repeat=1, + control_time_step=None, + env_randomizer=None, + forward_reward_cap=float("inf"), + log_path=None): + """Initialize the minitaur gym environment. + + Args: + urdf_root: The path to the urdf data folder. + urdf_version: [DEFAULT_URDF_VERSION, DERPY_V0_URDF_VERSION, + RAINBOW_DASH_V0_URDF_VERSION] are allowable + versions. If None, DEFAULT_URDF_VERSION is used. DERPY_V0_URDF_VERSION + is the result of first pass system identification for derpy. + We will have a different URDF and related Minitaur class each time we + perform system identification. While the majority of the code of the + class remains the same, some code changes (e.g. the constraint location + might change). __init__() will choose the right Minitaur class from + different minitaur modules based on + urdf_version. + distance_weight: The weight of the distance term in the reward. + energy_weight: The weight of the energy term in the reward. + shake_weight: The weight of the vertical shakiness term in the reward. + drift_weight: The weight of the sideways drift term in the reward. + distance_limit: The maximum distance to terminate the episode. + observation_noise_stdev: The standard deviation of observation noise. + self_collision_enabled: Whether to enable self collision in the sim. + motor_velocity_limit: The velocity limit of each motor. + pd_control_enabled: Whether to use PD controller for each motor. + leg_model_enabled: Whether to use a leg motor to reparameterize the action + space. + accurate_motor_model_enabled: Whether to use the accurate DC motor model. + remove_default_joint_damping: Whether to remove the default joint damping. + motor_kp: proportional gain for the accurate motor model. + motor_kd: derivative gain for the accurate motor model. + control_latency: It is the delay in the controller between when an + observation is made at some point, and when that reading is reported + back to the Neural Network. + pd_latency: latency of the PD controller loop. PD calculates PWM based on + the motor angle and velocity. The latency measures the time between when + the motor angle and velocity are observed on the microcontroller and + when the true state happens on the motor. It is typically (0.001- + 0.002s). + torque_control_enabled: Whether to use the torque control, if set to + False, pose control will be used. + motor_overheat_protection: Whether to shutdown the motor that has exerted + large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time + (OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in minitaur.py for more + details. + hard_reset: Whether to wipe the simulation and load everything when reset + is called. If set to false, reset just place the minitaur back to start + position and set its pose to initial configuration. + on_rack: Whether to place the minitaur on rack. This is only used to debug + the walking gait. In this mode, the minitaur's base is hanged midair so + that its walking gait is clearer to visualize. + render: Whether to render the simulation. + num_steps_to_log: The max number of control steps in one episode that will + be logged. If the number of steps is more than num_steps_to_log, the + environment will still be running, but only first num_steps_to_log will + be recorded in logging. + action_repeat: The number of simulation steps before actions are applied. + control_time_step: The time step between two successive control signals. + env_randomizer: An instance (or a list) of EnvRandomizer(s). An + EnvRandomizer may randomize the physical property of minitaur, change + the terrrain during reset(), or add perturbation forces during step(). + forward_reward_cap: The maximum value that forward reward is capped at. + Disabled (Inf) by default. + log_path: The path to write out logs. For the details of logging, refer to + minitaur_logging.proto. + Raises: + ValueError: If the urdf_version is not supported. + """ + # Set up logging. + self._log_path = log_path + self.logging = minitaur_logging.MinitaurLogging(log_path) + # PD control needs smaller time step for stability. + if control_time_step is not None: + self.control_time_step = control_time_step + self._action_repeat = action_repeat + self._time_step = control_time_step / action_repeat + else: + # Default values for time step and action repeat + if accurate_motor_model_enabled or pd_control_enabled: + self._time_step = 0.002 + self._action_repeat = 5 + else: + self._time_step = 0.01 + self._action_repeat = 1 + self.control_time_step = self._time_step * self._action_repeat + # TODO(b/73829334): Fix the value of self._num_bullet_solver_iterations. + self._num_bullet_solver_iterations = int( + NUM_SIMULATION_ITERATION_STEPS / self._action_repeat) + self._urdf_root = urdf_root + self._self_collision_enabled = self_collision_enabled + self._motor_velocity_limit = motor_velocity_limit + self._observation = [] + self._true_observation = [] + self._objectives = [] + self._objective_weights = [ + distance_weight, energy_weight, drift_weight, shake_weight + ] + self._env_step_counter = 0 + self._num_steps_to_log = num_steps_to_log + self._is_render = render + self._last_base_position = [0, 0, 0] + self._distance_weight = distance_weight + self._energy_weight = energy_weight + self._drift_weight = drift_weight + self._shake_weight = shake_weight + self._distance_limit = distance_limit + self._observation_noise_stdev = observation_noise_stdev + self._action_bound = 1 + self._pd_control_enabled = pd_control_enabled + self._leg_model_enabled = leg_model_enabled + self._accurate_motor_model_enabled = accurate_motor_model_enabled + self._remove_default_joint_damping = remove_default_joint_damping + self._motor_kp = motor_kp + self._motor_kd = motor_kd + self._torque_control_enabled = torque_control_enabled + self._motor_overheat_protection = motor_overheat_protection + self._on_rack = on_rack + self._cam_dist = 1.0 + self._cam_yaw = 0 + self._cam_pitch = -30 + self._forward_reward_cap = forward_reward_cap + self._hard_reset = True + self._last_frame_time = 0.0 + self._control_latency = control_latency + self._pd_latency = pd_latency + self._urdf_version = urdf_version + self._ground_id = None + self._env_randomizers = convert_to_list( + env_randomizer) if env_randomizer else [] + self._episode_proto = minitaur_logging_pb2.MinitaurEpisode() + if self._is_render: + self._pybullet_client = bullet_client.BulletClient( + connection_mode=pybullet.GUI) + else: + self._pybullet_client = bullet_client.BulletClient() + if self._urdf_version is None: + self._urdf_version = DEFAULT_URDF_VERSION + self._pybullet_client.setPhysicsEngineParameter(enableConeFriction=0) + self._seed() + self.reset() + observation_high = (self._get_observation_upper_bound() + OBSERVATION_EPS) + observation_low = (self._get_observation_lower_bound() - OBSERVATION_EPS) + action_dim = NUM_MOTORS + action_high = np.array([self._action_bound] * action_dim) + self.action_space = spaces.Box(-action_high, action_high) + self.observation_space = spaces.Box(observation_low, observation_high) + self.viewer = None + self._hard_reset = hard_reset # This assignment need to be after reset() + + def _close(self): + self.logging.save_episode(self._episode_proto) + self.minitaur.Terminate() + + def add_env_randomizer(self, env_randomizer): + self._env_randomizers.append(env_randomizer) + + def _reset(self, initial_motor_angles=None, reset_duration=1.0): + self._pybullet_client.configureDebugVisualizer( + self._pybullet_client.COV_ENABLE_RENDERING, 0) + self.logging.save_episode(self._episode_proto) + self._episode_proto = minitaur_logging_pb2.MinitaurEpisode() + minitaur_logging.preallocate_episode_proto(self._episode_proto, + self._num_steps_to_log) + if self._hard_reset: + self._pybullet_client.resetSimulation() + self._pybullet_client.setPhysicsEngineParameter( + numSolverIterations=int(self._num_bullet_solver_iterations)) + self._pybullet_client.setTimeStep(self._time_step) + self._ground_id = self._pybullet_client.loadURDF( + "%s/plane.urdf" % self._urdf_root) + self._pybullet_client.setGravity(0, 0, -10) + acc_motor = self._accurate_motor_model_enabled + motor_protect = self._motor_overheat_protection + if self._urdf_version not in MINIATUR_URDF_VERSION_MAP: + raise ValueError( + "%s is not a supported urdf_version." % self._urdf_version) + else: + self.minitaur = ( + MINIATUR_URDF_VERSION_MAP[self._urdf_version]( + pybullet_client=self._pybullet_client, + action_repeat=self._action_repeat, + urdf_root=self._urdf_root, + time_step=self._time_step, + self_collision_enabled=self._self_collision_enabled, + motor_velocity_limit=self._motor_velocity_limit, + pd_control_enabled=self._pd_control_enabled, + accurate_motor_model_enabled=acc_motor, + remove_default_joint_damping=self._remove_default_joint_damping, + motor_kp=self._motor_kp, + motor_kd=self._motor_kd, + control_latency=self._control_latency, + pd_latency=self._pd_latency, + observation_noise_stdev=self._observation_noise_stdev, + torque_control_enabled=self._torque_control_enabled, + motor_overheat_protection=motor_protect, + on_rack=self._on_rack)) + self.minitaur.Reset( + reload_urdf=False, + default_motor_angles=initial_motor_angles, + reset_time=reset_duration) + + # Loop over all env randomizers. + for env_randomizer in self._env_randomizers: + env_randomizer.randomize_env(self) + + self._pybullet_client.setPhysicsEngineParameter(enableConeFriction=0) + self._env_step_counter = 0 + self._last_base_position = [0, 0, 0] + self._objectives = [] + self._pybullet_client.resetDebugVisualizerCamera( + self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0]) + self._pybullet_client.configureDebugVisualizer( + self._pybullet_client.COV_ENABLE_RENDERING, 1) + return self._get_observation() + + def _seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def _transform_action_to_motor_command(self, action): + if self._leg_model_enabled: + for i, action_component in enumerate(action): + if not (-self._action_bound - ACTION_EPS <= action_component <= + self._action_bound + ACTION_EPS): + raise ValueError("{}th action {} out of bounds.".format( + i, action_component)) + action = self.minitaur.ConvertFromLegModel(action) + return action + + def _step(self, action): + """Step forward the simulation, given the action. + + Args: + action: A list of desired motor angles for eight motors. + + Returns: + observations: The angles, velocities and torques of all motors. + reward: The reward for the current state-action pair. + done: Whether the episode has ended. + info: A dictionary that stores diagnostic information. + + Raises: + ValueError: The action dimension is not the same as the number of motors. + ValueError: The magnitude of actions is out of bounds. + """ + self._last_base_position = self.minitaur.GetBasePosition() + + if self._is_render: + # Sleep, otherwise the computation takes less time than real time, + # which will make the visualization like a fast-forward video. + time_spent = time.time() - self._last_frame_time + self._last_frame_time = time.time() + time_to_sleep = self.control_time_step - time_spent + if time_to_sleep > 0: + time.sleep(time_to_sleep) + base_pos = self.minitaur.GetBasePosition() + # Keep the previous orientation of the camera set by the user. + [yaw, pitch, + dist] = self._pybullet_client.getDebugVisualizerCamera()[8:11] + self._pybullet_client.resetDebugVisualizerCamera(dist, yaw, pitch, + base_pos) + + for env_randomizer in self._env_randomizers: + env_randomizer.randomize_step(self) + + action = self._transform_action_to_motor_command(action) + self.minitaur.Step(action) + reward = self._reward() + done = self._termination() + if self._log_path is not None: + minitaur_logging.update_episode_proto(self._episode_proto, self.minitaur, + action, self._env_step_counter) + self._env_step_counter += 1 + if done: + self.minitaur.Terminate() + return np.array(self._get_observation()), reward, done, {} + + def _render(self, mode="rgb_array", close=False): + if mode != "rgb_array": + return np.array([]) + base_pos = self.minitaur.GetBasePosition() + view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll( + cameraTargetPosition=base_pos, + distance=self._cam_dist, + yaw=self._cam_yaw, + pitch=self._cam_pitch, + roll=0, + upAxisIndex=2) + proj_matrix = self._pybullet_client.computeProjectionMatrixFOV( + fov=60, + aspect=float(RENDER_WIDTH) / RENDER_HEIGHT, + nearVal=0.1, + farVal=100.0) + (_, _, px, _, _) = self._pybullet_client.getCameraImage( + width=RENDER_WIDTH, + height=RENDER_HEIGHT, + renderer=self._pybullet_client.ER_BULLET_HARDWARE_OPENGL, + viewMatrix=view_matrix, + projectionMatrix=proj_matrix) + rgb_array = np.array(px) + rgb_array = rgb_array[:, :, :3] + return rgb_array + + def get_minitaur_motor_angles(self): + """Get the minitaur's motor angles. + + Returns: + A numpy array of motor angles. + """ + return np.array( + self._observation[MOTOR_ANGLE_OBSERVATION_INDEX: + MOTOR_ANGLE_OBSERVATION_INDEX + NUM_MOTORS]) + + def get_minitaur_motor_velocities(self): + """Get the minitaur's motor velocities. + + Returns: + A numpy array of motor velocities. + """ + return np.array( + self._observation[MOTOR_VELOCITY_OBSERVATION_INDEX: + MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS]) + + def get_minitaur_motor_torques(self): + """Get the minitaur's motor torques. + + Returns: + A numpy array of motor torques. + """ + return np.array( + self._observation[MOTOR_TORQUE_OBSERVATION_INDEX: + MOTOR_TORQUE_OBSERVATION_INDEX + NUM_MOTORS]) + + def get_minitaur_base_orientation(self): + """Get the minitaur's base orientation, represented by a quaternion. + + Returns: + A numpy array of minitaur's orientation. + """ + return np.array(self._observation[BASE_ORIENTATION_OBSERVATION_INDEX:]) + + def is_fallen(self): + """Decide whether the minitaur has fallen. + + If the up directions between the base and the world is larger (the dot + product is smaller than 0.85) or the base is very low on the ground + (the height is smaller than 0.13 meter), the minitaur is considered fallen. + + Returns: + Boolean value that indicates whether the minitaur has fallen. + """ + orientation = self.minitaur.GetBaseOrientation() + rot_mat = self._pybullet_client.getMatrixFromQuaternion(orientation) + local_up = rot_mat[6:] + pos = self.minitaur.GetBasePosition() + return (np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.85 or + pos[2] < 0.13) + + def _termination(self): + position = self.minitaur.GetBasePosition() + distance = math.sqrt(position[0]**2 + position[1]**2) + return self.is_fallen() or distance > self._distance_limit + + def _reward(self): + current_base_position = self.minitaur.GetBasePosition() + forward_reward = current_base_position[0] - self._last_base_position[0] + # Cap the forward reward if a cap is set. + forward_reward = min(forward_reward, self._forward_reward_cap) + # Penalty for sideways translation. + drift_reward = -abs(current_base_position[1] - self._last_base_position[1]) + # Penalty for sideways rotation of the body. + orientation = self.minitaur.GetBaseOrientation() + rot_matrix = pybullet.getMatrixFromQuaternion(orientation) + local_up_vec = rot_matrix[6:] + shake_reward = -abs(np.dot(np.asarray([1, 1, 0]), np.asarray(local_up_vec))) + energy_reward = -np.abs( + np.dot(self.minitaur.GetMotorTorques(), + self.minitaur.GetMotorVelocities())) * self._time_step + objectives = [forward_reward, energy_reward, drift_reward, shake_reward] + weighted_objectives = [ + o * w for o, w in zip(objectives, self._objective_weights) + ] + reward = sum(weighted_objectives) + self._objectives.append(objectives) + return reward + + def get_objectives(self): + return self._objectives + + @property + def objective_weights(self): + """Accessor for the weights for all the objectives. + + Returns: + List of floating points that corresponds to weights for the objectives in + the order that objectives are stored. + """ + return self._objective_weights + + def _get_observation(self): + """Get observation of this environment, including noise and latency. + + The minitaur class maintains a history of true observations. Based on the + latency, this function will find the observation at the right time, + interpolate if necessary. Then Gaussian noise is added to this observation + based on self.observation_noise_stdev. + + Returns: + The noisy observation with latency. + """ + + observation = [] + observation.extend(self.minitaur.GetMotorAngles().tolist()) + observation.extend(self.minitaur.GetMotorVelocities().tolist()) + observation.extend(self.minitaur.GetMotorTorques().tolist()) + observation.extend(list(self.minitaur.GetBaseOrientation())) + self._observation = observation + return self._observation + + def _get_true_observation(self): + """Get the observations of this environment. + + It includes the angles, velocities, torques and the orientation of the base. + + Returns: + The observation list. observation[0:8] are motor angles. observation[8:16] + are motor velocities, observation[16:24] are motor torques. + observation[24:28] is the orientation of the base, in quaternion form. + """ + observation = [] + observation.extend(self.minitaur.GetTrueMotorAngles().tolist()) + observation.extend(self.minitaur.GetTrueMotorVelocities().tolist()) + observation.extend(self.minitaur.GetTrueMotorTorques().tolist()) + observation.extend(list(self.minitaur.GetTrueBaseOrientation())) + + self._true_observation = observation + return self._true_observation + + def _get_observation_upper_bound(self): + """Get the upper bound of the observation. + + Returns: + The upper bound of an observation. See GetObservation() for the details + of each element of an observation. + """ + upper_bound = np.zeros(self._get_observation_dimension()) + num_motors = self.minitaur.num_motors + upper_bound[0:num_motors] = math.pi # Joint angle. + upper_bound[num_motors:2 * num_motors] = ( + motor.MOTOR_SPEED_LIMIT) # Joint velocity. + upper_bound[2 * num_motors:3 * num_motors] = ( + motor.OBSERVED_TORQUE_LIMIT) # Joint torque. + upper_bound[3 * num_motors:] = 1.0 # Quaternion of base orientation. + return upper_bound + + def _get_observation_lower_bound(self): + """Get the lower bound of the observation.""" + return -self._get_observation_upper_bound() + + def _get_observation_dimension(self): + """Get the length of the observation list. + + Returns: + The length of the observation list. + """ + return len(self._get_observation()) + + def set_time_step(self, control_step, simulation_step=0.001): + """Sets the time step of the environment. + + Args: + control_step: The time period (in seconds) between two adjacent control + actions are applied. + simulation_step: The simulation time step in PyBullet. By default, the + simulation step is 0.001s, which is a good trade-off between simulation + speed and accuracy. + Raises: + ValueError: If the control step is smaller than the simulation step. + """ + if control_step < simulation_step: + raise ValueError( + "Control step should be larger than or equal to simulation step.") + self.control_time_step = control_step + self._time_step = simulation_step + self._action_repeat = int(round(control_step / simulation_step)) + self._num_bullet_solver_iterations = ( + NUM_SIMULATION_ITERATION_STEPS / self._action_repeat) + self._pybullet_client.setPhysicsEngineParameter( + numSolverIterations=self._num_bullet_solver_iterations) + self._pybullet_client.setTimeStep(self._time_step) + self.minitaur.SetTimeSteps( + action_repeat=self._action_repeat, simulation_step=self._time_step) + + @property + def pybullet_client(self): + return self._pybullet_client + + @property + def ground_id(self): + return self._ground_id + + @ground_id.setter + def ground_id(self, new_ground_id): + self._ground_id = new_ground_id + + @property + def env_step_counter(self): + return self._env_step_counter diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env_example.py new file mode 100644 index 000000000..bfd116114 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env_example.py @@ -0,0 +1,217 @@ +"""An example to run of the minitaur gym environment with sine gaits. + +""" + +import csv +import math + +import argparse +import numpy as np +import tensorflow as tf +import minitaur_gym_env + + +#FLAGS = flags.FLAGS +#flags.DEFINE_enum( +# "example_name", "sine", ["sine", "reset", "stand", "overheat"], +# "The name of the example: sine, reset, stand, or overheat.") +#flags.DEFINE_string("output_filename", None, "The name of the output CSV file." +# "Each line in the CSV file will contain the action, the " +# "motor position, speed and torques.") +#flags.DEFINE_string("log_path", None, "The directory to write the log file.") + + +def WriteToCSV(filename, actions_and_observations): + """Write simulation data to file. + + Save actions and observed angles, angular velocities and torques for data + analysis. + + Args: + filename: The file to write. Can be locally or on CNS. + actions_and_observations: the interested simulation quantities to save. + """ + with tf.gfile.Open(filename, "wb") as csvfile: + csv_writer = csv.writer(csvfile, delimiter=",") + for row in actions_and_observations: + csv_writer.writerow(row) + + +def ResetPoseExample(log_path=None): + """An example that the minitaur stands still using the reset pose.""" + steps = 10000 + environment = minitaur_gym_env.MinitaurGymEnv( + urdf_version=minitaur_gym_env.DERPY_V0_URDF_VERSION, + render=True, + leg_model_enabled=False, + motor_velocity_limit=np.inf, + pd_control_enabled=True, + accurate_motor_model_enabled=True, + motor_overheat_protection=True, + hard_reset=False, + log_path=log_path) + action = [math.pi / 2] * 8 + for _ in range(steps): + _, _, done, _ = environment.step(action) + if done: + break + + +def MotorOverheatExample(log_path=None): + """An example of minitaur motor overheat protection is triggered. + + The minitaur is leaning forward and the motors are getting obove threshold + torques. The overheat protection will be triggered in ~1 sec. + + Args: + log_path: The directory that the log files are written to. If log_path is + None, no logs will be written. + """ + + environment = minitaur_gym_env.MinitaurGymEnv( + urdf_version=minitaur_gym_env.DERPY_V0_URDF_VERSION, + render=True, + leg_model_enabled=False, + motor_velocity_limit=np.inf, + motor_overheat_protection=True, + accurate_motor_model_enabled=True, + motor_kp=1.20, + motor_kd=0.00, + on_rack=False, + log_path=log_path) + + action = [2.0] * 8 + for i in range(8): + action[i] = 2.0 - 0.5 * (-1 if i % 2 == 0 else 1) * (-1 if i < 4 else 1) + + steps = 500 + actions_and_observations = [] + for step_counter in range(steps): + # Matches the internal timestep. + time_step = 0.01 + t = step_counter * time_step + current_row = [t] + current_row.extend(action) + + observation, _, _, _ = environment.step(action) + current_row.extend(observation.tolist()) + actions_and_observations.append(current_row) + + if FLAGS.output_filename is not None: + WriteToCSV(FLAGS.output_filename, actions_and_observations) + + +def SineStandExample(log_path=None): + """An example of minitaur standing and squatting on the floor. + + To validate the accurate motor model we command the robot and sit and stand up + periodically in both simulation and experiment. We compare the measured motor + trajectories, torques and gains. The results are at: + https://colab.corp.google.com/v2/notebook#fileId=0BxTIAnWh1hb_ZnkyYWtNQ1RYdkU&scrollTo=ZGFMl84kKqRx + + Args: + log_path: The directory that the log files are written to. If log_path is + None, no logs will be written. + """ + environment = minitaur_gym_env.MinitaurGymEnv( + urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION, + render=True, + leg_model_enabled=False, + motor_velocity_limit=np.inf, + motor_overheat_protection=True, + accurate_motor_model_enabled=True, + motor_kp=1.20, + motor_kd=0.02, + on_rack=False, + log_path=log_path) + steps = 1000 + amplitude = 0.5 + speed = 3 + + actions_and_observations = [] + + for step_counter in range(steps): + # Matches the internal timestep. + time_step = 0.01 + t = step_counter * time_step + current_row = [t] + + action = [math.sin(speed * t) * amplitude + math.pi / 2] * 8 + current_row.extend(action) + + observation, _, _, _ = environment.step(action) + current_row.extend(observation.tolist()) + actions_and_observations.append(current_row) + + if FLAGS.output_filename is not None: + WriteToCSV(FLAGS.output_filename, actions_and_observations) + + +def SinePolicyExample(log_path=None): + """An example of minitaur walking with a sine gait. + + Args: + log_path: The directory that the log files are written to. If log_path is + None, no logs will be written. + """ + environment = minitaur_gym_env.MinitaurGymEnv( + urdf_version=minitaur_gym_env.DERPY_V0_URDF_VERSION, + render=True, + motor_velocity_limit=np.inf, + pd_control_enabled=True, + hard_reset=False, + on_rack=False, + log_path=log_path) + sum_reward = 0 + steps = 20000 + amplitude_1_bound = 0.5 + amplitude_2_bound = 0.5 + speed = 40 + + for step_counter in range(steps): + time_step = 0.01 + t = step_counter * time_step + + amplitude1 = amplitude_1_bound + amplitude2 = amplitude_2_bound + steering_amplitude = 0 + if t < 10: + steering_amplitude = 0.5 + elif t < 20: + steering_amplitude = -0.5 + else: + steering_amplitude = 0 + + # Applying asymmetrical sine gaits to different legs can steer the minitaur. + a1 = math.sin(t * speed) * (amplitude1 + steering_amplitude) + a2 = math.sin(t * speed + math.pi) * (amplitude1 - steering_amplitude) + a3 = math.sin(t * speed) * amplitude2 + a4 = math.sin(t * speed + math.pi) * amplitude2 + action = [a1, a2, a2, a1, a3, a4, a4, a3] + _, reward, done, _ = environment.step(action) + sum_reward += reward + if done: + tf.logging.info("Return is {}".format(sum_reward)) + environment.reset() + + + + +def main(): + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument('--env', help='environment ID (0==sine, 1==stand, 2=reset, 3=overheat)',type=int, default=0) + args = parser.parse_args() + print("--env=" + str(args.env)) + + if (args.env == 0): + SinePolicyExample() + if (args.env == 1): + SineStandExample() + if (args.env == 2): + ResetPoseExample() + if (args.env == 3): + MotorOverheatExample() + +if __name__ == '__main__': + main() + diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.proto b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.proto new file mode 100644 index 000000000..79258352a --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.proto @@ -0,0 +1,41 @@ +syntax = "proto3"; +package robotics.reinforcement_learning.minitaur.envs; + +import "timestamp.proto"; +import "vector.proto"; + +message MinitaurEpisode { + // The state-action pair at each step of the log. + repeated MinitaurStateAction state_action = 1; +} + +message MinitaurMotorState { + // The current angle of the motor. + double angle = 1; + // The current velocity of the motor. + double velocity = 2; + // The current torque exerted at this motor. + double torque = 3; + // The action directed to this motor. The action is the desired motor angle. + double action = 4; +} + +message MinitaurStateAction { + // Whether the state/action information is valid. It is always true if the + // proto is from simulation. It might be false when communication error + // happens on minitaur hardware. + bool info_valid = 6; + // The time stamp of this step. It is computed since the reset of the + // environment. + google.protobuf.Timestamp time = 1; + // The position of the base of the minitaur. + robotics.messages.Vector3d base_position = 2; + // The orientation of the base of the minitaur. It is represented as (roll, + // pitch, yaw). + robotics.messages.Vector3d base_orientation = 3; + // The angular velocity of the base of the minitaur. It is the time derivative + // of (roll, pitch, yaw). + robotics.messages.Vector3d base_angular_vel = 4; + // The motor states (angle, velocity, torque, action) of eight motors. + repeated MinitaurMotorState motor_states = 5; +} diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py new file mode 100644 index 000000000..f7022b728 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py @@ -0,0 +1,142 @@ +"""A proto buffer based logging system for minitaur experiments. + +The logging system records the time since reset, base position, orientation, +angular velocity and motor information (joint angle, speed, and torque) into a +proto buffer. See minitaur_logging.proto for more details. The episode_proto is +updated per time step by the environment and saved onto disk for each episode. +""" + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +import datetime +import os +import time + +import tensorflow as tf +import minitaur_logging_pb2 + +NUM_MOTORS = 8 + + +def _update_base_state(base_state, values): + base_state.x = values[0] + base_state.y = values[1] + base_state.z = values[2] + + +def preallocate_episode_proto(episode_proto, max_num_steps): + """Preallocate the memory for proto buffer. + + Dynamically allocating memory as the protobuf expands causes unexpected delay + that is not tolerable with locomotion control. + + Args: + episode_proto: The proto that holds the state/action data for the current + episode. + max_num_steps: The max number of steps that will be recorded in the proto. + The state/data over max_num_steps will not be stored in the proto. + """ + for _ in range(max_num_steps): + step_log = episode_proto.state_action.add() + step_log.info_valid = False + step_log.time.seconds = 0 + step_log.time.nanos = 0 + for _ in range(NUM_MOTORS): + motor_state = step_log.motor_states.add() + motor_state.angle = 0 + motor_state.velocity = 0 + motor_state.torque = 0 + motor_state.action = 0 + _update_base_state(step_log.base_position, [0, 0, 0]) + _update_base_state(step_log.base_orientation, [0, 0, 0]) + _update_base_state(step_log.base_angular_vel, [0, 0, 0]) + + +def update_episode_proto(episode_proto, minitaur, action, step): + """Update the episode proto by appending the states/action of the minitaur. + + Note that the state/data over max_num_steps preallocated + (len(episode_proto.state_action)) will not be stored in the proto. + Args: + episode_proto: The proto that holds the state/action data for the current + episode. + minitaur: The minitaur instance. See envs.minitaur for details. + action: The action applied at this time step. The action is an 8-element + numpy floating-point array. + step: The current step index. + """ + max_num_steps = len(episode_proto.state_action) + if step >= max_num_steps: + tf.logging.warning( + "{}th step is not recorded in the logging since only {} steps were " + "pre-allocated.".format(step, max_num_steps)) + return + step_log = episode_proto.state_action[step] + step_log.info_valid = minitaur.IsObservationValid() + time_in_seconds = minitaur.GetTimeSinceReset() + step_log.time.seconds = int(time_in_seconds) + step_log.time.nanos = int((time_in_seconds - int(time_in_seconds)) * 1e9) + + motor_angles = minitaur.GetMotorAngles() + motor_velocities = minitaur.GetMotorVelocities() + motor_torques = minitaur.GetMotorTorques() + for i in range(minitaur.num_motors): + step_log.motor_states[i].angle = motor_angles[i] + step_log.motor_states[i].velocity = motor_velocities[i] + step_log.motor_states[i].torque = motor_torques[i] + step_log.motor_states[i].action = action[i] + + _update_base_state(step_log.base_position, minitaur.GetBasePosition()) + _update_base_state(step_log.base_orientation, minitaur.GetBaseRollPitchYaw()) + _update_base_state(step_log.base_angular_vel, + minitaur.GetBaseRollPitchYawRate()) + + +class MinitaurLogging(object): + """A logging system that records the states/action of the minitaur.""" + + def __init__(self, log_path=None): + self._log_path = log_path + + # TODO(jietan): Consider using recordio to write the logs. + def save_episode(self, episode_proto): + """Save episode_proto to self._log_path. + + self._log_path is the directory name. A time stamp is the file name of the + log file. For example, when self._log_path is "/tmp/logs/", the actual + log file would be "/tmp/logs/yyyy-mm-dd-hh:mm:ss". + + Args: + episode_proto: The proto that holds the states/action for the current + episode that needs to be save to disk. + Returns: + The full log path, including the directory name and the file name. + """ + if not self._log_path or not episode_proto.state_action: + return self._log_path + if not tf.gfile.Exists(self._log_path): + tf.gfile.MakeDirs(self._log_path) + ts = time.time() + time_stamp = datetime.datetime.fromtimestamp(ts).strftime( + "%Y-%m-%d-%H:%M:%S") + log_path = os.path.join(self._log_path, + "minitaur_log_{}".format(time_stamp)) + with tf.gfile.Open(log_path, "w") as f: + f.write(episode_proto.SerializeToString()) + return log_path + + def restore_episode(self, log_path): + """Restore the episodic proto from the log path. + + Args: + log_path: The full path of the log file. + Returns: + The minitaur episode proto. + """ + with tf.gfile.Open(log_path) as f: + content = f.read() + episode_proto = minitaur_logging_pb2.MinitaurEpisode() + episode_proto.ParseFromString(content) + return episode_proto diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging_pb2.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging_pb2.py new file mode 100644 index 000000000..7ea35b0f6 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging_pb2.py @@ -0,0 +1,212 @@ +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: minitaur_logging.proto + +import sys +_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) +from google.protobuf import descriptor as _descriptor +from google.protobuf import message as _message +from google.protobuf import reflection as _reflection +from google.protobuf import symbol_database as _symbol_database +from google.protobuf import descriptor_pb2 +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + +import timestamp_pb2 as timestamp__pb2 +import vector_pb2 as vector__pb2 + + +DESCRIPTOR = _descriptor.FileDescriptor( + name='minitaur_logging.proto', + package='robotics.reinforcement_learning.minitaur.envs', + syntax='proto3', + serialized_pb=_b('\n\x16minitaur_logging.proto\x12-robotics.reinforcement_learning.minitaur.envs\x1a\x0ftimestamp.proto\x1a\x0cvector.proto\"k\n\x0fMinitaurEpisode\x12X\n\x0cstate_action\x18\x01 \x03(\x0b\x32\x42.robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction\"U\n\x12MinitaurMotorState\x12\r\n\x05\x61ngle\x18\x01 \x01(\x01\x12\x10\n\x08velocity\x18\x02 \x01(\x01\x12\x0e\n\x06torque\x18\x03 \x01(\x01\x12\x0e\n\x06\x61\x63tion\x18\x04 \x01(\x01\"\xce\x02\n\x13MinitaurStateAction\x12\x12\n\ninfo_valid\x18\x06 \x01(\x08\x12(\n\x04time\x18\x01 \x01(\x0b\x32\x1a.google.protobuf.Timestamp\x12\x32\n\rbase_position\x18\x02 \x01(\x0b\x32\x1b.robotics.messages.Vector3d\x12\x35\n\x10\x62\x61se_orientation\x18\x03 \x01(\x0b\x32\x1b.robotics.messages.Vector3d\x12\x35\n\x10\x62\x61se_angular_vel\x18\x04 \x01(\x0b\x32\x1b.robotics.messages.Vector3d\x12W\n\x0cmotor_states\x18\x05 \x03(\x0b\x32\x41.robotics.reinforcement_learning.minitaur.envs.MinitaurMotorStateb\x06proto3') + , + dependencies=[timestamp__pb2.DESCRIPTOR,vector__pb2.DESCRIPTOR,]) + + + + +_MINITAUREPISODE = _descriptor.Descriptor( + name='MinitaurEpisode', + full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurEpisode', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='state_action', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurEpisode.state_action', index=0, + number=1, type=11, cpp_type=10, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=104, + serialized_end=211, +) + + +_MINITAURMOTORSTATE = _descriptor.Descriptor( + name='MinitaurMotorState', + full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='angle', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState.angle', index=0, + number=1, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='velocity', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState.velocity', index=1, + number=2, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='torque', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState.torque', index=2, + number=3, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='action', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState.action', index=3, + number=4, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=213, + serialized_end=298, +) + + +_MINITAURSTATEACTION = _descriptor.Descriptor( + name='MinitaurStateAction', + full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='info_valid', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.info_valid', index=0, + number=6, type=8, cpp_type=7, label=1, + has_default_value=False, default_value=False, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='time', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.time', index=1, + number=1, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='base_position', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.base_position', index=2, + number=2, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='base_orientation', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.base_orientation', index=3, + number=3, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='base_angular_vel', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.base_angular_vel', index=4, + number=4, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='motor_states', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.motor_states', index=5, + number=5, type=11, cpp_type=10, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=301, + serialized_end=635, +) + +_MINITAUREPISODE.fields_by_name['state_action'].message_type = _MINITAURSTATEACTION +_MINITAURSTATEACTION.fields_by_name['time'].message_type = timestamp__pb2._TIMESTAMP +_MINITAURSTATEACTION.fields_by_name['base_position'].message_type = vector__pb2._VECTOR3D +_MINITAURSTATEACTION.fields_by_name['base_orientation'].message_type = vector__pb2._VECTOR3D +_MINITAURSTATEACTION.fields_by_name['base_angular_vel'].message_type = vector__pb2._VECTOR3D +_MINITAURSTATEACTION.fields_by_name['motor_states'].message_type = _MINITAURMOTORSTATE +DESCRIPTOR.message_types_by_name['MinitaurEpisode'] = _MINITAUREPISODE +DESCRIPTOR.message_types_by_name['MinitaurMotorState'] = _MINITAURMOTORSTATE +DESCRIPTOR.message_types_by_name['MinitaurStateAction'] = _MINITAURSTATEACTION +_sym_db.RegisterFileDescriptor(DESCRIPTOR) + +MinitaurEpisode = _reflection.GeneratedProtocolMessageType('MinitaurEpisode', (_message.Message,), dict( + DESCRIPTOR = _MINITAUREPISODE, + __module__ = 'minitaur_logging_pb2' + # @@protoc_insertion_point(class_scope:robotics.reinforcement_learning.minitaur.envs.MinitaurEpisode) + )) +_sym_db.RegisterMessage(MinitaurEpisode) + +MinitaurMotorState = _reflection.GeneratedProtocolMessageType('MinitaurMotorState', (_message.Message,), dict( + DESCRIPTOR = _MINITAURMOTORSTATE, + __module__ = 'minitaur_logging_pb2' + # @@protoc_insertion_point(class_scope:robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState) + )) +_sym_db.RegisterMessage(MinitaurMotorState) + +MinitaurStateAction = _reflection.GeneratedProtocolMessageType('MinitaurStateAction', (_message.Message,), dict( + DESCRIPTOR = _MINITAURSTATEACTION, + __module__ = 'minitaur_logging_pb2' + # @@protoc_insertion_point(class_scope:robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction) + )) +_sym_db.RegisterMessage(MinitaurStateAction) + + +# @@protoc_insertion_point(module_scope) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_rainbow_dash.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_rainbow_dash.py new file mode 100644 index 000000000..4eef2b663 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_rainbow_dash.py @@ -0,0 +1,174 @@ +"""Implements the functionalities of a minitaur rainbow dash using pybullet. + +It is the result of first pass system identification for the rainbow dash robot. + +""" +import math + +import numpy as np +import minitaur + +KNEE_CONSTRAINT_POINT_LONG = [0, 0.0045, 0.088] +KNEE_CONSTRAINT_POINT_SHORT = [0, 0.0045, 0.100] + + +class MinitaurRainbowDash(minitaur.Minitaur): + """The minitaur class that simulates a quadruped robot from Ghost Robotics. + + """ + + def Reset(self, reload_urdf=True, default_motor_angles=None, reset_time=3.0): + """Reset the minitaur to its initial states. + + Args: + reload_urdf: Whether to reload the urdf file. If not, Reset() just place + the minitaur back to its starting position. + default_motor_angles: The default motor angles. If it is None, minitaur + will hold a default pose (motor angle math.pi / 2) for 100 steps. In + torque control mode, the phase of holding the default pose is skipped. + reset_time: The duration (in seconds) to hold the default motor angles. If + reset_time <= 0 or in torque control mode, the phase of holding the + default pose is skipped. + """ + if self._on_rack: + init_position = minitaur.INIT_RACK_POSITION + else: + init_position = minitaur.INIT_POSITION + if reload_urdf: + if self._self_collision_enabled: + self.quadruped = self._pybullet_client.loadURDF( + "%s/quadruped/minitaur_rainbow_dash.urdf" % self._urdf_root, + init_position, + useFixedBase=self._on_rack, + flags=( + self._pybullet_client.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)) + else: + self.quadruped = self._pybullet_client.loadURDF( + "%s/quadruped/minitaur_rainbow_dash.urdf" % self._urdf_root, + init_position, + useFixedBase=self._on_rack) + self._BuildJointNameToIdDict() + self._BuildUrdfIds() + if self._remove_default_joint_damping: + self._RemoveDefaultJointDamping() + self._BuildMotorIdList() + self._RecordMassInfoFromURDF() + self._RecordInertiaInfoFromURDF() + self.ResetPose(add_constraint=True) + else: + self._pybullet_client.resetBasePositionAndOrientation( + self.quadruped, init_position, minitaur.INIT_ORIENTATION) + self._pybullet_client.resetBaseVelocity(self.quadruped, [0, 0, 0], + [0, 0, 0]) + self.ResetPose(add_constraint=False) + + self._overheat_counter = np.zeros(self.num_motors) + self._motor_enabled_list = [True] * self.num_motors + self._step_counter = 0 + + # Perform reset motion within reset_duration if in position control mode. + # Nothing is performed if in torque control mode for now. + # TODO(jietan): Add reset motion when the torque control is fully supported. + self._observation_history.clear() + if not self._torque_control_enabled and reset_time > 0.0: + self.ReceiveObservation() + for _ in range(100): + self.ApplyAction([math.pi / 2] * self.num_motors) + self._pybullet_client.stepSimulation() + self.ReceiveObservation() + if default_motor_angles is not None: + num_steps_to_reset = int(reset_time / self.time_step) + for _ in range(num_steps_to_reset): + self.ApplyAction(default_motor_angles) + self._pybullet_client.stepSimulation() + self.ReceiveObservation() + self.ReceiveObservation() + + def _ResetPoseForLeg(self, leg_id, add_constraint): + """Reset the initial pose for the leg. + + Args: + leg_id: It should be 0, 1, 2, or 3, which represents the leg at + front_left, back_left, front_right and back_right. + add_constraint: Whether to add a constraint at the joints of two feet. + """ + knee_friction_force = 0 + half_pi = math.pi / 2.0 + knee_angle = -2.1834 + + leg_position = minitaur.LEG_POSITION[leg_id] + self._pybullet_client.resetJointState( + self.quadruped, + self._joint_name_to_id["motor_" + leg_position + "L_joint"], + self._motor_direction[2 * leg_id] * half_pi, + targetVelocity=0) + self._pybullet_client.resetJointState( + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "L_joint"], + self._motor_direction[2 * leg_id] * knee_angle, + targetVelocity=0) + self._pybullet_client.resetJointState( + self.quadruped, + self._joint_name_to_id["motor_" + leg_position + "R_joint"], + self._motor_direction[2 * leg_id + 1] * half_pi, + targetVelocity=0) + self._pybullet_client.resetJointState( + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "R_joint"], + self._motor_direction[2 * leg_id + 1] * knee_angle, + targetVelocity=0) + if add_constraint: + if leg_id < 2: + self._pybullet_client.createConstraint( + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "R_joint"], + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "L_joint"], + self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0], + KNEE_CONSTRAINT_POINT_SHORT, KNEE_CONSTRAINT_POINT_LONG) + else: + self._pybullet_client.createConstraint( + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "R_joint"], + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "L_joint"], + self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0], + KNEE_CONSTRAINT_POINT_LONG, KNEE_CONSTRAINT_POINT_SHORT) + + if self._accurate_motor_model_enabled or self._pd_control_enabled: + # Disable the default motor in pybullet. + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=( + self._joint_name_to_id["motor_" + leg_position + "L_joint"]), + controlMode=self._pybullet_client.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=( + self._joint_name_to_id["motor_" + leg_position + "R_joint"]), + controlMode=self._pybullet_client.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) + + else: + self._SetDesiredMotorAngleByName( + "motor_" + leg_position + "L_joint", + self._motor_direction[2 * leg_id] * half_pi) + self._SetDesiredMotorAngleByName( + "motor_" + leg_position + "R_joint", + self._motor_direction[2 * leg_id + 1] * half_pi) + + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=(self._joint_name_to_id["knee_" + leg_position + "L_joint"]), + controlMode=self._pybullet_client.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) + self._pybullet_client.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=(self._joint_name_to_id["knee_" + leg_position + "R_joint"]), + controlMode=self._pybullet_client.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_randomize_terrain_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_randomize_terrain_gym_env.py new file mode 100644 index 000000000..4b8056175 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_randomize_terrain_gym_env.py @@ -0,0 +1,84 @@ +"""Gym environment of minitaur which randomize the terrain at each reset.""" +import math +import os + +import numpy as np +#from google3.pyglib import flags +#from google3.pyglib import gfile +from pybullet_envs.minitaur.envs import minitaur +from pybullet_envs.minitaur.envs import minitaur_gym_env + +flags.DEFINE_string( + 'terrain_dir', '/cns/od-d/home/brain-minitaur/terrain_obj', + 'The directory which contains terrain obj files to be used.') +flags.DEFINE_string('storage_dir', '/tmp', + 'The full path to the temporary directory to be used.') + +FLAGS = flags.FLAGS + + +class MinitaurRandomizeTerrainGymEnv(minitaur_gym_env.MinitaurGymEnv): + """The gym environment for the minitaur with randomized terrain. + + It simulates a minitaur (a quadruped robot) on a randomized terrain. The state + space include the angles, velocities and torques for all the motors and the + action space is the desired motor angle for each motor. The reward function is + based on how far the minitaur walks in 1000 steps and penalizes the energy + expenditure. + + """ + + def _reset(self): + self._pybullet_client.resetSimulation() + self._pybullet_client.setPhysicsEngineParameter( + numSolverIterations=self._num_bullet_solver_iterations) + self._pybullet_client.setTimeStep(self._time_step) + terrain_visual_shape_id = -1 + terrain_mass = 0 + terrain_position = [0, 0, 0] + terrain_orientation = [0, 0, 0, 1] + terrain_file_name = self.load_random_terrain(FLAGS.terrain_dir) + terrain_collision_shape_id = self._pybullet_client.createCollisionShape( + shapeType=self._pybullet_client.GEOM_MESH, + fileName=terrain_file_name, + flags=1, + meshScale=[0.5, 0.5, 0.5]) + self._pybullet_client.createMultiBody(terrain_mass, + terrain_collision_shape_id, + terrain_visual_shape_id, + terrain_position, + terrain_orientation) + self._pybullet_client.setGravity(0, 0, -10) + self.minitaur = (minitaur.Minitaur( + pybullet_client=self._pybullet_client, + urdf_root=self._urdf_root, + time_step=self._time_step, + self_collision_enabled=self._self_collision_enabled, + motor_velocity_limit=self._motor_velocity_limit, + pd_control_enabled=self._pd_control_enabled, + on_rack=self._on_rack)) + self._last_base_position = [0, 0, 0] + for _ in xrange(100): + if self._pd_control_enabled: + self.minitaur.ApplyAction([math.pi / 2] * 8) + self._pybullet_client.stepSimulation() + return self._get_observation() + + def load_random_terrain(self, terrain_dir): + """Load a random terrain obj file. + + Args: + terrain_dir: The directory which contains terrain obj files to be used. + + Returns: + terrain_file_name_complete: The complete terrain obj file name in the + local directory. + """ + terrain_file_names_all = gfile.ListDir(terrain_dir) + terrain_file_name = np.random.choice(terrain_file_names_all) + asset_source = os.path.join(terrain_dir, terrain_file_name) + asset_destination = os.path.join(FLAGS.storage_dir, terrain_file_name) + gfile.Copy(asset_source, asset_destination, overwrite=True) + terrain_file_name_complete = os.path.join(FLAGS.storage_dir, + terrain_file_name) + return terrain_file_name_complete diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_randomize_terrain_gym_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_randomize_terrain_gym_env_example.py new file mode 100644 index 000000000..0e5638790 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_randomize_terrain_gym_env_example.py @@ -0,0 +1,83 @@ +"""An example to run minitaur gym environment with randomized terrain. + +""" + +import math + +import numpy as np +import tensorflow as tf +#from google3.pyglib import app +#from google3.pyglib import flags +from pybullet_envs.minitaur.envs import minitaur_randomize_terrain_gym_env + +FLAGS = flags.FLAGS + +flags.DEFINE_enum("example_name", "reset", ["sine", "reset"], + "The name of the example: sine or reset.") + + +def ResetTerrainExample(): + """An example showing resetting random terrain env.""" + num_reset = 10 + steps = 100 + env = minitaur_randomize_terrain_gym_env.MinitaurRandomizeTerrainGymEnv( + render=True, + leg_model_enabled=False, + motor_velocity_limit=np.inf, + pd_control_enabled=True) + action = [math.pi / 2] * 8 + for _ in xrange(num_reset): + env.reset() + for _ in xrange(steps): + _, _, done, _ = env.step(action) + if done: + break + + +def SinePolicyExample(): + """An example of minitaur walking with a sine gait.""" + env = minitaur_randomize_terrain_gym_env.MinitaurRandomizeTerrainGymEnv( + render=True, + motor_velocity_limit=np.inf, + pd_control_enabled=True, + on_rack=False) + sum_reward = 0 + steps = 200 + amplitude_1_bound = 0.5 + amplitude_2_bound = 0.5 + speed = 40 + + for step_counter in xrange(steps): + time_step = 0.01 + t = step_counter * time_step + + amplitude1 = amplitude_1_bound + amplitude2 = amplitude_2_bound + steering_amplitude = 0 + if t < 10: + steering_amplitude = 0.5 + elif t < 20: + steering_amplitude = -0.5 + else: + steering_amplitude = 0 + + # Applying asymmetrical sine gaits to different legs can steer the minitaur. + a1 = math.sin(t * speed) * (amplitude1 + steering_amplitude) + a2 = math.sin(t * speed + math.pi) * (amplitude1 - steering_amplitude) + a3 = math.sin(t * speed) * amplitude2 + a4 = math.sin(t * speed + math.pi) * amplitude2 + action = [a1, a2, a2, a1, a3, a4, a4, a3] + _, reward, _, _ = env.step(action) + sum_reward += reward + + +def main(unused_argv): + if FLAGS.example_name == "sine": + SinePolicyExample() + elif FLAGS.example_name == "reset": + ResetTerrainExample() + + +if __name__ == "__main__": + tf.logging.set_verbosity(tf.logging.INFO) + app.run() diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env.py new file mode 100644 index 000000000..9f6f9b847 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env.py @@ -0,0 +1,221 @@ +"""This file implements the gym environment of minitaur alternating legs. + +""" +import collections +import math +from gym import spaces +import numpy as np +import minitaur_gym_env + +INIT_EXTENSION_POS = 2.0 +INIT_SWING_POS = 0.0 +NUM_LEGS = 4 +NUM_MOTORS = 2 * NUM_LEGS + +MinitaurPose = collections.namedtuple( + "MinitaurPose", + "swing_angle_1, swing_angle_2, swing_angle_3, swing_angle_4, " + "extension_angle_1, extension_angle_2, extension_angle_3, " + "extension_angle_4") + + +class MinitaurReactiveEnv(minitaur_gym_env.MinitaurGymEnv): + """The gym environment for the minitaur. + + It simulates the locomotion of a minitaur, a quadruped robot. The state space + include the angles, velocities and torques for all the motors and the action + space is the desired motor angle for each motor. The reward function is based + on how far the minitaur walks in 1000 steps and penalizes the energy + expenditure. + + """ + metadata = { + "render.modes": ["human", "rgb_array"], + "video.frames_per_second": 166 + } + + def __init__(self, + urdf_version=None, + energy_weight=0.005, + control_time_step=0.006, + action_repeat=6, + control_latency=0.02, + pd_latency=0.003, + on_rack=False, + motor_kp=1.0, + motor_kd=0.015, + remove_default_joint_damping=True, + render=False, + num_steps_to_log=1000, + accurate_motor_model_enabled=True, + use_angle_in_observation=True, + hard_reset=False, + env_randomizer=None, + log_path=None): + """Initialize the minitaur trotting gym environment. + + Args: + urdf_version: [DEFAULT_URDF_VERSION, DERPY_V0_URDF_VERSION] are allowable + versions. If None, DEFAULT_URDF_VERSION is used. Refer to + minitaur_gym_env for more details. + energy_weight: The weight of the energy term in the reward function. Refer + to minitaur_gym_env for more details. + control_time_step: The time step between two successive control signals. + action_repeat: The number of simulation steps that an action is repeated. + control_latency: The latency between get_observation() and the actual + observation. See minituar.py for more details. + pd_latency: The latency used to get motor angles/velocities used to + compute PD controllers. See minitaur.py for more details. + on_rack: Whether to place the minitaur on rack. This is only used to debug + the walking gait. In this mode, the minitaur"s base is hung midair so + that its walking gait is clearer to visualize. + motor_kp: The P gain of the motor. + motor_kd: The D gain of the motor. + remove_default_joint_damping: Whether to remove the default joint damping. + render: Whether to render the simulation. + num_steps_to_log: The max number of control steps in one episode. If the + number of steps is over num_steps_to_log, the environment will still + be running, but only first num_steps_to_log will be recorded in logging. + accurate_motor_model_enabled: Whether to use the accurate motor model from + system identification. Refer to minitaur_gym_env for more details. + use_angle_in_observation: Whether to include motor angles in observation. + hard_reset: Whether hard reset (swipe out everything and reload) the + simulation. If it is false, the minitaur is set to the default pose + and moved to the origin. + env_randomizer: An instance (or a list) of EnvRanzomier(s) that can + randomize the environment during when env.reset() is called and add + perturbations when env._step() is called. + log_path: The path to write out logs. For the details of logging, refer to + minitaur_logging.proto. + """ + self._use_angle_in_observation = use_angle_in_observation + + super(MinitaurReactiveEnv, self).__init__( + urdf_version=urdf_version, + energy_weight=energy_weight, + accurate_motor_model_enabled=accurate_motor_model_enabled, + motor_overheat_protection=True, + motor_kp=motor_kp, + motor_kd=motor_kd, + remove_default_joint_damping=remove_default_joint_damping, + control_latency=control_latency, + pd_latency=pd_latency, + on_rack=on_rack, + render=render, + hard_reset=hard_reset, + num_steps_to_log=num_steps_to_log, + env_randomizer=env_randomizer, + log_path=log_path, + control_time_step=control_time_step, + action_repeat=action_repeat) + + action_dim = 8 + action_low = np.array([-0.5] * action_dim) + action_high = -action_low + self.action_space = spaces.Box(action_low, action_high) + self._cam_dist = 1.0 + self._cam_yaw = 30 + self._cam_pitch = -30 + + def _reset(self): + # TODO(b/73666007): Use composition instead of inheritance. + # (http://go/design-for-testability-no-inheritance). + init_pose = MinitaurPose( + swing_angle_1=INIT_SWING_POS, + swing_angle_2=INIT_SWING_POS, + swing_angle_3=INIT_SWING_POS, + swing_angle_4=INIT_SWING_POS, + extension_angle_1=INIT_EXTENSION_POS, + extension_angle_2=INIT_EXTENSION_POS, + extension_angle_3=INIT_EXTENSION_POS, + extension_angle_4=INIT_EXTENSION_POS) + # TODO(b/73734502): Refactor input of _convert_from_leg_model to namedtuple. + initial_motor_angles = self._convert_from_leg_model(list(init_pose)) + super(MinitaurReactiveEnv, self)._reset( + initial_motor_angles=initial_motor_angles, reset_duration=0.5) + return self._get_observation() + + def _convert_from_leg_model(self, leg_pose): + motor_pose = np.zeros(NUM_MOTORS) + for i in range(NUM_LEGS): + motor_pose[2 * i] = leg_pose[NUM_LEGS + i] - (-1)**(i / 2) * leg_pose[i] + motor_pose[2 * i + 1] = ( + leg_pose[NUM_LEGS + i] + (-1)**(i / 2) * leg_pose[i]) + return motor_pose + + def _signal(self, t): + initial_pose = np.array([ + INIT_SWING_POS, INIT_SWING_POS, INIT_SWING_POS, INIT_SWING_POS, + INIT_EXTENSION_POS, INIT_EXTENSION_POS, INIT_EXTENSION_POS, + INIT_EXTENSION_POS + ]) + return initial_pose + + def _transform_action_to_motor_command(self, action): + # Add the reference trajectory (i.e. the trotting signal). + action += self._signal(self.minitaur.GetTimeSinceReset()) + return self._convert_from_leg_model(action) + + def is_fallen(self): + """Decides whether the minitaur is in a fallen state. + + If the roll or the pitch of the base is greater than 0.3 radians, the + minitaur is considered fallen. + + Returns: + Boolean value that indicates whether the minitaur has fallen. + """ + roll, pitch, _ = self.minitaur.GetTrueBaseRollPitchYaw() + return math.fabs(roll) > 0.3 or math.fabs(pitch) > 0.3 + + def _get_true_observation(self): + """Get the true observations of this environment. + + It includes the roll, the pitch, the roll dot and the pitch dot of the base. + If _use_angle_in_observation is true, eight motor angles are added into the + observation. + + Returns: + The observation list, which is a numpy array of floating-point values. + """ + roll, pitch, _ = self.minitaur.GetTrueBaseRollPitchYaw() + roll_rate, pitch_rate, _ = self.minitaur.GetTrueBaseRollPitchYawRate() + observation = [roll, pitch, roll_rate, pitch_rate] + if self._use_angle_in_observation: + observation.extend(self.minitaur.GetMotorAngles().tolist()) + self._true_observation = np.array(observation) + return self._true_observation + + def _get_observation(self): + roll, pitch, _ = self.minitaur.GetBaseRollPitchYaw() + roll_rate, pitch_rate, _ = self.minitaur.GetBaseRollPitchYawRate() + observation = [roll, pitch, roll_rate, pitch_rate] + if self._use_angle_in_observation: + observation.extend(self.minitaur.GetMotorAngles().tolist()) + self._observation = np.array(observation) + return self._observation + + def _get_observation_upper_bound(self): + """Get the upper bound of the observation. + + Returns: + The upper bound of an observation. See _get_true_observation() for the + details of each element of an observation. + """ + upper_bound_roll = 2 * math.pi + upper_bound_pitch = 2 * math.pi + upper_bound_roll_dot = 2 * math.pi / self._time_step + upper_bound_pitch_dot = 2 * math.pi / self._time_step + upper_bound_motor_angle = 2 * math.pi + upper_bound = [ + upper_bound_roll, upper_bound_pitch, upper_bound_roll_dot, + upper_bound_pitch_dot + ] + + if self._use_angle_in_observation: + upper_bound.extend([upper_bound_motor_angle] * NUM_MOTORS) + return np.array(upper_bound) + + def _get_observation_lower_bound(self): + lower_bound = -self._get_observation_upper_bound() + return lower_bound diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py new file mode 100644 index 000000000..41010d4b5 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py @@ -0,0 +1,55 @@ +r"""An example to use simple_ppo_agent. + +blaze run -c opt \ +//robotics/reinforcement_learning/minitaur/envs:minitaur_reactive_env_example +""" + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +import os +import time +import tensorflow as tf +from agents.scripts import utility +import simple_ppo_agent + +flags = tf.app.flags +FLAGS = tf.app.flags.FLAGS +LOG_DIR = ( + "testdata/minitaur_reactive_env_test") +CHECKPOINT = "model.ckpt-14000000" + + +def main(argv): + del argv # Unused. + config = utility.load_config(LOG_DIR) + policy_layers = config.policy_layers + value_layers = config.value_layers + env = config.env(render=True) + network = config.network + + with tf.Session() as sess: + agent = simple_ppo_agent.SimplePPOPolicy( + sess, + env, + network, + policy_layers=policy_layers, + value_layers=value_layers, + checkpoint=os.path.join(LOG_DIR, CHECKPOINT)) + + sum_reward = 0 + observation = env.reset() + while True: + action = agent.get_action([observation]) + observation, reward, done, _ = env.step(action[0]) + # This sleep is to prevent serial communication error on the real robot. + time.sleep(0.002) + sum_reward += reward + if done: + break + tf.logging.info("reward: %s", sum_reward) + + +if __name__ == "__main__": + tf.app.run(main) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_stand_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_stand_gym_env.py new file mode 100644 index 000000000..76fa9d2ba --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_stand_gym_env.py @@ -0,0 +1,241 @@ +"""This file implements the gym environment of minitaur. + +""" +import math +from operator import add +import time + +from gym import spaces +import numpy as np +from pybullet_envs.minitaur.envs import minitaur_gym_env + +ACTION_EPS = 0.01 +# RANGE_OF_LEG_MOTION defines how far legs can rotate in both directions +# (1.0 means rotation pi/2 (radians) in both directions). +RANGE_OF_LEG_MOTION = 0.7 +# LIMIT_FALLEN defines when to consider robot fallen to the ground. +# This is the vertical component of the robot's front vector (unit vector). +# 0.0 represents body of the robot being horizontal, 1.0 means vertical. +LIMIT_FALLEN = 0.7 + + +class MinitaurStandGymEnv(minitaur_gym_env.MinitaurGymEnv): + """The gym environment for the minitaur and a ball. + + It simulates the standing up behavior of a minitaur, a quadruped robot. The + state space include the angles, velocities and torques for all the motors and + the action space is the desired motor angle for each motor. The reward + function is based on how long the minitaur stays standing up. + + """ + metadata = { + "render.modes": ["human", "rgb_array"], + "video.frames_per_second": 50 + } + + def __init__(self, + urdf_root="third_party/bullet/data", + action_repeat=1, + observation_noise_stdev=minitaur_gym_env.SENSOR_NOISE_STDDEV, + self_collision_enabled=True, + motor_velocity_limit=np.inf, + pd_control_enabled=False, + render=False): + """Initialize the minitaur standing up gym environment. + + Args: + urdf_root: The path to the urdf data folder. + action_repeat: The number of simulation steps before actions are applied. + observation_noise_stdev: The standard deviation of observation noise. + self_collision_enabled: Whether to enable self collision in the sim. + motor_velocity_limit: The velocity limit of each motor. + pd_control_enabled: Whether to use PD controller for each motor. + render: Whether to render the simulation. + """ + super(MinitaurStandGymEnv, self).__init__( + urdf_root=urdf_root, + action_repeat=action_repeat, + observation_noise_stdev=observation_noise_stdev, + self_collision_enabled=self_collision_enabled, + motor_velocity_limit=motor_velocity_limit, + pd_control_enabled=pd_control_enabled, + accurate_motor_model_enabled=True, + motor_overheat_protection=True, + render=render) + # Set the action dimension to 1, and reset the action space. + action_dim = 1 + action_high = np.array([self._action_bound] * action_dim) + self.action_space = spaces.Box(-action_high, action_high) + + def _stand_up(self): + """Make the robot stand up to its two legs when started on 4 legs. + + This method is similar to the step function, but instead of using the action + provided, it uses a hand-coded policy to make the robot stand up to its + two legs. Once the robot is vertical enough it exits and leaves the + environment to the typical step function that uses agent's actions. + + Returns: + observations: The angles, velocities and torques of all motors. + reward: The reward for the current state-action pair. + done: Whether the episode has ended. + info: A dictionary that stores diagnostic information. + """ + for t in xrange(5000): + if self._is_render: + base_pos = self.minitaur.GetBasePosition() + self._pybullet_client.resetDebugVisualizerCamera( + self._cam_dist, self._cam_yaw, self._cam_pitch, base_pos) + state = self._get_true_observation() + action = self._policy_flip(t, state[24:28]) + self.minitaur.ApplyAction(action) + self._pybullet_client.stepSimulation() + self.minitaur.ReceiveObservation() + if self._is_render: + time.sleep(self._time_step) + self._env_step_counter += 1 + reward = self._reward() + if reward > 0.9: + break + done = self._termination() + return np.array(self._get_observation()), reward, done, {} + + def _step(self, action): + # At start, use policy_flip to lift the robot to its two legs. After the + # robot reaches the lift up stage, give control back to the agent by + # returning the current state and reward. + if self._env_step_counter < 1: + return self._stand_up() + return super(MinitaurStandGymEnv, self)._step(action) + + def _reward(self): + """Reward function for standing up pose. + + Returns: + reward: A number between -1 and 1 according to how vertical is the body of + the robot. + """ + orientation = self.minitaur.GetBaseOrientation() + rot_matrix = self._pybullet_client.getMatrixFromQuaternion(orientation) + local_front_vec = rot_matrix[6:9] + alignment = abs(np.dot(np.asarray([1, 0, 0]), np.asarray(local_front_vec))) + return alignment**4 + + def _termination(self): + if self._is_horizontal(): + return True + return False + + def _is_horizontal(self): + """Decide whether minitaur is almost parallel to the ground. + + This method is used in experiments where the robot is learning to stand up. + + Returns: + Boolean value that indicates whether the minitaur is almost parallel to + the ground. + """ + orientation = self.minitaur.GetBaseOrientation() + rot_matrix = self._pybullet_client.getMatrixFromQuaternion(orientation) + front_z_component = rot_matrix[6] + return abs(front_z_component) < LIMIT_FALLEN + + def _transform_action_to_motor_command(self, action): + """Method to transform the one dimensional action to rotate bottom two legs. + + Args: + action: A double between -1 and 1, where 0 means keep the legs parallel + to the body. + Returns: + actions: The angles for all motors. + Raises: + ValueError: The action dimension is not the same as the number of motors. + ValueError: The magnitude of actions is out of bounds. + """ + action = action[0] + # Scale the action from [-1 to 1] to [-range to +range] (angle in radians). + action *= RANGE_OF_LEG_MOTION + action_all_legs = [ + math.pi, # Upper leg pointing up. + 0, + 0, # Bottom leg pointing down. + math.pi, + 0, # Upper leg pointing up. + math.pi, + math.pi, # Bottom leg pointing down. + 0 + ] + action_all_legs = [angle - 0.7 for angle in action_all_legs] + + # Use the one dimensional action to rotate both bottom legs. + action_delta = [0, 0, -action, action, 0, 0, action, -action] + action_all_legs = map(add, action_all_legs, action_delta) + return action_all_legs + + def _policy_flip(self, time_step, orientation): + """Hand coded policy to make the minitaur stand up to its two legs. + + This method is the hand coded policy that uses sine waves and orientation + of the robot to make it stand up to its two legs. It is composed of these + behaviors: + - Rotate bottom legs to always point to the ground + - Rotate upper legs the other direction so that they point to the sky when + the robot is standing up, and they point to the ground when the robot is + horizontal. + - Shorten the bottom 2 legs + - Shorten the other two legs, then when the sine wave hits its maximum, + extend the legs pushing the robot up. + + Args: + time_step: The time (or frame number) used for sine function. + orientation: Quaternion specifying the orientation of the body. + + Returns: + actions: The angles for all motors. + """ + # Set the default behavior (stand on 4 short legs). + shorten = -0.7 + a0 = math.pi / 2 + shorten + a1 = math.pi / 2 + shorten + a2 = math.pi / 2 + shorten + a3 = math.pi / 2 + shorten + a4 = math.pi / 2 + shorten + a5 = math.pi / 2 + shorten + a6 = math.pi / 2 + shorten + a7 = math.pi / 2 + shorten + + # Rotate back legs to point to the ground independent of the orientation. + rot_matrix = self._pybullet_client.getMatrixFromQuaternion(orientation) + local_up = rot_matrix[6:] + multiplier = np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) + a2 -= (1 - multiplier) * (math.pi / 2) + a3 += (1 - multiplier) * (math.pi / 2) + a6 += (1 - multiplier) * (math.pi / 2) + a7 -= (1 - multiplier) * (math.pi / 2) + + # Rotate front legs the other direction, so that it points up when standing. + a0 += (1 - multiplier) * (math.pi / 2) + a1 -= (1 - multiplier) * (math.pi / 2) + a4 -= (1 - multiplier) * (math.pi / 2) + a5 += (1 - multiplier) * (math.pi / 2) + + # Periodically push the upper legs to stand up. + speed = 0.01 + intensity = 1.9 + # Lower the signal a little, so that it becomes positive only for a short + # amount time. + lower_signal = -0.94 + signal_unit = math.copysign(intensity, + math.sin(time_step * speed) + lower_signal) + # Only extend the leg, don't shorten. + if signal_unit < 0: + signal_unit = 0 + + # Only apply it after some time. + if time_step * speed > math.pi: + a0 += signal_unit + a1 += signal_unit + a4 += signal_unit + a5 += signal_unit + joint_values = [a0, a1, a2, a3, a4, a5, a6, a7] + return joint_values diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_stand_gym_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_stand_gym_env_example.py new file mode 100644 index 000000000..ff7ed2948 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_stand_gym_env_example.py @@ -0,0 +1,35 @@ +"""An example to run of the minitaur gym environment with standing up goal. + +""" + +import math + +import numpy as np +import tensorflow as tf + +from pybullet_envs.minitaur.envs import minitaur_stand_gym_env + + +def StandUpExample(): + """An example that the minitaur stands up.""" + steps = 1000 + environment = minitaur_stand_gym_env.MinitaurStandGymEnv( + render=True, + motor_velocity_limit=np.inf) + action = [0.5] + _, _, done, _ = environment.step(action) + for t in xrange(steps): + # A policy that oscillates between -1 and 1 + action = [math.sin(t * math.pi * 0.01)] + _, _, done, _ = environment.step(action) + if done: + break + + +def main(unused_argv): + StandUpExample() + + +if __name__ == "__main__": + tf.logging.set_verbosity(tf.logging.INFO) + app.run() diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_trotting_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_trotting_env.py new file mode 100644 index 000000000..3b5ff9e83 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_trotting_env.py @@ -0,0 +1,324 @@ +"""Implements the gym environment of minitaur moving with trotting style. +""" +import math + +from gym import spaces +import numpy as np +from pybullet_envs.minitaur.envs import minitaur_gym_env + +# TODO(tingnan): These constants should be moved to minitaur/minitaur_gym_env. +NUM_LEGS = 4 +NUM_MOTORS = 2 * NUM_LEGS + + +class MinitaurTrottingEnv(minitaur_gym_env.MinitaurGymEnv): + """The trotting gym environment for the minitaur. + + In this env, Minitaur performs a trotting style locomotion specified by + extension_amplitude, swing_amplitude, and step_frequency. Each diagonal pair + of legs will move according to the reference trajectory: + extension = extsion_amplitude * cos(2 * pi * step_frequency * t + phi) + swing = swing_amplitude * sin(2 * pi * step_frequency * t + phi) + And the two diagonal leg pairs have a phase (phi) difference of pi. The + reference signal may be modified by the feedback actions from a balance + controller (e.g. a neural network). + + """ + metadata = { + "render.modes": ["human", "rgb_array"], + "video.frames_per_second": 166 + } + + def __init__(self, + urdf_version=None, + control_time_step=0.006, + action_repeat=6, + control_latency=0.02, + pd_latency=0.003, + on_rack=False, + motor_kp=1.0, + motor_kd=0.015, + remove_default_joint_damping=True, + render=False, + num_steps_to_log=1000, + accurate_motor_model_enabled=True, + use_signal_in_observation=False, + use_angle_in_observation=False, + hard_reset=False, + env_randomizer=None, + log_path=None, + init_extension=2.0, + init_swing=0.0, + step_frequency=2.0, + extension_amplitude=0.35, + swing_amplitude=0.3): + """Initialize the minitaur trotting gym environment. + + Args: + urdf_version: [DEFAULT_URDF_VERSION, DERPY_V0_URDF_VERSION, + RAINBOW_DASH_V0_URDF_VERSION] are allowable versions. If None, + DEFAULT_URDF_VERSION is used. Refer to minitaur_gym_env for more + details. + control_time_step: The time step between two successive control signals. + action_repeat: The number of simulation steps that an action is repeated. + control_latency: The latency between get_observation() and the actual + observation. See minituar.py for more details. + pd_latency: The latency used to get motor angles/velocities used to + compute PD controllers. See minitaur.py for more details. + on_rack: Whether to place the minitaur on rack. This is only used to debug + the walking gait. In this mode, the minitaur's base is hung midair so + that its walking gait is clearer to visualize. + motor_kp: The P gain of the motor. + motor_kd: The D gain of the motor. + remove_default_joint_damping: Whether to remove the default joint damping. + render: Whether to render the simulation. + num_steps_to_log: The max number of control steps in one episode. If the + number of steps is over num_steps_to_log, the environment will still + be running, but only first num_steps_to_log will be recorded in logging. + accurate_motor_model_enabled: Uses the nonlinear DC motor model if set to + True. + use_signal_in_observation: Includes the reference motor angles in the + observation vector. + use_angle_in_observation: Includes the measured motor angles in the + observation vector. + hard_reset: Whether to reset the whole simulation environment or just + reposition the robot. + env_randomizer: A list of EnvRandomizers that can randomize the + environment during when env.reset() is called and add perturbation + forces when env._step() is called. + log_path: The path to write out logs. For the details of logging, refer to + minitaur_logging.proto. + init_extension: The initial reset length of the leg. + init_swing: The initial reset swing position of the leg. + step_frequency: The desired leg stepping frequency. + extension_amplitude: The maximum leg extension change within a locomotion + cycle. + swing_amplitude: The maximum leg swing change within a cycle. + """ + + # _swing_offset and _extension_offset is to mimick the bent legs. The + # offsets will be added when applying the motor commands. + self._swing_offset = np.zeros(NUM_LEGS) + self._extension_offset = np.zeros(NUM_LEGS) + + # The reset position. + self._init_pose = [ + init_swing, init_swing, init_swing, init_swing, init_extension, + init_extension, init_extension, init_extension + ] + + self._step_frequency = step_frequency + self._extension_amplitude = extension_amplitude + self._swing_amplitude = swing_amplitude + self._use_signal_in_observation = use_signal_in_observation + self._use_angle_in_observation = use_angle_in_observation + super(MinitaurTrottingEnv, self).__init__( + urdf_version=urdf_version, + accurate_motor_model_enabled=accurate_motor_model_enabled, + motor_overheat_protection=True, + motor_kp=motor_kp, + motor_kd=motor_kd, + remove_default_joint_damping=remove_default_joint_damping, + control_latency=control_latency, + pd_latency=pd_latency, + on_rack=on_rack, + render=render, + hard_reset=hard_reset, + num_steps_to_log=num_steps_to_log, + env_randomizer=env_randomizer, + log_path=log_path, + control_time_step=control_time_step, + action_repeat=action_repeat) + + action_dim = NUM_LEGS * 2 + action_high = np.array([0.25] * action_dim) + self.action_space = spaces.Box(-action_high, action_high) + + # For render purpose. + self._cam_dist = 1.0 + self._cam_yaw = 30 + self._cam_pitch = -30 + + def _reset(self): + # In this environment, the actions are + # [swing leg 1, swing leg 2, swing leg 3, swing leg 4, + # extension leg 1, extension leg 2, extension leg 3, extension leg 4] + initial_motor_angles = self._convert_from_leg_model(self._init_pose) + super(MinitaurTrottingEnv, self)._reset( + initial_motor_angles=initial_motor_angles, reset_duration=0.5) + return self._get_observation() + + def _convert_from_leg_model(self, leg_pose): + """Converts leg space action into motor commands. + + Args: + leg_pose: A numpy array. leg_pose[0:NUM_LEGS] are leg swing angles + and leg_pose[NUM_LEGS:2*NUM_LEGS] contains leg extensions. + + Returns: + A numpy array of the corresponding motor angles for the given leg pose. + """ + motor_pose = np.zeros(NUM_MOTORS) + for i in range(NUM_LEGS): + motor_pose[2 * i] = leg_pose[NUM_LEGS + i] - (-1)**(i / 2) * leg_pose[i] + motor_pose[2 * i + + 1] = leg_pose[NUM_LEGS + i] + (-1)**(i / 2) * leg_pose[i] + return motor_pose + + def _gen_signal(self, t, phase): + """Generates a sinusoidal reference leg trajectory. + + The foot (leg tip) will move in a ellipse specified by extension and swing + amplitude. + + Args: + t: Current time in simulation. + phase: The phase offset for the periodic trajectory. + + Returns: + The desired leg extension and swing angle at the current time. + """ + period = 1 / self._step_frequency + extension = self._extension_amplitude * math.cos( + 2 * math.pi / period * t + phase) + swing = self._swing_amplitude * math.sin(2 * math.pi / period * t + phase) + return extension, swing + + def _signal(self, t): + """Generates the trotting gait for the robot. + + Args: + t: Current time in simulation. + + Returns: + A numpy array of the reference leg positions. + """ + # Generates the leg trajectories for the two digonal pair of legs. + ext_first_pair, sw_first_pair = self._gen_signal(t, 0) + ext_second_pair, sw_second_pair = self._gen_signal(t, math.pi) + + trotting_signal = np.array([ + sw_first_pair, sw_second_pair, sw_second_pair, sw_first_pair, + ext_first_pair, ext_second_pair, ext_second_pair, ext_first_pair + ]) + signal = np.array(self._init_pose) + trotting_signal + return signal + + def _transform_action_to_motor_command(self, action): + """Generates the motor commands for the given action. + + Swing/extension offsets and the reference leg trajectory will be added on + top of the inputs before the conversion. + + Args: + action: A numpy array contains the leg swings and extensions that will be + added to the reference trotting trajectory. action[0:NUM_LEGS] are leg + swing angles, and action[NUM_LEGS:2*NUM_LEGS] contains leg extensions. + + Returns: + A numpy array of the desired motor angles for the given leg space action. + """ + # Add swing_offset and extension_offset to mimick the bent legs. + action[0:NUM_LEGS] += self._swing_offset + action[NUM_LEGS:2 * NUM_LEGS] += self._extension_offset + + # Add the reference trajectory (i.e. the trotting signal). + action += self._signal(self.minitaur.GetTimeSinceReset()) + return self._convert_from_leg_model(action) + + def is_fallen(self): + """Decide whether the minitaur has fallen. + + Returns: + Boolean value that indicates whether the minitaur has fallen. + """ + roll, pitch, _ = self.minitaur.GetTrueBaseRollPitchYaw() + is_fallen = math.fabs(roll) > 0.3 or math.fabs(pitch) > 0.3 + return is_fallen + + def _get_true_observation(self): + """Get the true observations of this environment. + + It includes the true roll, pitch, roll dot and pitch dot of the base. Also + includes the disired/observed motor angles if the relevant flags are set. + + Returns: + The observation list. + """ + observation = [] + roll, pitch, _ = self.minitaur.GetTrueBaseRollPitchYaw() + roll_rate, pitch_rate, _ = self.minitaur.GetTrueBaseRollPitchYawRate() + observation.extend([roll, pitch, roll_rate, pitch_rate]) + if self._use_signal_in_observation: + observation.extend(self._transform_action_to_motor_command([0] * 8)) + if self._use_angle_in_observation: + observation.extend(self.minitaur.GetMotorAngles().tolist()) + self._true_observation = np.array(observation) + return self._true_observation + + def _get_observation(self): + """Get observations of this environment. + + It includes the base roll, pitch, roll dot and pitch dot which may contain + noises, bias, and latency. Also includes the disired/observed motor angles + if the relevant flags are set. + + Returns: + The observation list. + """ + observation = [] + roll, pitch, _ = self.minitaur.GetBaseRollPitchYaw() + roll_rate, pitch_rate, _ = self.minitaur.GetBaseRollPitchYawRate() + observation.extend([roll, pitch, roll_rate, pitch_rate]) + if self._use_signal_in_observation: + observation.extend(self._transform_action_to_motor_command([0] * 8)) + if self._use_angle_in_observation: + observation.extend(self.minitaur.GetMotorAngles().tolist()) + self._observation = np.array(observation) + return self._observation + + def _get_observation_upper_bound(self): + """Get the upper bound of the observation. + + Returns: + A numpy array contains the upper bound of an observation. See + GetObservation() for the details of each element of an observation. + """ + upper_bound = [] + upper_bound.extend([2 * math.pi] * 2) # Roll, pitch, yaw of the base. + upper_bound.extend( + [2 * math.pi / self._time_step] * 2) # Roll, pitch, yaw rate. + if self._use_signal_in_observation: + upper_bound.extend([2 * math.pi] * NUM_MOTORS) # Signal + if self._use_angle_in_observation: + upper_bound.extend([2 * math.pi] * NUM_MOTORS) # Motor angles + return np.array(upper_bound) + + def _get_observation_lower_bound(self): + """Get the lower bound of the observation. + + Returns: + The lower bound of an observation (the reverse of the upper bound). + """ + lower_bound = -self._get_observation_upper_bound() + return lower_bound + + def set_swing_offset(self, value): + """Set the swing offset of each leg. + + It is to mimic the bent leg. + + Args: + value: A list of four values. + """ + self._swing_offset = value + + def set_extension_offset(self, value): + """Set the extension offset of each leg. + + It is to mimic the bent leg. + + Args: + value: A list of four values. + """ + self._extension_offset = value diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_trotting_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_trotting_env_example.py new file mode 100644 index 000000000..1edba8727 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_trotting_env_example.py @@ -0,0 +1,41 @@ +"""An example to run the minitaur environment of trotting gait. + +""" +import time + +import numpy as np +import tensorflow as tf +from pybullet_envs.minitaur.envs import minitaur_gym_env +from pybullet_envs.minitaur.envs import minitaur_trotting_env + +#FLAGS = tf.flags.FLAGS +#tf.flags.DEFINE_string("log_path", None, "The directory to write the log file.") + + +def main(unused_argv): + environment = minitaur_trotting_env.MinitaurTrottingEnv( + urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION, + use_signal_in_observation=False, + use_angle_in_observation=False, + render=True, + log_path=FLAGS.log_path) + + np.random.seed(100) + sum_reward = 0 + environment.reset() + + steps = 5000 + for _ in xrange(steps): + # Sleep to prevent serial buffer overflow on microcontroller. + time.sleep(0.002) + action = [0] * 8 + _, reward, done, _ = environment.step(action) + sum_reward += reward + if done: + break + tf.logging.info("reward: {}".format(sum_reward)) + + +if __name__ == "__main__": + tf.logging.set_verbosity(tf.logging.INFO) + tf.app.run() diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/motor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/motor.py new file mode 100644 index 000000000..20e271e1b --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/motor.py @@ -0,0 +1,144 @@ +"""This file implements an accurate motor model.""" + +import numpy as np +VOLTAGE_CLIPPING = 50 +# TODO(b/73728631): Clamp the pwm signal instead of the OBSERVED_TORQUE_LIMIT. +OBSERVED_TORQUE_LIMIT = 5.7 +MOTOR_VOLTAGE = 16.0 +MOTOR_RESISTANCE = 0.186 +MOTOR_TORQUE_CONSTANT = 0.0954 +MOTOR_VISCOUS_DAMPING = 0 +MOTOR_SPEED_LIMIT = MOTOR_VOLTAGE / ( + MOTOR_VISCOUS_DAMPING + MOTOR_TORQUE_CONSTANT) +NUM_MOTORS = 8 + + +class MotorModel(object): + """The accurate motor model, which is based on the physics of DC motors. + + The motor model support two types of control: position control and torque + control. In position control mode, a desired motor angle is specified, and a + torque is computed based on the internal motor model. When the torque control + is specified, a pwm signal in the range of [-1.0, 1.0] is converted to the + torque. + + The internal motor model takes the following factors into consideration: + pd gains, viscous friction, back-EMF voltage and current-torque profile. + """ + + def __init__(self, torque_control_enabled=False, kp=1.2, kd=0): + self._torque_control_enabled = torque_control_enabled + self._kp = kp + self._kd = kd + self._resistance = MOTOR_RESISTANCE + self._voltage = MOTOR_VOLTAGE + self._torque_constant = MOTOR_TORQUE_CONSTANT + self._viscous_damping = MOTOR_VISCOUS_DAMPING + self._current_table = [0, 10, 20, 30, 40, 50, 60] + self._torque_table = [0, 1, 1.9, 2.45, 3.0, 3.25, 3.5] + self._strength_ratios = [1.0] * NUM_MOTORS + + def set_strength_ratios(self, ratios): + """Set the strength of each motors relative to the default value. + + Args: + ratios: The relative strength of motor output. A numpy array ranging from + 0.0 to 1.0. + """ + self._strength_ratios = np.array(ratios) + + def set_motor_gains(self, kp, kd): + """Set the gains of all motors. + + These gains are PD gains for motor positional control. kp is the + proportional gain and kd is the derivative gain. + + Args: + kp: proportional gain of the motors. + kd: derivative gain of the motors. + """ + self._kp = kp + self._kd = kd + + def set_voltage(self, voltage): + self._voltage = voltage + + def get_voltage(self): + return self._voltage + + def set_viscous_damping(self, viscous_damping): + self._viscous_damping = viscous_damping + + def get_viscous_dampling(self): + return self._viscous_damping + + def convert_to_torque(self, + motor_commands, + motor_angle, + motor_velocity, + true_motor_velocity, + kp=None, + kd=None): + """Convert the commands (position control or torque control) to torque. + + Args: + motor_commands: The desired motor angle if the motor is in position + control mode. The pwm signal if the motor is in torque control mode. + motor_angle: The motor angle observed at the current time step. It is + actually the true motor angle observed a few milliseconds ago (pd + latency). + motor_velocity: The motor velocity observed at the current time step, it + is actually the true motor velocity a few milliseconds ago (pd latency). + true_motor_velocity: The true motor velocity. The true velocity is used + to compute back EMF voltage and viscous damping. + kp: Proportional gains for the motors' PD controllers. If not provided, it + uses the default kp of the minitaur for all the motors. + kd: Derivative gains for the motors' PD controllers. If not provided, it + uses the default kp of the minitaur for all the motors. + + Returns: + actual_torque: The torque that needs to be applied to the motor. + observed_torque: The torque observed by the sensor. + """ + if self._torque_control_enabled: + pwm = motor_commands + else: + if kp is None: + kp = np.full(NUM_MOTORS, self._kp) + if kd is None: + kd = np.full(NUM_MOTORS, self._kd) + pwm = -1 * kp * (motor_angle - motor_commands) - kd * motor_velocity + + pwm = np.clip(pwm, -1.0, 1.0) + return self._convert_to_torque_from_pwm(pwm, true_motor_velocity) + + def _convert_to_torque_from_pwm(self, pwm, true_motor_velocity): + """Convert the pwm signal to torque. + + Args: + pwm: The pulse width modulation. + true_motor_velocity: The true motor velocity at the current moment. It is + used to compute the back EMF voltage and the viscous damping. + Returns: + actual_torque: The torque that needs to be applied to the motor. + observed_torque: The torque observed by the sensor. + """ + observed_torque = np.clip( + self._torque_constant * + (np.asarray(pwm) * self._voltage / self._resistance), + -OBSERVED_TORQUE_LIMIT, OBSERVED_TORQUE_LIMIT) + + # Net voltage is clipped at 50V by diodes on the motor controller. + voltage_net = np.clip( + np.asarray(pwm) * self._voltage - + (self._torque_constant + self._viscous_damping) * + np.asarray(true_motor_velocity), -VOLTAGE_CLIPPING, VOLTAGE_CLIPPING) + current = voltage_net / self._resistance + current_sign = np.sign(current) + current_magnitude = np.absolute(current) + # Saturate torque based on empirical current relation. + actual_torque = np.interp(current_magnitude, self._current_table, + self._torque_table) + actual_torque = np.multiply(current_sign, actual_torque) + actual_torque = np.multiply(self._strength_ratios, actual_torque) + return actual_torque, observed_torque diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/simple_ppo_agent.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/simple_ppo_agent.py new file mode 100644 index 000000000..e3c4b4ed3 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/simple_ppo_agent.py @@ -0,0 +1,101 @@ +"""An agent that can restore and run a policy learned by PPO.""" + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + + +import tensorflow as tf +from pybullet_envs.agents.ppo import normalize +from pybullet_envs.agents import utility + + +class SimplePPOPolicy(object): + """A simple PPO policy that is independent to the PPO infrastructure. + + This class restores the policy network from a tensorflow checkpoint that was + learned from PPO training. The purpose of this class is to conveniently + visualize a learned policy or deploy the learned policy on real robots without + need to change the PPO evaluation infrastructure: + https://cs.corp.google.com/piper///depot/google3/robotics/reinforcement_learning/agents/scripts/visualize.py. + """ + + def __init__(self, sess, env, network, policy_layers, value_layers, + checkpoint): + self.env = env + self.sess = sess + observation_size = len(env.observation_space.low) + action_size = len(env.action_space.low) + self.observation_placeholder = tf.placeholder( + tf.float32, [None, observation_size], name="Input") + self._observ_filter = normalize.StreamingNormalize( + self.observation_placeholder[0], + center=True, + scale=True, + clip=5, + name="normalize_observ") + self._restore_policy( + network, + policy_layers=policy_layers, + value_layers=value_layers, + action_size=action_size, + checkpoint=checkpoint) + + def _restore_policy(self, network, policy_layers, value_layers, action_size, + checkpoint): + """Restore the PPO policy from a TensorFlow checkpoint. + + Args: + network: The neural network definition. + policy_layers: A tuple specify the number of layers and number of neurons + of each layer for the policy network. + value_layers: A tuple specify the number of layers and number of neurons + of each layer for the value network. + action_size: The dimension of the action space. + checkpoint: The checkpoint path. + """ + observ = self._observ_filter.transform(self.observation_placeholder) + with tf.variable_scope("network/rnn"): + self.network = network( + policy_layers=policy_layers, + value_layers=value_layers, + action_size=action_size) + + with tf.variable_scope("temporary"): + self.last_state = tf.Variable( + self.network.zero_state(1, tf.float32), False) + self.sess.run(self.last_state.initializer) + + with tf.variable_scope("network"): + (mean_action, _, _), new_state = tf.nn.dynamic_rnn( + self.network, + observ[:, None], + tf.ones(1), + self.last_state, + tf.float32, + swap_memory=True) + self.mean_action = mean_action + self.update_state = self.last_state.assign(new_state) + + saver = utility.define_saver(exclude=(r"temporary/.*",)) + saver.restore(self.sess, checkpoint) + + def get_action(self, observation): + normalized_observation = self._normalize_observ(observation) + normalized_action, _ = self.sess.run( + [self.mean_action, self.update_state], + feed_dict={self.observation_placeholder: normalized_observation}) + action = self._denormalize_action(normalized_action) + return action[:, 0] + + def _denormalize_action(self, action): + min_ = self.env.action_space.low + max_ = self.env.action_space.high + action = (action + 1) / 2 * (max_ - min_) + min_ + return action + + def _normalize_observ(self, observ): + min_ = self.env.observation_space.low + max_ = self.env.observation_space.high + observ = 2 * (observ - min_) / (max_ - min_) - 1 + return observ diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/simple_ppo_agent_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/simple_ppo_agent_example.py new file mode 100644 index 000000000..0d6f6f807 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/simple_ppo_agent_example.py @@ -0,0 +1,69 @@ +r"""An example to use simple_ppo_agent. + +A galloping example: +blaze run -c opt \ +//robotics/reinforcement_learning/minitaur/agents:simple_ppo_agent_example -- \ +--logdir=/cns/ij-d/home/jietan/experiment/minitaur_vizier_study_ppo/\ +minreact_nonexp_nr_01_186515603_186518344/15/ \ +--checkpoint=model.ckpt-14000000 + +A trotting example: +blaze run -c opt \ +//robotics/reinforcement_learning/minitaur/agents:simple_ppo_agent_example -- \ +--logdir=/cns/ij-d/home/jietan/experiment/minitaur_vizier_study_ppo/\ +mintrot_nonexp_rd_01_186515603_186518344/24/ \ +--checkpoint=model.ckpt-14000000 + +""" + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +import os +import time + +import tensorflow as tf +from pybullet_envs.agents import utility +from pybullet_envs.minitaur.agents import simple_ppo_agent + +flags = tf.app.flags +FLAGS = tf.app.flags.FLAGS +flags.DEFINE_string("logdir", None, + "The directory that contains checkpoint and config.") +flags.DEFINE_string("checkpoint", None, "The checkpoint file path.") +flags.DEFINE_string("log_path", None, "The output path to write log.") + + +def main(argv): + del argv # Unused. + config = utility.load_config(FLAGS.logdir) + policy_layers = config.policy_layers + value_layers = config.value_layers + env = config.env(render=True, log_path=FLAGS.log_path, env_randomizer=None) + network = config.network + + with tf.Session() as sess: + agent = simple_ppo_agent.SimplePPOPolicy( + sess, + env, + network, + policy_layers=policy_layers, + value_layers=value_layers, + checkpoint=os.path.join(FLAGS.logdir, FLAGS.checkpoint)) + + sum_reward = 0 + observation = env.reset() + while True: + action = agent.get_action([observation]) + observation, reward, done, _ = env.step(action[0]) + # This sleep is to prevent serial communication error on the real robot. + time.sleep(0.002) + sum_reward += reward + if done: + break + tf.logging.info("reward: {}".format(sum_reward)) + + +if __name__ == "__main__": + tf.app.run(main) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp.proto b/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp.proto new file mode 100644 index 000000000..b67072deb --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp.proto @@ -0,0 +1,18 @@ +syntax = "proto3"; + +package google.protobuf; + +message Timestamp { + + // Represents seconds of UTC time since Unix epoch + // 1970-01-01T00:00:00Z. Must be from 0001-01-01T00:00:00Z to + // 9999-12-31T23:59:59Z inclusive. + int64 seconds = 1; + + // Non-negative fractions of a second at nanosecond resolution. Negative + // second values with fractions must still have non-negative nanos values + // that count forward in time. Must be from 0 to 999,999,999 + // inclusive. + int32 nanos = 2; +} + diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp_pb2.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp_pb2.py new file mode 100644 index 000000000..25e3a230d --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp_pb2.py @@ -0,0 +1,76 @@ +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: timestamp.proto + +import sys +_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) +from google.protobuf import descriptor as _descriptor +from google.protobuf import message as _message +from google.protobuf import reflection as _reflection +from google.protobuf import symbol_database as _symbol_database +from google.protobuf import descriptor_pb2 +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor.FileDescriptor( + name='timestamp.proto', + package='google.protobuf', + syntax='proto3', + serialized_pb=_b('\n\x0ftimestamp.proto\x12\x0fgoogle.protobuf\"+\n\tTimestamp\x12\x0f\n\x07seconds\x18\x01 \x01(\x03\x12\r\n\x05nanos\x18\x02 \x01(\x05\x62\x06proto3') +) + + + + +_TIMESTAMP = _descriptor.Descriptor( + name='Timestamp', + full_name='google.protobuf.Timestamp', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='seconds', full_name='google.protobuf.Timestamp.seconds', index=0, + number=1, type=3, cpp_type=2, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='nanos', full_name='google.protobuf.Timestamp.nanos', index=1, + number=2, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=36, + serialized_end=79, +) + +DESCRIPTOR.message_types_by_name['Timestamp'] = _TIMESTAMP +_sym_db.RegisterFileDescriptor(DESCRIPTOR) + +Timestamp = _reflection.GeneratedProtocolMessageType('Timestamp', (_message.Message,), dict( + DESCRIPTOR = _TIMESTAMP, + __module__ = 'timestamp_pb2' + # @@protoc_insertion_point(class_scope:google.protobuf.Timestamp) + )) +_sym_db.RegisterMessage(Timestamp) + + +# @@protoc_insertion_point(module_scope) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/vector.proto b/examples/pybullet/gym/pybullet_envs/minitaur/envs/vector.proto new file mode 100644 index 000000000..c12fa3bba --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/vector.proto @@ -0,0 +1,56 @@ +syntax = "proto3"; + +package robotics.messages; + +// A four-dimensional double precision vector. +message Vector4d { + double x = 1; + double y = 2; + double z = 3; + double w = 4; +} + +// A four-dimensional single precision vector. +message Vector4f { + float x = 1; + float y = 2; + float z = 3; + float w = 4; +} + +// A three-dimensional double precision vector. +message Vector3d { + double x = 1; + double y = 2; + double z = 3; +} + +// A three-dimensional single precision vector. +message Vector3f { + float x = 1; + float y = 2; + float z = 3; +} + +// A two-dimensional double precision vector. +message Vector2d { + double x = 1; + double y = 2; +} + +// A two-dimensional single precision vector. +message Vector2f { + float x = 1; + float y = 2; +} + +// Double precision vector of arbitrary size. +message Vectord { + repeated double data = 1 [packed = true]; +} + +// Single precision vector of arbitrary size. +message Vectorf { + repeated float data = 1 [packed = true]; +} + diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/vector_pb2.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/vector_pb2.py new file mode 100644 index 000000000..67bacb9cd --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/vector_pb2.py @@ -0,0 +1,430 @@ +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: vector.proto + +import sys +_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) +from google.protobuf import descriptor as _descriptor +from google.protobuf import message as _message +from google.protobuf import reflection as _reflection +from google.protobuf import symbol_database as _symbol_database +from google.protobuf import descriptor_pb2 +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor.FileDescriptor( + name='vector.proto', + package='robotics.messages', + syntax='proto3', + serialized_pb=_b('\n\x0cvector.proto\x12\x11robotics.messages\"6\n\x08Vector4d\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\x12\t\n\x01z\x18\x03 \x01(\x01\x12\t\n\x01w\x18\x04 \x01(\x01\"6\n\x08Vector4f\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\x12\t\n\x01w\x18\x04 \x01(\x02\"+\n\x08Vector3d\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\x12\t\n\x01z\x18\x03 \x01(\x01\"+\n\x08Vector3f\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\" \n\x08Vector2d\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\" \n\x08Vector2f\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\"\x1b\n\x07Vectord\x12\x10\n\x04\x64\x61ta\x18\x01 \x03(\x01\x42\x02\x10\x01\"\x1b\n\x07Vectorf\x12\x10\n\x04\x64\x61ta\x18\x01 \x03(\x02\x42\x02\x10\x01\x62\x06proto3') +) + + + + +_VECTOR4D = _descriptor.Descriptor( + name='Vector4d', + full_name='robotics.messages.Vector4d', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='x', full_name='robotics.messages.Vector4d.x', index=0, + number=1, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='y', full_name='robotics.messages.Vector4d.y', index=1, + number=2, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='z', full_name='robotics.messages.Vector4d.z', index=2, + number=3, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='w', full_name='robotics.messages.Vector4d.w', index=3, + number=4, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=35, + serialized_end=89, +) + + +_VECTOR4F = _descriptor.Descriptor( + name='Vector4f', + full_name='robotics.messages.Vector4f', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='x', full_name='robotics.messages.Vector4f.x', index=0, + number=1, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='y', full_name='robotics.messages.Vector4f.y', index=1, + number=2, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='z', full_name='robotics.messages.Vector4f.z', index=2, + number=3, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='w', full_name='robotics.messages.Vector4f.w', index=3, + number=4, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=91, + serialized_end=145, +) + + +_VECTOR3D = _descriptor.Descriptor( + name='Vector3d', + full_name='robotics.messages.Vector3d', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='x', full_name='robotics.messages.Vector3d.x', index=0, + number=1, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='y', full_name='robotics.messages.Vector3d.y', index=1, + number=2, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='z', full_name='robotics.messages.Vector3d.z', index=2, + number=3, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=147, + serialized_end=190, +) + + +_VECTOR3F = _descriptor.Descriptor( + name='Vector3f', + full_name='robotics.messages.Vector3f', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='x', full_name='robotics.messages.Vector3f.x', index=0, + number=1, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='y', full_name='robotics.messages.Vector3f.y', index=1, + number=2, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='z', full_name='robotics.messages.Vector3f.z', index=2, + number=3, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=192, + serialized_end=235, +) + + +_VECTOR2D = _descriptor.Descriptor( + name='Vector2d', + full_name='robotics.messages.Vector2d', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='x', full_name='robotics.messages.Vector2d.x', index=0, + number=1, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='y', full_name='robotics.messages.Vector2d.y', index=1, + number=2, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=237, + serialized_end=269, +) + + +_VECTOR2F = _descriptor.Descriptor( + name='Vector2f', + full_name='robotics.messages.Vector2f', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='x', full_name='robotics.messages.Vector2f.x', index=0, + number=1, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='y', full_name='robotics.messages.Vector2f.y', index=1, + number=2, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=271, + serialized_end=303, +) + + +_VECTORD = _descriptor.Descriptor( + name='Vectord', + full_name='robotics.messages.Vectord', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='data', full_name='robotics.messages.Vectord.data', index=0, + number=1, type=1, cpp_type=5, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=_descriptor._ParseOptions(descriptor_pb2.FieldOptions(), _b('\020\001')), file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=305, + serialized_end=332, +) + + +_VECTORF = _descriptor.Descriptor( + name='Vectorf', + full_name='robotics.messages.Vectorf', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='data', full_name='robotics.messages.Vectorf.data', index=0, + number=1, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=_descriptor._ParseOptions(descriptor_pb2.FieldOptions(), _b('\020\001')), file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=334, + serialized_end=361, +) + +DESCRIPTOR.message_types_by_name['Vector4d'] = _VECTOR4D +DESCRIPTOR.message_types_by_name['Vector4f'] = _VECTOR4F +DESCRIPTOR.message_types_by_name['Vector3d'] = _VECTOR3D +DESCRIPTOR.message_types_by_name['Vector3f'] = _VECTOR3F +DESCRIPTOR.message_types_by_name['Vector2d'] = _VECTOR2D +DESCRIPTOR.message_types_by_name['Vector2f'] = _VECTOR2F +DESCRIPTOR.message_types_by_name['Vectord'] = _VECTORD +DESCRIPTOR.message_types_by_name['Vectorf'] = _VECTORF +_sym_db.RegisterFileDescriptor(DESCRIPTOR) + +Vector4d = _reflection.GeneratedProtocolMessageType('Vector4d', (_message.Message,), dict( + DESCRIPTOR = _VECTOR4D, + __module__ = 'vector_pb2' + # @@protoc_insertion_point(class_scope:robotics.messages.Vector4d) + )) +_sym_db.RegisterMessage(Vector4d) + +Vector4f = _reflection.GeneratedProtocolMessageType('Vector4f', (_message.Message,), dict( + DESCRIPTOR = _VECTOR4F, + __module__ = 'vector_pb2' + # @@protoc_insertion_point(class_scope:robotics.messages.Vector4f) + )) +_sym_db.RegisterMessage(Vector4f) + +Vector3d = _reflection.GeneratedProtocolMessageType('Vector3d', (_message.Message,), dict( + DESCRIPTOR = _VECTOR3D, + __module__ = 'vector_pb2' + # @@protoc_insertion_point(class_scope:robotics.messages.Vector3d) + )) +_sym_db.RegisterMessage(Vector3d) + +Vector3f = _reflection.GeneratedProtocolMessageType('Vector3f', (_message.Message,), dict( + DESCRIPTOR = _VECTOR3F, + __module__ = 'vector_pb2' + # @@protoc_insertion_point(class_scope:robotics.messages.Vector3f) + )) +_sym_db.RegisterMessage(Vector3f) + +Vector2d = _reflection.GeneratedProtocolMessageType('Vector2d', (_message.Message,), dict( + DESCRIPTOR = _VECTOR2D, + __module__ = 'vector_pb2' + # @@protoc_insertion_point(class_scope:robotics.messages.Vector2d) + )) +_sym_db.RegisterMessage(Vector2d) + +Vector2f = _reflection.GeneratedProtocolMessageType('Vector2f', (_message.Message,), dict( + DESCRIPTOR = _VECTOR2F, + __module__ = 'vector_pb2' + # @@protoc_insertion_point(class_scope:robotics.messages.Vector2f) + )) +_sym_db.RegisterMessage(Vector2f) + +Vectord = _reflection.GeneratedProtocolMessageType('Vectord', (_message.Message,), dict( + DESCRIPTOR = _VECTORD, + __module__ = 'vector_pb2' + # @@protoc_insertion_point(class_scope:robotics.messages.Vectord) + )) +_sym_db.RegisterMessage(Vectord) + +Vectorf = _reflection.GeneratedProtocolMessageType('Vectorf', (_message.Message,), dict( + DESCRIPTOR = _VECTORF, + __module__ = 'vector_pb2' + # @@protoc_insertion_point(class_scope:robotics.messages.Vectorf) + )) +_sym_db.RegisterMessage(Vectorf) + + +_VECTORD.fields_by_name['data'].has_options = True +_VECTORD.fields_by_name['data']._options = _descriptor._ParseOptions(descriptor_pb2.FieldOptions(), _b('\020\001')) +_VECTORF.fields_by_name['data'].has_options = True +_VECTORF.fields_by_name['data']._options = _descriptor._ParseOptions(descriptor_pb2.FieldOptions(), _b('\020\001')) +# @@protoc_insertion_point(module_scope) |