AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

first pass of updated minitaur quadruped environment

Browse Source
This commit is contained in:
erwincoumans 2018-03-31 21:15:27 -07:00
parent ec68290497
commit 14d37ecb43
42 changed files with 9464 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1073
examples/pybullet/gym/pybullet_data/quadruped/minitaur_derpy.urdf Normal file
View File

File diff suppressed because it is too large Load Diff

914
examples/pybullet/gym/pybullet_data/quadruped/minitaur_rainbow_dash.urdf Normal file
View File

@ -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>

980
examples/pybullet/gym/pybullet_data/quadruped/minitaur_rainbow_dash_v1.urdf Normal file
View File

@ -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>

203
examples/pybullet/gym/pybullet_data/quadruped/minitaur_single_motor.urdf Normal file
View File

@ -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>

33
examples/pybullet/gym/pybullet_data/sphere_with_restitution.urdf Normal file
View File

@ -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>

50
examples/pybullet/gym/pybullet_envs/minitaur/envs/bullet_client.py Normal file
View File

@ -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

35
examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py Normal file
View File

@ -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

59
examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_alternating_legs_env_randomizer.py Normal file
View File

@ -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))

69
examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_env_randomizer.py Normal file
View File

@ -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)

25
examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_env_randomizer_config.py Normal file
View File

@ -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

139
examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_env_randomizer_from_config.py Normal file
View File

@ -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))

91
examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py Normal file
View File

@ -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)

295
examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_terrain_randomizer.py Normal file
View File

@ -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])

996
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur.py Normal file
View File

@ -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

244
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_alternating_legs_env.py Normal file
View File

@ -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

109
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_alternating_legs_env_example.py Normal file
View File

@ -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()

147
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_ball_gym_env.py Normal file
View File

@ -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

34
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_ball_gym_env_example.py Normal file
View File

@ -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()

175
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_derpy.py Normal file
View File

@ -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)

359
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_four_leg_stand_env.py Normal file
View File

@ -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

69
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_four_leg_stand_env_example.py Normal file
View File

@ -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()

601
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py Normal file
View File

@ -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

217
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env_example.py Normal file
View File

@ -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()

41
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.proto Normal file
View File

@ -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;
}

142
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py Normal file
View File

@ -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

212
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging_pb2.py Normal file
View File

@ -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)

174
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_rainbow_dash.py Normal file
View File

@ -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)

84
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_randomize_terrain_gym_env.py Normal file
View File

@ -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

83
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_randomize_terrain_gym_env_example.py Normal file
View File

@ -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()

221
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env.py Normal file
View File

@ -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

55
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py Normal file
View File

@ -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)

241
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_stand_gym_env.py Normal file
View File

@ -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

35
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_stand_gym_env_example.py Normal file
View File

@ -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()

324
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_trotting_env.py Normal file
View File

@ -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

41
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_trotting_env_example.py Normal file
View File

@ -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()

144
examples/pybullet/gym/pybullet_envs/minitaur/envs/motor.py Normal file
View File

@ -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

101
examples/pybullet/gym/pybullet_envs/minitaur/envs/simple_ppo_agent.py Normal file
View File

@ -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

69
examples/pybullet/gym/pybullet_envs/minitaur/envs/simple_ppo_agent_example.py Normal file
View File

@ -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)

18
examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp.proto Normal file
View File

@ -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;
}

76
examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp_pb2.py Normal file
View File

@ -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)

56
examples/pybullet/gym/pybullet_envs/minitaur/envs/vector.proto Normal file
View File

@ -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];
}

430
examples/pybullet/gym/pybullet_envs/minitaur/envs/vector_pb2.py Normal file
View File

@ -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)