Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
Erwin Coumans
2017-06-21 12:00:53 -07:00
parent 5f770a833d 9213f944f1
commit d1481c78ba
14 changed files with 1143 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
data/racecar/meshes/chassis_differential.STL Normal file
View File

Binary file not shown.

743
data/racecar/racecar_differential.urdf Normal file
View File

@@ -0,0 +1,743 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from racecar.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="racecar" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- inertial parameter macros -->
<!-- geometry macros -->
<!-- transmission macros -->
<!-- Add chassis and it's inertia link -->
<link name="chassis">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="1 1 1 1" filename="meshes/chassis_differential.STL"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/chassis_differential.STL"/>
</geometry>
<material name="blue"/>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.1477 0 0"/>
<mass value="4.0"/>
<inertia ixx="0.010609" ixy="0" ixz="0" iyy="0.050409" iyz="0" izz="0.05865"/>
</inertial>
</link>
<!-- Add the left rear wheel with its joints and tranmissions -->
<link name="left_rear_wheel">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 -0.0225"/>
<mass value="0.034055"/>
<inertia ixx="0.00026046" ixy="0" ixz="0" iyy="0.00026046" iyz="0" izz="0.00041226"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/left_rear_wheel.obj"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.0225"/>
<geometry>
<cylinder length="0.045" radius="0.05"/>
</geometry>
</collision>
</link>
<joint name="left_rear_wheel_joint" type="fixed">
<origin rpy="1.5708 0 0" xyz="0 0.1 0"/>
<parent link="diff_sideA"/>
<child link="left_rear_wheel"/>
</joint>
<!-- Add the right rear wheel with its joints and tranmissions -->
<link name="right_rear_wheel">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.0225"/>
<mass value="0.034055"/>
<inertia ixx="0.00026046" ixy="0" ixz="0" iyy="0.00026046" iyz="0" izz="0.00041226"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/right_rear_wheel.obj"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0225"/>
<geometry>
<cylinder length="0.045" radius="0.05"/>
</geometry>
</collision>
</link>
<joint name="right_rear_wheel_joint" type="fixed">
<origin rpy="1.5708 0 0" xyz="0 -0.1 0"/>
<parent link="diff_sideB"/>
<child link="right_rear_wheel"/>
</joint>
<!-- Add the left steering hinge with its joints and tranmissions -->
<link name="left_steering_hinge">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.100"/>
<inertia ixx="4E-06" ixy="0" ixz="0" iyy="4E-06" iyz="0" izz="4E-06"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/left_steering_hinge.STL"/>
</geometry>
<material name="grey"/>
</visual>
</link>
<joint name="left_steering_hinge_joint" type="revolute">
<origin rpy="0 1.5708 0" xyz="0.325 0.1 0"/>
<parent link="chassis"/>
<child link="left_steering_hinge"/>
<axis xyz="-1 0 0"/>
<limit effort="10" lower="-1.0" upper="1.0" velocity="100"/>
</joint>
<transmission name="left_steering_hinge_transmission" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="left_steering_hinge_joint">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="left_steering_hinge_motor">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!-- Add the right steering hinge with its joints and tranmissions -->
<link name="right_steering_hinge">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.100"/>
<inertia ixx="4E-06" ixy="0" ixz="0" iyy="4E-06" iyz="0" izz="4E-06"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/right_steering_hinge.STL"/>
</geometry>
<material name="grey"/>
</visual>
</link>
<joint name="right_steering_hinge_joint" type="continuous">
<origin rpy="0 1.5708 0" xyz="0.325 -0.1 0"/>
<parent link="chassis"/>
<child link="right_steering_hinge"/>
<axis xyz="-1 0 0"/>
<limit effort="10" lower="-1.0" upper="1.0" velocity="100"/>
</joint>
<transmission name="right_steering_hinge_transmission" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="right_steering_hinge_joint">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="right_steering_hinge_motor">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!-- Add the left front wheel with its joints and tranmissions -->
<link name="left_front_wheel">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 -0.0225"/>
<mass value="0.034055"/>
<inertia ixx="0.00026046" ixy="0" ixz="0" iyy="0.00026046" iyz="0" izz="0.00041226"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/left_front_wheel.obj"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.0225"/>
<geometry>
<cylinder length="0.045" radius="0.05"/>
</geometry>
</collision>
</link>
<joint name="left_front_wheel_joint" type="continuous">
<origin rpy="1.5708 0 0" xyz="0 0 0"/>
<parent link="left_steering_hinge"/>
<child link="left_front_wheel"/>
<axis xyz="0 0 -1"/>
<limit effort="10" velocity="100"/>
</joint>
<transmission name="left_front_wheel_transmission" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="left_front_wheel_joint">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="left_front_wheel_motor">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!-- Add the left front wheel with its joints and tranmissions -->
<link name="right_front_wheel">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.0225"/>
<mass value="0.034055"/>
<inertia ixx="0.00026046" ixy="0" ixz="0" iyy="0.00026046" iyz="0" izz="0.00041226"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/right_front_wheel.obj"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0225"/>
<geometry>
<cylinder length="0.045" radius="0.05"/>
</geometry>
</collision>
</link>
<joint name="right_front_wheel_joint" type="continuous">
<origin rpy="1.5708 0 0" xyz="0 0 0"/>
<parent link="right_steering_hinge"/>
<child link="right_front_wheel"/>
<axis xyz="0 0 -1"/>
<limit effort="10" velocity="100"/>
</joint>
<transmission name="right_front_wheel_transmission" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="right_front_wheel_joint">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="right_front_wheel_motor">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!-- Add Hokuyo laser scanner -->
<link name="laser">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.130"/>
<inertia ixx="4E-06" ixy="0" ixz="0" iyy="4E-06" iyz="0" izz="4E-06"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/hokuyo.obj"/>
<material name="grey"/>
</geometry>
</visual>
</link>
<joint name="hokuyo_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.265 0.0 0.075"/>
<parent link="chassis"/>
<child link="laser"/>
<axis xyz="0 0 1"/>
</joint>
<!-- zed camera -->
<link name="zed_camera_link">
<inertial>
<mass value="0.1"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.033 0.175 0.030"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.033 0.175 0.030"/>
</geometry>
<material name="red"/>
</visual>
</link>
<joint name="zed_camera_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.390 0 0.04"/>
<parent link="chassis"/>
<child link="zed_camera_link"/>
<axis xyz="0 0 1"/>
<!-- <limit lower="0.0" upper="0.0" effort="0.0" velocity="0.0" /> -->
</joint>
<!-- zed camera lenses -->
<!-- It seems these have to have a non-zero mass to have a camera attached? -->
<link name="camera_link">
<!-- this needs to match the name in zed_wrapper/zed_tf.launch -->
<inertial>
<mass value="0.01"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
</link>
<link name="zed_camera_right_link">
<inertial>
<mass value="0.01"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
</link>
<joint name="zed_camera_left_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0.06 0"/>
<parent link="zed_camera_link"/>
<child link="camera_link"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="zed_camera_right_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.06 0"/>
<parent link="zed_camera_link"/>
<child link="zed_camera_right_link"/>
<axis xyz="0 0 1"/>
</joint>
<!-- Add the remaining xacros -->
<link name="diff_ring">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="0.0637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.0005 0.0005 0.001" filename="differential/diff_ring.stl"/>
</geometry>
<material name="green"/>
</visual>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_carrier.stl"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.025 0"/>
<geometry>
<cylinder length="0.01" radius="0.025"/>
</geometry>
</collision>
</link>
<joint name="diff_ring_chassis" type="continuous">
<parent link="chassis"/>
<child link="diff_ring"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0."/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff_spiderA">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="0.056"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_spider.stl"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff_spiderA_ring" type="continuous">
<parent link="diff_ring"/>
<child link="diff_spiderA"/>
<origin rpy="0 0 1.570795" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff_spiderB">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="0.056"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_spider.stl"/>
</geometry>
<material name="brown"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff_spiderB_ring" type="continuous">
<parent link="diff_ring"/>
<child link="diff_spiderB"/>
<origin rpy="0 0 -1.570795" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff_sideA">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="0.056"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_side.stl"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff_sideA_ring" type="continuous">
<parent link="diff_ring"/>
<child link="diff_sideA"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff_sideB">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="0.056"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="-1.570795 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_side.stl"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="-1.570795 0 0" xyz="0 -0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff_sideB_ring" type="continuous">
<parent link="diff_ring"/>
<child link="diff_sideB"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff2_ring">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="0.5637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.0005 0.0005 0.001" filename="differential/diff_ring.stl"/>
</geometry>
<material name="green"/>
</visual>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_carrier.stl"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.025 0"/>
<geometry>
<cylinder length="0.01" radius="0.025"/>
</geometry>
</collision>
</link>
<joint name="diff2_ring_chassis" type="continuous">
<parent link="chassis"/>
<child link="diff2_ring"/>
<origin rpy="0 0 0" xyz="0.32 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff2_spiderA">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="0.556"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_spider.stl"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff2_spiderA_ring" type="continuous">
<parent link="diff2_ring"/>
<child link="diff2_spiderA"/>
<origin rpy="0 0 1.570795" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff2_spiderB">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="0.556"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_spider.stl"/>
</geometry>
<material name="brown"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff2_spiderB_ring" type="continuous">
<parent link="diff2_ring"/>
<child link="diff2_spiderB"/>
<origin rpy="0 0 -1.570795" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff2_sideA">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="0.556"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_side.stl"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff2_sideA_ring" type="continuous">
<parent link="diff2_ring"/>
<child link="diff2_sideA"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff2_sideB">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="0.556"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="-1.570795 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_side.stl"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="-1.570795 0 0" xyz="0 -0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff2_sideB_ring" type="continuous">
<parent link="diff2_ring"/>
<child link="diff2_sideB"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<material name="white">
<color rgba="1. 1. 1. 1.0"/>
</material>
<material name="black">
<color rgba="0. 0. 0. 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="orange">
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
</material>
<material name="brown">
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</robot>

4
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -1296,8 +1296,10 @@ void OpenGLExampleBrowser::update(float deltaTime)
float pitch = s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraPitch();
float yaw = s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraYaw();
float camTarget[3];
float camPos[3];
s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraPosition(camPos);
s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraTargetPosition(camTarget);
sprintf(msg,"dist=%f, pitch=%f, yaw=%f,target=%f,%f,%f", camDist,pitch,yaw,camTarget[0],camTarget[1],camTarget[2]);
sprintf(msg,"camPos=%f,%f,%f, dist=%f, pitch=%f, yaw=%f,target=%f,%f,%f", camPos[0],camPos[1],camPos[2],camDist,pitch,yaw,camTarget[0],camTarget[1],camTarget[2]);
gui2->setStatusBarMessage(msg, true);
}

30
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -3181,24 +3181,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_requestPixelDataArguments.m_projectionMatrix);
} else
{
SharedMemoryStatus tmpCmd = serverStatusOut;
b3OpenGLVisualizerCameraInfo tmpCamResult;
bool result = this->m_data->m_guiHelper->getCameraInfo(
&tmpCmd.m_visualizerCameraResultArgs.m_width,
&tmpCmd.m_visualizerCameraResultArgs.m_height,
tmpCmd.m_visualizerCameraResultArgs.m_viewMatrix,
tmpCmd.m_visualizerCameraResultArgs.m_projectionMatrix,
tmpCmd.m_visualizerCameraResultArgs.m_camUp,
tmpCmd.m_visualizerCameraResultArgs.m_camForward,
tmpCmd.m_visualizerCameraResultArgs.m_horizontal,
tmpCmd.m_visualizerCameraResultArgs.m_vertical,
&tmpCmd.m_visualizerCameraResultArgs.m_yaw,
&tmpCmd.m_visualizerCameraResultArgs.m_pitch,
&tmpCmd.m_visualizerCameraResultArgs.m_dist,
tmpCmd.m_visualizerCameraResultArgs.m_target);
&tmpCamResult.m_width,
&tmpCamResult.m_height,
tmpCamResult.m_viewMatrix,
tmpCamResult.m_projectionMatrix,
tmpCamResult.m_camUp,
tmpCamResult.m_camForward,
tmpCamResult.m_horizontal,
tmpCamResult.m_vertical,
&tmpCamResult.m_yaw,
&tmpCamResult.m_pitch,
&tmpCamResult.m_dist,
tmpCamResult.m_target);
if (result)
{
m_data->m_visualConverter.render(tmpCmd.m_visualizerCameraResultArgs.m_viewMatrix,
tmpCmd.m_visualizerCameraResultArgs.m_projectionMatrix);
m_data->m_visualConverter.render(tmpCamResult.m_viewMatrix,
tmpCamResult.m_projectionMatrix);
} else
{
m_data->m_visualConverter.render();

22
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -686,17 +686,19 @@ void TinyRendererVisualShapeConverter::changeRGBAColor(int bodyUniqueId, int lin
break;
}
}
TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(start);
TinyRendererObjectArray* visualArray = *visualArrayPtr;
btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(start);
const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
float rgba[4] = {rgbaColor[0], rgbaColor[1], rgbaColor[2], rgbaColor[3]};
for (int v=0;v<visualArray->m_renderObjects.size();v++)
if (start>=0)
{
visualArray->m_renderObjects[v]->m_model->setColorRGBA(rgba);
TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(start);
TinyRendererObjectArray* visualArray = *visualArrayPtr;
btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(start);
const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
float rgba[4] = {rgbaColor[0], rgbaColor[1], rgbaColor[2], rgbaColor[3]};
for (int v=0;v<visualArray->m_renderObjects.size();v++)
{
visualArray->m_renderObjects[v]->m_model->setColorRGBA(rgba);
}
}
}

11
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -124,7 +124,7 @@ struct Shader : public IShader {
m_projectionLightViewMat = m_projectionMat*m_lightModelView;
}
virtual Vec4f vertex(int iface, int nthvert) {
B3_PROFILE("vertex");
//B3_PROFILE("vertex");
Vec2f uv = m_model->uv(iface, nthvert);
varying_uv.set_col(nthvert, uv);
varying_nrm.set_col(nthvert, proj<3>(m_invModelMat*embed<4>(m_model->normal(iface, nthvert), 0.f)));
@@ -142,7 +142,7 @@ struct Shader : public IShader {
}
virtual bool fragment(Vec3f bar, TGAColor &color) {
B3_PROFILE("fragment");
//B3_PROFILE("fragment");
Vec4f p = m_viewportMat*(varying_tri_light_view*bar);
float depth = p[2];
p = p/p[3];
@@ -485,6 +485,7 @@ static bool clipTriangleAgainstNearplane(const mat<4,3,float>& triangleIn, b3Ali
void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
{
B3_PROFILE("renderObject");
int width = renderData.m_rgbColorBuffer.get_width();
int height = renderData.m_rgbColorBuffer.get_height();
@@ -517,10 +518,11 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
Shader shader(model, light_dir_local, light_color, modelViewMatrix, lightModelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, renderData.m_viewportMatrix, localScaling, model->getColorRGBA(), width, height, shadowBufferPtr, renderData.m_lightAmbientCoeff, renderData.m_lightDiffuseCoeff, renderData.m_lightSpecularCoeff);
{
B3_PROFILE("face");
for (int i=0; i<model->nfaces(); i++)
{
B3_PROFILE("face");
for (int j=0; j<3; j++) {
shader.vertex(i, j);
}
@@ -552,6 +554,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
}
}
}
}
}

71
examples/pybullet/examples/racecar_differential.py Normal file
View File

@@ -0,0 +1,71 @@
import pybullet as p
import time
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0,0,-10)
p.setPhysicsEngineParameter(numSolverIterations=1000)
useRealTimeSim = 1
#for video recording (works best on Mac and Linux, not well on Windows)
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
p.setRealTimeSimulation(useRealTimeSim) # either this
#p.loadURDF("plane.urdf")
p.loadSDF("stadium.sdf")
car = p.loadURDF("racecar/racecar_differential.urdf")#, [0,0,2],useFixedBase=True)
for i in range (p.getNumJoints(car)):
print (p.getJointInfo(car,i))
for wheel in range(p.getNumJoints(car)):
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.getJointInfo(car,wheel)
wheels = [8,15]
print("----------------")
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
steering = [0,2]
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-50,50,0)
maxForceSlider = p.addUserDebugParameter("maxForce",0,50,20)
steeringSlider = p.addUserDebugParameter("steering",-1,1,0)
while (True):
maxForce = p.readUserDebugParameter(maxForceSlider)
targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = p.readUserDebugParameter(steeringSlider)
#print(targetVelocity)
for wheel in wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=maxForce)
for steer in steering:
p.setJointMotorControl2(car,steer,p.POSITION_CONTROL,targetPosition=-steeringAngle)
steering
if (useRealTimeSim==0):
p.stepSimulation()
time.sleep(0.01)

7
examples/pybullet/gym/envs/__init__.py
View File

@@ -42,4 +42,11 @@ register(
entry_point='envs.bullet:KukaGymEnv',
timestep_limit=1000,
reward_threshold=5.0,
)
register(
id='KukaCamBulletEnv-v0',
entry_point='envs.bullet:KukaCamGymEnv',
timestep_limit=1000,
reward_threshold=5.0,
)

1
examples/pybullet/gym/envs/bullet/__init__.py
View File

@@ -4,3 +4,4 @@ from envs.bullet.racecarGymEnv import RacecarGymEnv
from envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv
from envs.bullet.humanoidGymEnv import HumanoidGymEnv
from envs.bullet.kukaGymEnv import KukaGymEnv
from envs.bullet.kukaCamGymEnv import KukaCamGymEnv

3
examples/pybullet/gym/envs/bullet/kuka.py
View File

@@ -72,9 +72,10 @@ class Kuka:
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
pos = state[0]
orn = state[1]
euler = p.getEulerFromQuaternion(orn)
observation.extend(list(pos))
observation.extend(list(orn))
observation.extend(list(euler))
return observation

191
examples/pybullet/gym/envs/bullet/kukaCamGymEnv.py Normal file
View File

@@ -0,0 +1,191 @@
import math
import gym
from gym import spaces
from gym.utils import seeding
import numpy as np
import time
import pybullet as p
from . import kuka
import random
class KukaCamGymEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second' : 50
}
def __init__(self,
urdfRoot="",
actionRepeat=1,
isEnableSelfCollision=True,
renders=True):
print("init")
self._timeStep = 1./240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._width = 341
self._height = 256
self.terminated = 0
self._p = p
if self._renders:
p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
else:
p.connect(p.DIRECT)
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
self._seed()
self.reset()
observationDim = len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
self.action_space = spaces.Discrete(7)
self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4))
self.viewer = None
def _reset(self):
print("reset")
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1])
p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
xpos = 0.5 +0.05*random.random()
ypos = 0 +0.05*random.random()
ang = 3.1415925438*random.random()
orn = p.getQuaternionFromEuler([0,0,ang])
self.blockUid =p.loadURDF("block.urdf", xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3])
p.setGravity(0,0,-10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def __del__(self):
p.disconnect()
def _seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def getExtendedObservation(self):
#camEyePos = [0.03,0.236,0.54]
#distance = 1.06
#pitch=-56
#yaw = 258
#roll=0
#upAxisIndex = 2
#camInfo = p.getDebugVisualizerCamera()
#print("width,height")
#print(camInfo[0])
#print(camInfo[1])
#print("viewMatrix")
#print(camInfo[2])
#print("projectionMatrix")
#print(camInfo[3])
#viewMat = camInfo[2]
#viewMat = p.computeViewMatrixFromYawPitchRoll(camEyePos,distance,yaw, pitch,roll,upAxisIndex)
viewMat = [-0.5120397806167603, 0.7171027660369873, -0.47284144163131714, 0.0, -0.8589617609977722, -0.42747554183006287, 0.28186774253845215, 0.0, 0.0, 0.5504802465438843, 0.8348482847213745, 0.0, 0.1925382763147354, -0.24935829639434814, -0.4401884973049164, 1.0]
#projMatrix = camInfo[3]#[0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
projMatrix = [0.75, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
img_arr = p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix)
rgb=img_arr[2]
np_img_arr = np.reshape(rgb, (self._height, self._width, 4))
self._observation = np_img_arr
return self._observation
def _step(self, action):
dv = 0.01
dx = [0,-dv,dv,0,0,0,0][action]
dy = [0,0,0,-dv,dv,0,0][action]
da = [0,0,0,0,0,-0.1,0.1][action]
f = 0.3
realAction = [dx,dy,-0.002,da,f]
return self.step2( realAction)
def step2(self, action):
self._kuka.applyAction(action)
for i in range(self._actionRepeat):
p.stepSimulation()
if self._renders:
time.sleep(self._timeStep)
self._observation = self.getExtendedObservation()
if self._termination():
break
self._envStepCounter += 1
#print("self._envStepCounter")
#print(self._envStepCounter)
done = self._termination()
reward = self._reward()
#print("len=%r" % len(self._observation))
return np.array(self._observation), reward, done, {}
def _render(self, mode='human', close=False):
return
def _termination(self):
#print (self._kuka.endEffectorPos[2])
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("self._envStepCounter")
#print(self._envStepCounter)
if (self.terminated or self._envStepCounter>1000):
self._observation = self.getExtendedObservation()
return True
if (actualEndEffectorPos[2] <= 0.10):
self.terminated = 1
#print("closing gripper, attempting grasp")
#start grasp and terminate
fingerAngle = 0.3
for i in range (1000):
graspAction = [0,0,0.001,0,fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
fingerAngle = fingerAngle-(0.3/100.)
if (fingerAngle<0):
fingerAngle=0
self._observation = self.getExtendedObservation()
return True
return False
def _reward(self):
#rewards is height of target object
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000)
reward = -1000
numPt = len(closestPoints)
#print(numPt)
if (numPt>0):
#print("reward:")
reward = -closestPoints[0][8]*10
if (blockPos[2] >0.2):
print("grasped a block!!!")
print("self._envStepCounter")
print(self._envStepCounter)
reward = reward+1000
#print("reward")
#print(reward)
return reward

21
examples/pybullet/gym/envs/bullet/kukaGymEnv.py
View File

@@ -31,6 +31,7 @@ class KukaGymEnv(gym.Env):
self._p = p
if self._renders:
p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
else:
p.connect(p.DIRECT)
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
@@ -52,8 +53,7 @@ class KukaGymEnv(gym.Env):
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1])
if self._renders:
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
xpos = 0.5 +0.05*random.random()
@@ -78,14 +78,21 @@ class KukaGymEnv(gym.Env):
def getExtendedObservation(self):
self._observation = self._kuka.getObservation()
pos,orn = p.getBasePositionAndOrientation(self.blockUid)
self._observation.extend(list(pos))
self._observation.extend(list(orn))
eeState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
endEffectorPos = eeState[0]
endEffectorOrn = eeState[1]
blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)
invEEPos,invEEOrn = p.invertTransform(endEffectorPos,endEffectorOrn)
blockPosInEE,blockOrnInEE = p.multiplyTransforms(invEEPos,invEEOrn,blockPos,blockOrn)
blockEulerInEE = p.getEulerFromQuaternion(blockOrnInEE)
self._observation.extend(list(blockPosInEE))
self._observation.extend(list(blockEulerInEE))
return self._observation
def _step(self, action):
dv = 0.002
dv = 0.01
dx = [0,-dv,dv,0,0,0,0][action]
dy = [0,0,0,-dv,dv,0,0][action]
da = [0,0,0,0,0,-0.1,0.1][action]

44
examples/pybullet/gym/train_kuka_cam_grasping.py Normal file
View File

@@ -0,0 +1,44 @@
import gym
from envs.bullet.kukaCamGymEnv import KukaCamGymEnv
from baselines import deepq
import datetime
def callback(lcl, glb):
# stop training if reward exceeds 199
total = sum(lcl['episode_rewards'][-101:-1]) / 100
totalt = lcl['t']
#print("totalt")
#print(totalt)
is_solved = totalt > 2000 and total >= 10
return is_solved
def main():
env = KukaCamGymEnv(renders=True)
model = deepq.models.cnn_to_mlp(
convs=[(32, 8, 4), (64, 4, 2), (64, 3, 1)],
hiddens=[256],
dueling=False
)
act = deepq.learn(
env,
q_func=model,
lr=1e-3,
max_timesteps=10000000,
buffer_size=50000,
exploration_fraction=0.1,
exploration_final_eps=0.02,
print_freq=10,
callback=callback
)
print("Saving model to kuka_cam_model.pkl")
act.save("kuka_cam_model.pkl")
if __name__ == '__main__':
main()

33
examples/pybullet/gym/train_pybullet_cartpole.py Normal file
View File

@@ -0,0 +1,33 @@
import gym
from envs.bullet.cartpole_bullet import CartPoleBulletEnv
from baselines import deepq
def callback(lcl, glb):
# stop training if reward exceeds 199
is_solved = lcl['t'] > 100 and sum(lcl['episode_rewards'][-101:-1]) / 100 >= 199
return is_solved
def main():
env = gym.make('CartPoleBulletEnv-v0')
model = deepq.models.mlp([64])
act = deepq.learn(
env,
q_func=model,
lr=1e-3,
max_timesteps=100000,
buffer_size=50000,
exploration_fraction=0.1,
exploration_final_eps=0.02,
print_freq=10,
callback=callback
)
print("Saving model to cartpole_model.pkl")
act.save("cartpole_model.pkl")
if __name__ == '__main__':
main()