reduce stack usage (cause some crashes in low-stack tests)
fix crashing bug in changeVisualShape add differential gear version of racecar (only 2 back wheels are powered)
This commit is contained in:
Erwin Coumans
BIN
data/racecar/meshes/chassis_differential.STL
Normal file
BIN
data/racecar/meshes/chassis_differential.STL
Normal file
Binary file not shown.
743
data/racecar/racecar_differential.urdf
Normal file
743
data/racecar/racecar_differential.urdf
Normal 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>
|
||||||
|
|
||||||
@@ -3181,24 +3181,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
clientCmd.m_requestPixelDataArguments.m_projectionMatrix);
|
clientCmd.m_requestPixelDataArguments.m_projectionMatrix);
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
SharedMemoryStatus tmpCmd = serverStatusOut;
|
b3OpenGLVisualizerCameraInfo tmpCamResult;
|
||||||
bool result = this->m_data->m_guiHelper->getCameraInfo(
|
bool result = this->m_data->m_guiHelper->getCameraInfo(
|
||||||
&tmpCmd.m_visualizerCameraResultArgs.m_width,
|
&tmpCamResult.m_width,
|
||||||
&tmpCmd.m_visualizerCameraResultArgs.m_height,
|
&tmpCamResult.m_height,
|
||||||
tmpCmd.m_visualizerCameraResultArgs.m_viewMatrix,
|
tmpCamResult.m_viewMatrix,
|
||||||
tmpCmd.m_visualizerCameraResultArgs.m_projectionMatrix,
|
tmpCamResult.m_projectionMatrix,
|
||||||
tmpCmd.m_visualizerCameraResultArgs.m_camUp,
|
tmpCamResult.m_camUp,
|
||||||
tmpCmd.m_visualizerCameraResultArgs.m_camForward,
|
tmpCamResult.m_camForward,
|
||||||
tmpCmd.m_visualizerCameraResultArgs.m_horizontal,
|
tmpCamResult.m_horizontal,
|
||||||
tmpCmd.m_visualizerCameraResultArgs.m_vertical,
|
tmpCamResult.m_vertical,
|
||||||
&tmpCmd.m_visualizerCameraResultArgs.m_yaw,
|
&tmpCamResult.m_yaw,
|
||||||
&tmpCmd.m_visualizerCameraResultArgs.m_pitch,
|
&tmpCamResult.m_pitch,
|
||||||
&tmpCmd.m_visualizerCameraResultArgs.m_dist,
|
&tmpCamResult.m_dist,
|
||||||
tmpCmd.m_visualizerCameraResultArgs.m_target);
|
tmpCamResult.m_target);
|
||||||
if (result)
|
if (result)
|
||||||
{
|
{
|
||||||
m_data->m_visualConverter.render(tmpCmd.m_visualizerCameraResultArgs.m_viewMatrix,
|
m_data->m_visualConverter.render(tmpCamResult.m_viewMatrix,
|
||||||
tmpCmd.m_visualizerCameraResultArgs.m_projectionMatrix);
|
tmpCamResult.m_projectionMatrix);
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
m_data->m_visualConverter.render();
|
m_data->m_visualConverter.render();
|
||||||
|
|||||||
@@ -686,7 +686,8 @@ void TinyRendererVisualShapeConverter::changeRGBAColor(int bodyUniqueId, int lin
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
if (start>=0)
|
||||||
|
{
|
||||||
TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(start);
|
TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(start);
|
||||||
TinyRendererObjectArray* visualArray = *visualArrayPtr;
|
TinyRendererObjectArray* visualArray = *visualArrayPtr;
|
||||||
|
|
||||||
@@ -698,6 +699,7 @@ void TinyRendererVisualShapeConverter::changeRGBAColor(int bodyUniqueId, int lin
|
|||||||
{
|
{
|
||||||
visualArray->m_renderObjects[v]->m_model->setColorRGBA(rgba);
|
visualArray->m_renderObjects[v]->m_model->setColorRGBA(rgba);
|
||||||
}
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void TinyRendererVisualShapeConverter::setUpAxis(int axis)
|
void TinyRendererVisualShapeConverter::setUpAxis(int axis)
|
||||||
|
|||||||
71
examples/pybullet/examples/racecar_differential.py
Normal file
71
examples/pybullet/examples/racecar_differential.py
Normal 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)
|
||||||
@@ -72,9 +72,10 @@ class Kuka:
|
|||||||
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
|
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
|
||||||
pos = state[0]
|
pos = state[0]
|
||||||
orn = state[1]
|
orn = state[1]
|
||||||
|
euler = p.getEulerFromQuaternion(orn)
|
||||||
|
|
||||||
observation.extend(list(pos))
|
observation.extend(list(pos))
|
||||||
observation.extend(list(orn))
|
observation.extend(list(euler))
|
||||||
|
|
||||||
return observation
|
return observation
|
||||||
|
|
||||||
|
|||||||
@@ -31,6 +31,7 @@ class KukaGymEnv(gym.Env):
|
|||||||
self._p = p
|
self._p = p
|
||||||
if self._renders:
|
if self._renders:
|
||||||
p.connect(p.GUI)
|
p.connect(p.GUI)
|
||||||
|
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
|
||||||
else:
|
else:
|
||||||
p.connect(p.DIRECT)
|
p.connect(p.DIRECT)
|
||||||
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
|
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
|
||||||
@@ -52,8 +53,7 @@ class KukaGymEnv(gym.Env):
|
|||||||
p.setPhysicsEngineParameter(numSolverIterations=150)
|
p.setPhysicsEngineParameter(numSolverIterations=150)
|
||||||
p.setTimeStep(self._timeStep)
|
p.setTimeStep(self._timeStep)
|
||||||
p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1])
|
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)
|
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()
|
xpos = 0.5 +0.05*random.random()
|
||||||
@@ -78,14 +78,21 @@ class KukaGymEnv(gym.Env):
|
|||||||
|
|
||||||
def getExtendedObservation(self):
|
def getExtendedObservation(self):
|
||||||
self._observation = self._kuka.getObservation()
|
self._observation = self._kuka.getObservation()
|
||||||
pos,orn = p.getBasePositionAndOrientation(self.blockUid)
|
eeState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
|
||||||
self._observation.extend(list(pos))
|
endEffectorPos = eeState[0]
|
||||||
self._observation.extend(list(orn))
|
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
|
return self._observation
|
||||||
|
|
||||||
def _step(self, action):
|
def _step(self, action):
|
||||||
dv = 0.002
|
dv = 0.01
|
||||||
dx = [0,-dv,dv,0,0,0,0][action]
|
dx = [0,-dv,dv,0,0,0,0][action]
|
||||||
dy = [0,0,0,-dv,dv,0,0][action]
|
dy = [0,0,0,-dv,dv,0,0][action]
|
||||||
da = [0,0,0,0,0,-0.1,0.1][action]
|
da = [0,0,0,0,0,-0.1,0.1][action]
|
||||||
|
|||||||
Reference in New Issue
Block a user