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

Merge pull request #1023 from erwincoumans/master

Browse Source 
update minitaur.py and RobotSimulator to use data/quadruped/minitaur.urdf
This commit is contained in:
erwincoumans
2017-03-15 21:43:56 -07:00
committed by GitHub
parent 6f171f90dc e0a4393bd2
commit 8ce4444638
17 changed files with 2495 additions and 524 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
data/MPL/MPL.xml
View File

@@ -49,30 +49,30 @@
</visual> </visual>
<asset> <asset>
<mesh name="index0" file="index0_collision.stl"/> <mesh name="index0" file="index0_collision.STL"/>
<mesh name="index1" file="index1_collision.stl"/> <mesh name="index1" file="index1_collision.STL"/>
<mesh name="index2" file="index2_collision.stl"/> <mesh name="index2" file="index2_collision.STL"/>
<mesh name="index3" file="index3_collision.stl"/> <mesh name="index3" file="index3_collision.STL"/>
<mesh name="middle0" file="middle0_collision.stl"/> <mesh name="middle0" file="middle0_collision.STL"/>
<mesh name="middle1" file="middle1_collision.stl"/> <mesh name="middle1" file="middle1_collision.STL"/>
<mesh name="middle2" file="middle2_collision.stl"/> <mesh name="middle2" file="middle2_collision.STL"/>
<mesh name="middle3" file="middle3_collision.stl"/> <mesh name="middle3" file="middle3_collision.STL"/>
<mesh name="palm" file="palm.stl"/> <mesh name="palm" file="palm.STL"/>
<mesh name="pinky0" file="pinky0_collision.stl"/> <mesh name="pinky0" file="pinky0_collision.STL"/>
<mesh name="pinky1" file="pinky1_collision.stl"/> <mesh name="pinky1" file="pinky1_collision.STL"/>
<mesh name="pinky2" file="pinky2_collision.stl"/> <mesh name="pinky2" file="pinky2_collision.STL"/>
<mesh name="pinky3" file="pinky3_collision.stl"/> <mesh name="pinky3" file="pinky3_collision.STL"/>
<mesh name="ring0" file="ring0_collision.stl"/> <mesh name="ring0" file="ring0_collision.STL"/>
<mesh name="ring1" file="ring1_collision.stl"/> <mesh name="ring1" file="ring1_collision.STL"/>
<mesh name="ring2" file="ring2_collision.stl"/> <mesh name="ring2" file="ring2_collision.STL"/>
<mesh name="ring3" file="ring3_collision.stl"/> <mesh name="ring3" file="ring3_collision.STL"/>
<mesh name="thumb0" file="thumb0_collision.stl"/> <mesh name="thumb0" file="thumb0_collision.STL"/>
<mesh name="thumb1" file="thumb1_collision.stl"/> <mesh name="thumb1" file="thumb1_collision.STL"/>
<mesh name="thumb2" file="thumb2_collision.stl"/> <mesh name="thumb2" file="thumb2_collision.STL"/>
<mesh name="thumb3" file="thumb3_collision.stl"/> <mesh name="thumb3" file="thumb3_collision.STL"/>
<mesh name="wristx" file="wristx_collision.stl"/> <mesh name="wristx" file="wristx_collision.STL"/>
<mesh name="wristy" file="wristy_collision.stl"/> <mesh name="wristy" file="wristy_collision.STL"/>
<mesh name="wristz" file="wristz_collision.stl"/> <mesh name="wristz" file="wristz_collision.STL"/>
<texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0" <texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
width="100" height="100"/> width="100" height="100"/>

34
data/quadruped/minitaur.urdf
View File

@@ -427,7 +427,9 @@
<link name="lower_leg_front_rightR_link"> <link name="lower_leg_front_rightR_link">
<contact> <contact>
<lateral_friction value="1"/> <stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact> </contact>
<visual> <visual>
@@ -493,7 +495,9 @@
<link name="lower_leg_front_rightL_link"> <link name="lower_leg_front_rightL_link">
<contact> <contact>
<lateral_friction value="1"/> <stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact> </contact>
<visual> <visual>
@@ -557,7 +561,9 @@
<link name="lower_leg_front_leftR_link"> <link name="lower_leg_front_leftR_link">
<contact> <contact>
<lateral_friction value="1"/> <stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact> </contact>
<visual> <visual>
@@ -622,7 +628,9 @@
<link name="lower_leg_front_leftL_link"> <link name="lower_leg_front_leftL_link">
<contact> <contact>
<lateral_friction value="1"/> <stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact> </contact>
<visual> <visual>
@@ -684,7 +692,9 @@
<link name="lower_leg_back_rightR_link"> <link name="lower_leg_back_rightR_link">
<contact> <contact>
<lateral_friction value="1"/> <stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact> </contact>
<visual> <visual>
@@ -750,9 +760,11 @@
<link name="lower_leg_back_rightL_link"> <link name="lower_leg_back_rightL_link">
<contact> <contact>
<lateral_friction value="1"/> <stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact> </contact>
<visual> <visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/> <origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry> <geometry>
@@ -814,7 +826,9 @@
<link name="lower_leg_back_leftR_link"> <link name="lower_leg_back_leftR_link">
<contact> <contact>
<lateral_friction value="1"/> <stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact> </contact>
<visual> <visual>
@@ -877,7 +891,9 @@
<link name="lower_leg_back_leftL_link"> <link name="lower_leg_back_leftL_link">
<contact> <contact>
<lateral_friction value="1"/> <stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact> </contact>
<visual> <visual>

913
data/quadruped/minitaur_fixed_all.urdf Normal file
View File

@@ -0,0 +1,913 @@
<?xml version="0.0" ?>
<!-- ======================================================================= -->
<!--LICENSE: -->
<!--Copyright (c) 2017, Erwin Coumans -->
<!--Google Inc. -->
<!--All rights reserved. -->
<!-- -->
<!--Redistribution and use in source and binary forms, with or without -->
<!--modification, are permitted provided that the following conditions are -->
<!--met: -->
<!-- -->
<!--1. Redistributions or derived work must retain this copyright notice, -->
<!-- this list of conditions and the following disclaimer. -->
<!-- -->
<!--2. Redistributions in binary form must reproduce the above copyright -->
<!-- notice, this list of conditions and the following disclaimer in the -->
<!-- documentation and/or other materials provided with the distribution. -->
<!-- -->
<!--THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS -->
<!--IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,-->
<!--THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -->
<!--PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -->
<!--CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, -->
<!--EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, -->
<!--PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR -->
<!--PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -->
<!--LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING -->
<!--NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -->
<!--SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -->
<robot name="quadruped">
<link name="base_chassis_link">
<visual>
<geometry>
<box size=".33 0.14 .07"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0 0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".34 0.14 .07"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
</collision>
<inertial>
<mass value="3"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="chassis_right">
<visual>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<collision>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<inertial>
<mass value=".1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="chassis_right_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_right"/>
<origin rpy="-0.0872665 0 0" xyz="0.0 -0.10 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="chassis_left">
<visual>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<collision>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<inertial>
<mass value=".1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="chassis_left_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_left"/>
<origin rpy="0.0872665 0 0" xyz="0.0 0.10 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_front_rightR_link"/>
<origin rpy="1.57075 -1.570796 0" xyz="0.21 -0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_front_rightL_link"/>
<origin rpy="1.57075 -1.570796 3.141592" xyz="0.21 0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_front_leftR_link"/>
<origin rpy="1.57075 1.570796 3.141592" xyz="0.21 0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_front_leftL_link"/>
<origin rpy="1.57075 1.570796 0" xyz="0.21 -0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_back_rightR_link"/>
<origin rpy="1.57075 -1.570796 0" xyz="-0.21 -0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_back_rightL_link"/>
<origin rpy="1.57075 -1.570796 3.141592" xyz="-0.21 0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_back_leftR_link"/>
<origin rpy="1.57075 1.570796 3.141592" xyz="-0.21 0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_back_leftL_link"/>
<origin rpy="1.57075 1.570796 0" xyz="-0.21 -0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_rightR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_front_rightR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightR_link"/>
<child link="upper_leg_front_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_rightR_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_rightR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightR_link"/>
<child link="lower_leg_front_rightR_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_rightL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightL_link"/>
<child link="upper_leg_front_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_rightL_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_rightL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightL_link"/>
<child link="lower_leg_front_rightL_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_leftR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_front_leftR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftR_link"/>
<child link="upper_leg_front_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_leftR_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_leftR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftR_link"/>
<child link="lower_leg_front_leftR_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_leftL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftL_link"/>
<child link="upper_leg_front_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_leftL_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_leftL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftL_link"/>
<child link="lower_leg_front_leftL_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_rightR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_rightR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightR_link"/>
<child link="upper_leg_back_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_rightR_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_rightR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightR_link"/>
<child link="lower_leg_back_rightR_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_rightL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightL_link"/>
<child link="upper_leg_back_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_rightL_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_rightL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightL_link"/>
<child link="lower_leg_back_rightL_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_leftR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_leftR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftR_link"/>
<child link="upper_leg_back_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_leftR_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_leftR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftR_link"/>
<child link="lower_leg_back_leftR_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_leftL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftL_link"/>
<child link="upper_leg_back_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_leftL_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_leftL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftL_link"/>
<child link="lower_leg_back_leftL_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
</robot>

929
data/quadruped/minitaur_fixed_knees.urdf Normal file
View File

@@ -0,0 +1,929 @@
<?xml version="0.0" ?>
<!-- ======================================================================= -->
<!--LICENSE: -->
<!--Copyright (c) 2017, Erwin Coumans -->
<!--Google Inc. -->
<!--All rights reserved. -->
<!-- -->
<!--Redistribution and use in source and binary forms, with or without -->
<!--modification, are permitted provided that the following conditions are -->
<!--met: -->
<!-- -->
<!--1. Redistributions or derived work must retain this copyright notice, -->
<!-- this list of conditions and the following disclaimer. -->
<!-- -->
<!--2. Redistributions in binary form must reproduce the above copyright -->
<!-- notice, this list of conditions and the following disclaimer in the -->
<!-- documentation and/or other materials provided with the distribution. -->
<!-- -->
<!--THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS -->
<!--IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,-->
<!--THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -->
<!--PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -->
<!--CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, -->
<!--EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, -->
<!--PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR -->
<!--PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -->
<!--LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING -->
<!--NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -->
<!--SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -->
<robot name="quadruped">
<link name="base_chassis_link">
<visual>
<geometry>
<box size=".33 0.14 .07"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0 0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".34 0.14 .07"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
</collision>
<inertial>
<mass value="3"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="chassis_right">
<visual>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<collision>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<inertial>
<mass value=".1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="chassis_right_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_right"/>
<origin rpy="-0.0872665 0 0" xyz="0.0 -0.10 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="chassis_left">
<visual>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<collision>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<inertial>
<mass value=".1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="chassis_left_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_left"/>
<origin rpy="0.0872665 0 0" xyz="0.0 0.10 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_front_rightR_link"/>
<origin rpy="1.57075 0 0" xyz="0.21 -0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_front_rightL_link"/>
<origin rpy="1.57075 0 3.141592" xyz="0.21 0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_front_leftR_link"/>
<origin rpy="1.57075 0 3.141592" xyz="0.21 0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_front_leftL_link"/>
<origin rpy="1.57075 0 0" xyz="0.21 -0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_back_rightR_link"/>
<origin rpy="1.57075 0 0" xyz="-0.21 -0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_back_rightL_link"/>
<origin rpy="1.57075 0 3.141592" xyz="-0.21 0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_back_leftR_link"/>
<origin rpy="1.57075 0 3.141592" xyz="-0.21 0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_back_leftL_link"/>
<origin rpy="1.57075 0 0" xyz="-0.21 -0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_rightR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_front_rightR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightR_link"/>
<child link="upper_leg_front_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_rightR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_rightR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightR_link"/>
<child link="lower_leg_front_rightR_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_rightL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightL_link"/>
<child link="upper_leg_front_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_rightL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_rightL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightL_link"/>
<child link="lower_leg_front_rightL_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_leftR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_front_leftR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftR_link"/>
<child link="upper_leg_front_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_leftR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_leftR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftR_link"/>
<child link="lower_leg_front_leftR_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_leftL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftL_link"/>
<child link="upper_leg_front_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_leftL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_leftL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftL_link"/>
<child link="lower_leg_front_leftL_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_rightR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_rightR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightR_link"/>
<child link="upper_leg_back_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_rightR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_rightR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightR_link"/>
<child link="lower_leg_back_rightR_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_rightL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightL_link"/>
<child link="upper_leg_back_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_rightL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_rightL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightL_link"/>
<child link="lower_leg_back_rightL_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_leftR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_leftR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftR_link"/>
<child link="upper_leg_back_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_leftR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_leftR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftR_link"/>
<child link="lower_leg_back_leftR_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_leftL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftL_link"/>
<child link="upper_leg_back_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_leftL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_leftL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftL_link"/>
<child link="lower_leg_back_leftL_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
</robot>

98
examples/RobotSimulator/MinitaurSetup.cpp
View File

@@ -49,57 +49,69 @@ void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim)
sim->setJointMotorControl(m_data->m_quadrupedUniqueId,i,controlArgs); sim->setJointMotorControl(m_data->m_quadrupedUniqueId,i,controlArgs);
} }
//right front leg b3Scalar startAngle = B3_HALF_PI;
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightR_joint"],1.57); b3Scalar upperLegLength = 11.5;
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],-2.2); b3Scalar lowerLegLength = 20;
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightL_joint"],-1.57); b3Scalar kneeAngle = B3_PI+b3Acos(upperLegLength/lowerLegLength);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],2.2);
b3Scalar motorDirs[8] = {-1,-1,-1,-1,1,1,1,1};
b3JointInfo jointInfo; b3JointInfo jointInfo;
jointInfo.m_jointType = ePoint2PointType; jointInfo.m_jointType = ePoint2PointType;
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.01; jointInfo.m_parentFrame[2] = 0.2; //left front leg
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = -0.015; jointInfo.m_childFrame[2] = 0.2; sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftL_joint"],motorDirs[0] * startAngle);
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"], sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],motorDirs[0]*kneeAngle);
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],&jointInfo); sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftR_joint"],motorDirs[1] * startAngle);
setDesiredMotorAngle(sim,"motor_front_rightR_joint",1.57); sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],motorDirs[1]*kneeAngle);
setDesiredMotorAngle(sim,"motor_front_rightL_joint",-1.57);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2;
//left front leg jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2;
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftR_joint"],1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],-2.2);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftL_joint"],-1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],2.2);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = -0.01; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.015; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"], sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],&jointInfo); m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_front_leftR_joint", 1.57); setDesiredMotorAngle(sim,"motor_front_leftL_joint", motorDirs[0] * startAngle);
setDesiredMotorAngle(sim,"motor_front_leftL_joint", -1.57); setDesiredMotorAngle(sim,"motor_front_leftR_joint", motorDirs[1] * startAngle);
//right back leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightR_joint"],1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],-2.2);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightL_joint"],-1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],2.2);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.01; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = -0.015; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_back_rightR_joint", 1.57);
setDesiredMotorAngle(sim,"motor_back_rightL_joint", -1.57);
//left back leg //left back leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftR_joint"],1.57); sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftL_joint"],motorDirs[2] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],-2.2); sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],motorDirs[2] * kneeAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftL_joint"],-1.57); sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftR_joint"],motorDirs[3] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],2.2); sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],motorDirs[3] * kneeAngle);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = -0.01; jointInfo.m_parentFrame[2] = 0.2; jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.015; jointInfo.m_childFrame[2] = 0.2; jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"], sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],&jointInfo); m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_back_leftR_joint", 1.57); setDesiredMotorAngle(sim,"motor_back_leftL_joint", motorDirs[2] * startAngle);
setDesiredMotorAngle(sim,"motor_back_leftL_joint", -1.57); setDesiredMotorAngle(sim,"motor_back_leftR_joint", motorDirs[3] * startAngle);
//right front leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightL_joint"],motorDirs[4] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],motorDirs[4] * kneeAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightR_joint"],motorDirs[5]*startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],motorDirs[5] * kneeAngle);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_front_rightL_joint",motorDirs[4] * startAngle);
setDesiredMotorAngle(sim,"motor_front_rightR_joint",motorDirs[5] * startAngle);
//right back leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightL_joint"],motorDirs[6] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],motorDirs[6] * kneeAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightR_joint"],motorDirs[7] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],motorDirs[7] * kneeAngle);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_back_rightL_joint", motorDirs[6] * startAngle);
setDesiredMotorAngle(sim,"motor_back_rightR_joint", motorDirs[7] * startAngle);
} }
int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn) int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn)
@@ -109,7 +121,7 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3V
args.m_startPosition = startPos; args.m_startPosition = startPos;
args.m_startOrientation = startOrn; args.m_startOrientation = startOrn;
m_data->m_quadrupedUniqueId = sim->loadURDF("quadruped/quadruped.urdf",args); m_data->m_quadrupedUniqueId = sim->loadURDF("quadruped/minitaur.urdf",args);
int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId); int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId);
for (int i=0;i<numJoints;i++) for (int i=0;i<numJoints;i++)

36
examples/RobotSimulator/RobotSimulatorMain.cpp
View File

@@ -19,14 +19,19 @@ int main(int argc, char* argv[])
//syncBodies is only needed when connecting to an existing physics server that has already some bodies //syncBodies is only needed when connecting to an existing physics server that has already some bodies
sim->syncBodies(); sim->syncBodies();
b3Scalar fixedTimeStep = 1./240.;
sim->setTimeStep(1./240.);
sim->setGravity(b3MakeVector3(0,0,-10)); sim->setTimeStep(fixedTimeStep);
int blockId = sim->loadURDF("cube.urdf"); b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3));
b3BodyInfo bodyInfo; b3Vector3 rpy;
sim->getBodyInfo(blockId,&bodyInfo); rpy = sim->getEulerFromQuaternion(q);
sim->setGravity(b3MakeVector3(0,0,-9.8));
//int blockId = sim->loadURDF("cube.urdf");
//b3BodyInfo bodyInfo;
//sim->getBodyInfo(blockId,&bodyInfo);
sim->loadURDF("plane.urdf"); sim->loadURDF("plane.urdf");
@@ -34,15 +39,15 @@ int main(int argc, char* argv[])
int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3)); int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3));
b3RobotSimulatorLoadUrdfFileArgs args; //b3RobotSimulatorLoadUrdfFileArgs args;
args.m_startPosition.setValue(2,0,1); //args.m_startPosition.setValue(2,0,1);
int r2d2 = sim->loadURDF("r2d2.urdf",args); //int r2d2 = sim->loadURDF("r2d2.urdf",args);
b3RobotSimulatorLoadFileResults sdfResults; //b3RobotSimulatorLoadFileResults sdfResults;
if (!sim->loadSDF("two_cubes.sdf",sdfResults)) //if (!sim->loadSDF("two_cubes.sdf",sdfResults))
{ //{
b3Warning("Can't load SDF!\n"); // b3Warning("Can't load SDF!\n");
} //}
b3Clock clock; b3Clock clock;
double startTime = clock.getTimeInSeconds(); double startTime = clock.getTimeInSeconds();
@@ -69,7 +74,8 @@ int main(int argc, char* argv[])
printf("keyEvent[%d].m_keyCode = %d, state = %d\n", i,keyEvents.m_keyboardEvents[i].m_keyCode,keyEvents.m_keyboardEvents[i].m_keyState); printf("keyEvent[%d].m_keyCode = %d, state = %d\n", i,keyEvents.m_keyboardEvents[i].m_keyCode,keyEvents.m_keyboardEvents[i].m_keyState);
} }
} }
b3Clock::usleep(1000*1000); sim->stepSimulation();
b3Clock::usleep(1000.*1000.*fixedTimeStep);
} }
printf("sim->disconnect\n"); printf("sim->disconnect\n");

522
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

@@ -1,17 +1,16 @@
#include "b3RobotSimulatorClientAPI.h" #include "b3RobotSimulatorClientAPI.h"
//#include "SharedMemoryCommands.h" //#include "SharedMemoryCommands.h"
#include "SharedMemory/PhysicsClientC_API.h" #include "SharedMemory/PhysicsClientC_API.h"
#ifdef BT_ENABLE_ENET #ifdef BT_ENABLE_ENET
#include "SharedMemory/PhysicsClientUDP_C_API.h" #include "SharedMemory/PhysicsClientUDP_C_API.h"
#endif//PHYSICS_UDP #endif //PHYSICS_UDP
#ifdef BT_ENABLE_CLSOCKET #ifdef BT_ENABLE_CLSOCKET
#include "SharedMemory/PhysicsClientTCP_C_API.h" #include "SharedMemory/PhysicsClientTCP_C_API.h"
#endif//PHYSICS_TCP #endif //PHYSICS_TCP
#include "SharedMemory/PhysicsDirectC_API.h" #include "SharedMemory/PhysicsDirectC_API.h"
@@ -25,7 +24,7 @@ struct b3RobotSimulatorClientAPI_InternalData
b3PhysicsClientHandle m_physicsClientHandle; b3PhysicsClientHandle m_physicsClientHandle;
b3RobotSimulatorClientAPI_InternalData() b3RobotSimulatorClientAPI_InternalData()
:m_physicsClientHandle(0) : m_physicsClientHandle(0)
{ {
} }
}; };
@@ -35,17 +34,16 @@ b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI()
m_data = new b3RobotSimulatorClientAPI_InternalData(); m_data = new b3RobotSimulatorClientAPI_InternalData();
} }
b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI() b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI()
{ {
delete m_data; delete m_data;
} }
bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey) bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey)
{ {
if (m_data->m_physicsClientHandle) if (m_data->m_physicsClientHandle)
{ {
b3Warning ("Already connected, disconnect first."); b3Warning("Already connected, disconnect first.");
return false; return false;
} }
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
@@ -54,89 +52,88 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
int tcpPort = 6667; int tcpPort = 6667;
int key = SHARED_MEMORY_KEY; int key = SHARED_MEMORY_KEY;
bool connected = false; bool connected = false;
switch (mode) switch (mode)
{ {
case eCONNECT_GUI: case eCONNECT_GUI:
{ {
int argc = 0; int argc = 0;
char* argv[1] = {0}; char* argv[1] = {0};
#ifdef __APPLE__ #ifdef __APPLE__
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
#else #else
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
#endif #endif
break; break;
} }
case eCONNECT_DIRECT: { case eCONNECT_DIRECT:
sm = b3ConnectPhysicsDirect(); {
break; sm = b3ConnectPhysicsDirect();
break;
} }
case eCONNECT_SHARED_MEMORY: { case eCONNECT_SHARED_MEMORY:
if (portOrKey>=0) {
if (portOrKey >= 0)
{ {
key = portOrKey; key = portOrKey;
} }
sm = b3ConnectSharedMemory(key); sm = b3ConnectSharedMemory(key);
break; break;
} }
case eCONNECT_UDP: case eCONNECT_UDP:
{ {
if (portOrKey>=0) if (portOrKey >= 0)
{ {
udpPort = portOrKey; udpPort = portOrKey;
} }
#ifdef BT_ENABLE_ENET #ifdef BT_ENABLE_ENET
sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort); sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort);
#else #else
b3Warning("UDP is not enabled in this build"); b3Warning("UDP is not enabled in this build");
#endif //BT_ENABLE_ENET #endif //BT_ENABLE_ENET
break; break;
} }
case eCONNECT_TCP: case eCONNECT_TCP:
{ {
if (portOrKey>=0) if (portOrKey >= 0)
{ {
tcpPort = portOrKey; tcpPort = portOrKey;
} }
#ifdef BT_ENABLE_CLSOCKET #ifdef BT_ENABLE_CLSOCKET
sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort); sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort);
#else #else
b3Warning("TCP is not enabled in this pybullet build"); b3Warning("TCP is not enabled in this pybullet build");
#endif //BT_ENABLE_CLSOCKET #endif //BT_ENABLE_CLSOCKET
break; break;
} }
default: { default:
b3Warning("connectPhysicsServer unexpected argument"); {
b3Warning("connectPhysicsServer unexpected argument");
} }
}; };
if (sm) if (sm)
{ {
m_data->m_physicsClientHandle = sm; m_data->m_physicsClientHandle = sm;
if (!b3CanSubmitCommand(m_data->m_physicsClientHandle)) if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
{ {
disconnect(); disconnect();
return false; return false;
} }
return true; return true;
} }
return false; return false;
} }
bool b3RobotSimulatorClientAPI::isConnected() const bool b3RobotSimulatorClientAPI::isConnected() const
{ {
return (m_data->m_physicsClientHandle!=0); return (m_data->m_physicsClientHandle != 0);
} }
void b3RobotSimulatorClientAPI::disconnect() void b3RobotSimulatorClientAPI::disconnect()
{ {
if (!isConnected()) if (!isConnected())
@@ -157,15 +154,13 @@ void b3RobotSimulatorClientAPI::syncBodies()
return; return;
} }
b3SharedMemoryCommandHandle command;
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle); command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
} }
void b3RobotSimulatorClientAPI::resetSimulation() void b3RobotSimulatorClientAPI::resetSimulation()
@@ -175,9 +170,9 @@ void b3RobotSimulatorClientAPI::resetSimulation()
b3Warning("Not connected"); b3Warning("Not connected");
return; return;
} }
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus( statusHandle = b3SubmitClientCommandAndWaitStatus(
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle)); m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
} }
bool b3RobotSimulatorClientAPI::canSubmitCommand() const bool b3RobotSimulatorClientAPI::canSubmitCommand() const
@@ -186,7 +181,7 @@ bool b3RobotSimulatorClientAPI::canSubmitCommand() const
{ {
return false; return false;
} }
return (b3CanSubmitCommand(m_data->m_physicsClientHandle)); return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0);
} }
void b3RobotSimulatorClientAPI::stepSimulation() void b3RobotSimulatorClientAPI::stepSimulation()
@@ -200,11 +195,11 @@ void b3RobotSimulatorClientAPI::stepSimulation()
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
{ {
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle)); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED); b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
} }
} }
void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration) void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
@@ -215,57 +210,26 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
return; return;
} }
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]); b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
} }
b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw) b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw)
{ {
double phi, the, psi; b3Quaternion q;
phi = rollPitchYaw[0] / 2.0; q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]);
the = rollPitchYaw[1] / 2.0;
psi = rollPitchYaw[2] / 2.0;
double quat[4] = {
sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)};
// normalize the quaternion
double len = sqrt(quat[0] * quat[0] + quat[1] * quat[1] +
quat[2] * quat[2] + quat[3] * quat[3]);
quat[0] /= len;
quat[1] /= len;
quat[2] /= len;
quat[3] /= len;
b3Quaternion q(quat[0],quat[1],quat[2],quat[3]);
return q; return q;
} }
b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat) b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat)
{ {
double squ; b3Scalar roll,pitch,yaw;
double sqx; quat.getEulerZYX(yaw,pitch,roll);
double sqy; b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw);
double sqz; return rpy2;
double sarg;
b3Vector3 rpy;
sqx = quat[0] * quat[0];
sqy = quat[1] * quat[1];
sqz = quat[2] * quat[2];
squ = quat[3] * quat[3];
rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]),
squ - sqx - sqy + sqz);
sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]);
rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538
: (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg));
rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]),
squ + sqx - sqy - sqz);
return rpy;
} }
int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args) int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
@@ -278,31 +242,26 @@ int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struc
return robotUniqueId; return robotUniqueId;
} }
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
//setting the initial position, orientation and other arguments are optional //setting the initial position, orientation and other arguments are optional
b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0], b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
args.m_startPosition[1], args.m_startPosition[1],
args.m_startPosition[2]); args.m_startPosition[2]);
b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0] b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
,args.m_startOrientation[1]
,args.m_startOrientation[2]
,args.m_startOrientation[3]);
if (args.m_forceOverrideFixedBase) if (args.m_forceOverrideFixedBase)
{ {
b3LoadUrdfCommandSetUseFixedBase(command,true); b3LoadUrdfCommandSetUseFixedBase(command, true);
} }
b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody); b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_URDF_LOADING_COMPLETED); b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
if (statusType==CMD_URDF_LOADING_COMPLETED) if (statusType == CMD_URDF_LOADING_COMPLETED)
{ {
robotUniqueId = b3GetStatusBodyIndex(statusHandle); robotUniqueId = b3GetStatusBodyIndex(statusHandle);
} }
@@ -329,12 +288,12 @@ bool b3RobotSimulatorClientAPI::loadMJCF(const std::string& fileName, b3RobotSim
b3Warning("Couldn't load .mjcf file."); b3Warning("Couldn't load .mjcf file.");
return false; return false;
} }
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
if (numBodies) if (numBodies)
{ {
results.m_uniqueObjectIds.resize(numBodies); results.m_uniqueObjectIds.resize(numBodies);
int numBodies; int numBodies;
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
} }
return true; return true;
@@ -359,12 +318,12 @@ bool b3RobotSimulatorClientAPI::loadBullet(const std::string& fileName, b3RobotS
{ {
return false; return false;
} }
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
if (numBodies) if (numBodies)
{ {
results.m_uniqueObjectIds.resize(numBodies); results.m_uniqueObjectIds.resize(numBodies);
int numBodies; int numBodies;
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
} }
return true; return true;
@@ -383,19 +342,18 @@ bool b3RobotSimulatorClientAPI::loadSDF(const std::string& fileName, b3RobotSimu
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody); b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
b3Assert(statusType == CMD_SDF_LOADING_COMPLETED); b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
if (statusType == CMD_SDF_LOADING_COMPLETED) if (statusType == CMD_SDF_LOADING_COMPLETED)
{ {
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
if (numBodies) if (numBodies)
{ {
results.m_uniqueObjectIds.resize(numBodies); results.m_uniqueObjectIds.resize(numBodies);
int numBodies; int numBodies;
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
} }
statusOk = true; statusOk = true;
} }
@@ -411,7 +369,7 @@ bool b3RobotSimulatorClientAPI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo*
return false; return false;
} }
int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo); int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
return (result != 0); return (result != 0);
} }
@@ -431,7 +389,8 @@ bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId,
const int status_type = b3GetStatusType(status_handle); const int status_type = b3GetStatusType(status_handle);
const double* actualStateQ; const double* actualStateQ;
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
return false; return false;
} }
@@ -466,14 +425,72 @@ bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId
b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]); b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]);
b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1], b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1],
baseOrientation[2], baseOrientation[3]); baseOrientation[2], baseOrientation[3]);
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
return true; return true;
} }
bool b3RobotSimulatorClientAPI::getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
const int status_type = b3GetStatusType(statusHandle);
const double* actualStateQdot;
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
return false;
}
b3GetStatusActualState(statusHandle, 0 /* body_unique_id */,
0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
0 /*root_local_inertial_frame*/, 0,
&actualStateQdot, 0 /* joint_reaction_forces */);
baseLinearVelocity[0] = actualStateQdot[0];
baseLinearVelocity[1] = actualStateQdot[1];
baseLinearVelocity[2] = actualStateQdot[2];
baseAngularVelocity[0] = actualStateQdot[3];
baseAngularVelocity[1] = actualStateQdot[4];
baseAngularVelocity[2] = actualStateQdot[5];
return true;
}
bool b3RobotSimulatorClientAPI::resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
b3Vector3DoubleData linVelDouble;
linearVelocity.serializeDouble(linVelDouble);
b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats);
b3Vector3DoubleData angVelDouble;
angularVelocity.serializeDouble(angVelDouble);
b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
return true;
}
void b3RobotSimulatorClientAPI::setRealTimeSimulation(bool enableRealTimeSimulation) void b3RobotSimulatorClientAPI::setRealTimeSimulation(bool enableRealTimeSimulation)
{ {
@@ -500,10 +517,9 @@ int b3RobotSimulatorClientAPI::getNumJoints(int bodyUniqueId) const
b3Warning("Not connected"); b3Warning("Not connected");
return false; return false;
} }
return b3GetNumJoints(m_data->m_physicsClientHandle,bodyUniqueId); return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
} }
bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo) bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
{ {
if (!isConnected()) if (!isConnected())
@@ -511,7 +527,7 @@ bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b
b3Warning("Not connected"); b3Warning("Not connected");
return false; return false;
} }
return (b3GetJointInfo(m_data->m_physicsClientHandle,bodyUniqueId, jointIndex,jointInfo)!=0); return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0);
} }
void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
@@ -521,55 +537,55 @@ void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parent
b3Warning("Not connected"); b3Warning("Not connected");
return; return;
} }
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
{ {
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo)); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
} }
} }
bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state) bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
{ {
if (!isConnected()) if (!isConnected())
{ {
b3Warning("Not connected"); b3Warning("Not connected");
return false; return false;
} }
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
int statusType = b3GetStatusType(statusHandle); int statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{ {
if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state)) if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
{ {
return true; return true;
} }
} }
return false; return false;
} }
bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue) bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
{ {
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int numJoints; int numJoints;
numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
if ((jointIndex >= numJoints) || (jointIndex < 0)) { if ((jointIndex >= numJoints) || (jointIndex < 0))
return false; {
} return false;
}
commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex, b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
targetValue); targetValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
return false; return false;
} }
void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args) void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
{ {
if (!isConnected()) if (!isConnected())
@@ -582,39 +598,39 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
{ {
case CONTROL_MODE_VELOCITY: case CONTROL_MODE_VELOCITY:
{ {
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY); b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY);
b3JointInfo jointInfo; b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex; int uIndex = jointInfo.m_uIndex;
b3JointControlSetKd(command,uIndex,args.m_kd); b3JointControlSetKd(command, uIndex, args.m_kd);
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
break; break;
} }
case CONTROL_MODE_POSITION_VELOCITY_PD: case CONTROL_MODE_POSITION_VELOCITY_PD:
{ {
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD); b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD);
b3JointInfo jointInfo; b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex; int uIndex = jointInfo.m_uIndex;
int qIndex = jointInfo.m_qIndex; int qIndex = jointInfo.m_qIndex;
b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition); b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition);
b3JointControlSetKp(command,uIndex,args.m_kp); b3JointControlSetKp(command, uIndex, args.m_kp);
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
b3JointControlSetKd(command,uIndex,args.m_kd); b3JointControlSetKd(command, uIndex, args.m_kd);
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
break; break;
} }
case CONTROL_MODE_TORQUE: case CONTROL_MODE_TORQUE:
{ {
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE); b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE);
b3JointInfo jointInfo; b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex; int uIndex = jointInfo.m_uIndex;
b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue); b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
break; break;
} }
@@ -625,7 +641,6 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
} }
} }
void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations) void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
{ {
if (!isConnected()) if (!isConnected())
@@ -633,12 +648,11 @@ void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
b3Warning("Not connected"); b3Warning("Not connected");
return; return;
} }
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetNumSolverIterations(command, numIterations); b3PhysicsParamSetNumSolverIterations(command, numIterations);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
} }
void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds) void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
@@ -654,10 +668,8 @@ void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
int ret; int ret;
ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds); ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
} }
void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps) void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
{ {
if (!isConnected()) if (!isConnected())
@@ -665,14 +677,13 @@ void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
b3Warning("Not connected"); b3Warning("Not connected");
return; return;
} }
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetNumSubSteps(command, numSubSteps); b3PhysicsParamSetNumSubSteps(command, numSubSteps);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
} }
bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results) bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
{ {
if (!isConnected()) if (!isConnected())
@@ -680,55 +691,52 @@ bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotS
b3Warning("Not connected"); b3Warning("Not connected");
return false; return false;
} }
b3Assert(args.m_endEffectorLinkIndex>=0); b3Assert(args.m_endEffectorLinkIndex >= 0);
b3Assert(args.m_bodyUniqueId>=0); b3Assert(args.m_bodyUniqueId >= 0);
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle,args.m_bodyUniqueId); b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle, args.m_bodyUniqueId);
if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)) if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY))
{
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
} else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
{ {
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation); b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
} else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) }
{ else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); {
} else b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation);
{ }
b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
{
b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
}
else
{
b3CalculateInverseKinematicsAddTargetPurePosition(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition);
} }
if (args.m_flags & B3_HAS_JOINT_DAMPING)
{
b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
}
b3SharedMemoryStatusHandle statusHandle; if (args.m_flags & B3_HAS_JOINT_DAMPING)
{
b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
}
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
int numPos = 0;
int numPos=0;
bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
&results.m_bodyUniqueId, &results.m_bodyUniqueId,
&numPos, &numPos,
0)!=0; 0) != 0;
if (result && numPos) if (result && numPos)
{ {
results.m_calculatedJointPositions.resize(numPos); results.m_calculatedJointPositions.resize(numPos);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle, result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
&results.m_bodyUniqueId, &results.m_bodyUniqueId,
&numPos, &numPos,
&results.m_calculatedJointPositions[0])!=0; &results.m_calculatedJointPositions[0]) != 0;
}
}
return result; return result;
} }
bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian) bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
{ {
if (!isConnected()) if (!isConnected())
@@ -736,12 +744,12 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex,
b3Warning("Not connected"); b3Warning("Not connected");
return false; return false;
} }
b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations); b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED) if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
{ {
b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian); b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
return true; return true;
} }
return false; return false;
@@ -754,14 +762,14 @@ bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3
b3Warning("Not connected"); b3Warning("Not connected");
return false; return false;
} }
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{ {
b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState); b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
return true; return true;
} }
return false; return false;
} }
@@ -787,9 +795,9 @@ void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData)
return; return;
} }
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle); b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData); b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
} }
void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData) void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData)
@@ -802,32 +810,35 @@ void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboard
return; return;
} }
b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle); b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
b3GetKeyboardEventsData(m_data->m_physicsClientHandle,keyboardEventsData); b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData);
} }
int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds)
{ {
int loggingUniqueId = -1; int loggingUniqueId = -1;
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
b3StateLoggingStart(commandHandle,loggingType,fileName.c_str());
for ( int i=0;i<objectUniqueIds.size();i++) b3StateLoggingStart(commandHandle, loggingType, fileName.c_str());
for (int i = 0; i < objectUniqueIds.size(); i++)
{ {
int objectUid = objectUniqueIds[i]; int objectUid = objectUniqueIds[i];
b3StateLoggingAddLoggingObjectUniqueId(commandHandle,objectUid); b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid);
}
if (maxLogDof > 0)
{
b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
} }
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_STATE_LOGGING_START_COMPLETED) if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
{ {
loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle); loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
} }
@@ -842,4 +853,3 @@ void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId)
b3StateLoggingStop(commandHandle, stateLoggerUniqueId); b3StateLoggingStop(commandHandle, stateLoggerUniqueId);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
} }

118
examples/RobotSimulator/b3RobotSimulatorClientAPI.h
View File

@@ -9,23 +9,18 @@
#include <string> #include <string>
struct b3RobotSimulatorLoadUrdfFileArgs struct b3RobotSimulatorLoadUrdfFileArgs
{ {
b3Vector3 m_startPosition; b3Vector3 m_startPosition;
b3Quaternion m_startOrientation; b3Quaternion m_startOrientation;
bool m_forceOverrideFixedBase; bool m_forceOverrideFixedBase;
bool m_useMultiBody; bool m_useMultiBody;
b3RobotSimulatorLoadUrdfFileArgs() b3RobotSimulatorLoadUrdfFileArgs()
: : m_startPosition(b3MakeVector3(0, 0, 0)),
m_startPosition(b3MakeVector3(0,0,0)), m_startOrientation(b3Quaternion(0, 0, 0, 1)),
m_startOrientation(b3Quaternion(0,0,0,1)), m_forceOverrideFixedBase(false),
m_forceOverrideFixedBase(false), m_useMultiBody(true)
m_useMultiBody(true)
{ {
} }
}; };
@@ -33,17 +28,15 @@ struct b3RobotSimulatorLoadUrdfFileArgs
struct b3RobotSimulatorLoadSdfFileArgs struct b3RobotSimulatorLoadSdfFileArgs
{ {
bool m_forceOverrideFixedBase; bool m_forceOverrideFixedBase;
bool m_useMultiBody; bool m_useMultiBody;
b3RobotSimulatorLoadSdfFileArgs() b3RobotSimulatorLoadSdfFileArgs()
: : m_forceOverrideFixedBase(false),
m_forceOverrideFixedBase(false), m_useMultiBody(true)
m_useMultiBody(true)
{ {
} }
}; };
struct b3RobotSimulatorLoadFileResults struct b3RobotSimulatorLoadFileResults
{ {
b3AlignedObjectArray<int> m_uniqueObjectIds; b3AlignedObjectArray<int> m_uniqueObjectIds;
@@ -55,7 +48,7 @@ struct b3RobotSimulatorLoadFileResults
struct b3RobotSimulatorJointMotorArgs struct b3RobotSimulatorJointMotorArgs
{ {
int m_controlMode; int m_controlMode;
double m_targetPosition; double m_targetPosition;
double m_kp; double m_kp;
@@ -65,52 +58,52 @@ struct b3RobotSimulatorJointMotorArgs
double m_maxTorqueValue; double m_maxTorqueValue;
b3RobotSimulatorJointMotorArgs(int controlMode) b3RobotSimulatorJointMotorArgs(int controlMode)
:m_controlMode(controlMode), : m_controlMode(controlMode),
m_targetPosition(0), m_targetPosition(0),
m_kp(0.1), m_kp(0.1),
m_targetVelocity(0), m_targetVelocity(0),
m_kd(0.9), m_kd(0.9),
m_maxTorqueValue(1000) m_maxTorqueValue(1000)
{ {
} }
}; };
enum b3RobotSimulatorInverseKinematicsFlags enum b3RobotSimulatorInverseKinematicsFlags
{ {
B3_HAS_IK_TARGET_ORIENTATION=1, B3_HAS_IK_TARGET_ORIENTATION = 1,
B3_HAS_NULL_SPACE_VELOCITY=2, B3_HAS_NULL_SPACE_VELOCITY = 2,
B3_HAS_JOINT_DAMPING=4, B3_HAS_JOINT_DAMPING = 4,
}; };
struct b3RobotSimulatorInverseKinematicArgs struct b3RobotSimulatorInverseKinematicArgs
{ {
int m_bodyUniqueId; int m_bodyUniqueId;
// double* m_currentJointPositions; // double* m_currentJointPositions;
// int m_numPositions; // int m_numPositions;
double m_endEffectorTargetPosition[3]; double m_endEffectorTargetPosition[3];
double m_endEffectorTargetOrientation[4]; double m_endEffectorTargetOrientation[4];
int m_endEffectorLinkIndex; int m_endEffectorLinkIndex;
int m_flags; int m_flags;
int m_numDegreeOfFreedom; int m_numDegreeOfFreedom;
b3AlignedObjectArray<double> m_lowerLimits; b3AlignedObjectArray<double> m_lowerLimits;
b3AlignedObjectArray<double> m_upperLimits; b3AlignedObjectArray<double> m_upperLimits;
b3AlignedObjectArray<double> m_jointRanges; b3AlignedObjectArray<double> m_jointRanges;
b3AlignedObjectArray<double> m_restPoses; b3AlignedObjectArray<double> m_restPoses;
b3AlignedObjectArray<double> m_jointDamping; b3AlignedObjectArray<double> m_jointDamping;
b3RobotSimulatorInverseKinematicArgs() b3RobotSimulatorInverseKinematicArgs()
:m_bodyUniqueId(-1), : m_bodyUniqueId(-1),
m_endEffectorLinkIndex(-1), m_endEffectorLinkIndex(-1),
m_flags(0) m_flags(0)
{ {
m_endEffectorTargetPosition[0]=0; m_endEffectorTargetPosition[0] = 0;
m_endEffectorTargetPosition[1]=0; m_endEffectorTargetPosition[1] = 0;
m_endEffectorTargetPosition[2]=0; m_endEffectorTargetPosition[2] = 0;
m_endEffectorTargetOrientation[0]=0; m_endEffectorTargetOrientation[0] = 0;
m_endEffectorTargetOrientation[1]=0; m_endEffectorTargetOrientation[1] = 0;
m_endEffectorTargetOrientation[2]=0; m_endEffectorTargetOrientation[2] = 0;
m_endEffectorTargetOrientation[3]=1; m_endEffectorTargetOrientation[3] = 1;
} }
}; };
@@ -120,20 +113,19 @@ struct b3RobotSimulatorInverseKinematicsResults
b3AlignedObjectArray<double> m_calculatedJointPositions; b3AlignedObjectArray<double> m_calculatedJointPositions;
}; };
///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet ///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
///as documented in the pybullet Quickstart Guide ///as documented in the pybullet Quickstart Guide
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA ///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
class b3RobotSimulatorClientAPI class b3RobotSimulatorClientAPI
{ {
struct b3RobotSimulatorClientAPI_InternalData* m_data; struct b3RobotSimulatorClientAPI_InternalData* m_data;
public: public:
b3RobotSimulatorClientAPI(); b3RobotSimulatorClientAPI();
virtual ~b3RobotSimulatorClientAPI(); virtual ~b3RobotSimulatorClientAPI();
bool connect(int mode, const std::string& hostName="localhost", int portOrKey=-1); bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
void disconnect(); void disconnect();
bool isConnected() const; bool isConnected() const;
@@ -145,8 +137,8 @@ public:
b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw); b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw);
b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat); b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat);
int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args=b3RobotSimulatorLoadUrdfFileArgs()); int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args = b3RobotSimulatorLoadUrdfFileArgs());
bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs()); bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs());
bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
@@ -155,14 +147,17 @@ public:
bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const; bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const;
bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation); bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation);
bool getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const;
bool resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const;
int getNumJoints(int bodyUniqueId) const; int getNumJoints(int bodyUniqueId) const;
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state); void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue); bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue);
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args); void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args);
@@ -170,29 +165,28 @@ public:
void stepSimulation(); void stepSimulation();
bool canSubmitCommand() const; bool canSubmitCommand() const;
void setRealTimeSimulation(bool enableRealTimeSimulation); void setRealTimeSimulation(bool enableRealTimeSimulation);
void setGravity(const b3Vector3& gravityAcceleration); void setGravity(const b3Vector3& gravityAcceleration);
void setTimeStep(double timeStepInSeconds); void setTimeStep(double timeStepInSeconds);
void setNumSimulationSubSteps(int numSubSteps); void setNumSimulationSubSteps(int numSubSteps);
void setNumSolverIterations(int numIterations); void setNumSolverIterations(int numIterations);
bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results); bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results);
bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian); bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState); bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState);
void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable); void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable);
int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds); int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds, int maxLogDof = -1);
void stopStateLogging(int stateLoggerUniqueId); void stopStateLogging(int stateLoggerUniqueId);
void getVREvents(b3VREventsData* vrEventsData); void getVREvents(b3VREventsData* vrEventsData);
void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData); void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData);
}; };
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H #endif //B3_ROBOT_SIMULATOR_CLIENT_API_H

14
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
View File

@@ -218,8 +218,18 @@ updateVertex(
} }
if (i.vt_idx >= 0) { if (i.vt_idx >= 0) {
texcoords.push_back(in_texcoords[2*i.vt_idx+0]); int numTexCoords = in_texcoords.size();
texcoords.push_back(in_texcoords[2*i.vt_idx+1]); int index0 = 2*i.vt_idx+0;
int index1 = 2*i.vt_idx+1;
if (index0>=0 && (index0)<numTexCoords)
{
texcoords.push_back(in_texcoords[index0]);
}
if (index1>=0 && (index1)<numTexCoords)
{
texcoords.push_back(in_texcoords[index1]);
}
} }
unsigned int idx = positions.size() / 3 - 1; unsigned int idx = positions.size() / 3 - 1;

4
examples/pybullet/hand.py
View File

@@ -16,7 +16,7 @@ if (c<0):
p.connect(p.GUI) p.connect(p.GUI)
#load the MuJoCo MJCF hand #load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/mpl.xml") objects = p.loadMJCF("MPL/MPL.xml")
hand=objects[0] hand=objects[0]
#clamp in range 400-600 #clamp in range 400-600
@@ -75,4 +75,4 @@ if (ser.isOpen()):
#print(middle) #print(middle)
#print(pink) #print(pink)
#print(index) #print(index)
#print(thumb) #print(thumb)

89
examples/pybullet/minitaur.py
View File

@@ -17,10 +17,10 @@ class Minitaur:
p.stepSimulation() p.stepSimulation()
def buildMotorIdList(self): def buildMotorIdList(self):
self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint'])
self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint']) self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint'])
self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint']) self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint'])
self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint']) self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint'])
self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint'])
self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint']) self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint'])
self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint']) self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint'])
self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint']) self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint'])
@@ -28,21 +28,17 @@ class Minitaur:
def reset(self): def reset(self):
self.quadruped = p.loadURDF("%s/quadruped/quadruped.urdf" % self.urdfRootPath,0,0,.3) self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath,0,0,.2)
self.kp = 1 self.kp = 1
self.kd = 0.1 self.kd = 0.1
self.maxForce = 3.5 self.maxForce = 3.5
self.nMotors = 8 self.nMotors = 8
self.motorIdList = [] self.motorIdList = []
self.motorDir = [1, -1, 1, -1, -1, 1, -1, 1] self.motorDir = [1, 1, 1, 1, 1, 1, 1, 1]
self.buildJointNameToIdDict() self.buildJointNameToIdDict()
self.buildMotorIdList() self.buildMotorIdList()
def disableAllMotors(self):
nJoints = p.getNumJoints(self.quadruped)
for i in range(nJoints):
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=i, controlMode=p.VELOCITY_CONTROL, force=0)
def setMotorAngleById(self, motorId, desiredAngle): def setMotorAngleById(self, motorId, desiredAngle):
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce) p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
@@ -51,42 +47,55 @@ class Minitaur:
self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle) self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle)
def resetPose(self): def resetPose(self):
#right front leg kneeFrictionForce = 0
self.disableAllMotors() halfpi = 1.57079632679
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57) kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
self.setMotorAngleByName('motor_front_rightR_joint', 1.57)
self.setMotorAngleByName('motor_front_rightL_joint',-1.57)
#left front leg #left front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57) p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57) p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_front_leftR_joint', 1.57) self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi)
self.setMotorAngleByName('motor_front_leftL_joint',-1.57) self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#right back leg p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
self.setMotorAngleByName('motor_back_rightR_joint', 1.57)
self.setMotorAngleByName('motor_back_rightL_joint',-1.57)
#left back leg #left back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57) p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57) p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_back_leftR_joint', 1.57) self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi)
self.setMotorAngleByName('motor_back_leftL_joint',-1.57) self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#right front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi)
self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#right back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi)
self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
def getBasePosition(self): def getBasePosition(self):
position, orientation = p.getBasePositionAndOrientation(self.quadruped) position, orientation = p.getBasePositionAndOrientation(self.quadruped)

2
examples/pybullet/minitaur_evaluate.py
View File

@@ -58,7 +58,7 @@ evaluate_func_map['evaluate_desired_motorAngle_hop'] = evaluate_desired_motorAng
def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=1000, sleepTime=0): def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=10000, sleepTime=0):
print('start evaluation') print('start evaluation')
beforeTime = time.time() beforeTime = time.time()
p.resetSimulation() p.resetSimulation()

4
examples/pybullet/minitaur_test.py
View File

@@ -1,3 +1,7 @@
import sys
#some python interpreters need '.' added
sys.path.append(".")
import pybullet as p import pybullet as p
from minitaur import Minitaur from minitaur import Minitaur
from minitaur_evaluate import * from minitaur_evaluate import *

21
examples/pybullet/mylittleminitaur.py Normal file
View File

@@ -0,0 +1,21 @@
import pybullet as p
import time
import math
import sys
sys.path.append(".")
from minitaur import Minitaur
p.connect(p.GUI)
p.setTimeOut(5)
#p.setPhysicsEngineParameter(numSolverIterations=50)
p.setGravity(0,0,-10)
p.setTimeStep(0.01)
urdfRoot = ''
p.loadURDF("%s/plane.urdf" % urdfRoot)
minitaur = Minitaur(urdfRoot)
while (True):
p.stepSimulation()
time.sleep(0.01)

141
examples/pybullet/quadruped.py
View File

@@ -3,28 +3,31 @@ import time
import math import math
useRealTime = 0 useRealTime = 0
fixedTimeStep = 0.001 fixedTimeStep = 0.01
speed = 10 speed = 10
amplitude = 0.8 amplitude = 0.8
jump_amp = 0.5 jump_amp = 0.5
maxForce = 3.5 maxForce = 3.5
kneeFrictionForce = 0.00 kneeFrictionForce = 0.00
kp = .05 kp = 1
kd = .5 kd = .1
physId = p.connect(p.SHARED_MEMORY) physId = p.connect(p.SHARED_MEMORY)
if (physId<0): if (physId<0):
p.connect(p.GUI) p.connect(p.GUI)
#p.resetSimulation() #p.resetSimulation()
p.loadURDF("plane.urdf",0,0,0)
p.setPhysicsEngineParameter(numSolverIterations=50)
p.setTimeOut(4) p.setTimeOut(4)
p.loadURDF("plane.urdf",0,0,0.1)
#p.loadSDF("kitchens/1.sdf")
p.setGravity(0,0,0) p.setGravity(0,0,0)
p.setTimeStep(fixedTimeStep) p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0,0,0.4])
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur.urdf",[0,0,0.2],useFixedBase=False) quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,0,0.2],orn,useFixedBase=False)
nJoints = p.getNumJoints(quadruped) nJoints = p.getNumJoints(quadruped)
jointNameToId = {} jointNameToId = {}
@@ -58,47 +61,68 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
motordir=[-1,-1,-1,-1,1,1,1,1]
halfpi = 1.57079632679
kneeangle = -2.1834
p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi)
p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle)
p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi)
p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle)
cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi)
p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle)
p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi)
p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle)
cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
#p.getNumJoints(1) #p.getNumJoints(1)
p.resetJointState(quadruped,motor_front_rightR_joint,1.57)
p.resetJointState(quadruped,knee_front_rightR_link,-2.2)
p.resetJointState(quadruped,motor_front_rightL_joint,1.57) p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi)
p.resetJointState(quadruped,knee_front_rightL_link,-2.2) p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle)
p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi)
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.resetJointState(quadruped,knee_front_rightR_link,motordir[5]*kneeangle)
cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.resetJointState(quadruped,motor_front_leftR_joint,-1.57) p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi)
p.resetJointState(quadruped,knee_front_leftR_link,2.2) p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle)
p.resetJointState(quadruped,motor_front_leftL_joint,-1.57) p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi)
p.resetJointState(quadruped,knee_front_leftL_link,2.2) p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle)
p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) cid = p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.resetJointState(quadruped,motor_back_rightR_joint,1.57)
p.resetJointState(quadruped,knee_back_rightR_link,-2.2)
p.resetJointState(quadruped,motor_back_rightL_joint,1.57)
p.resetJointState(quadruped,knee_back_rightL_link,-2.2)
p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.resetJointState(quadruped,motor_back_leftR_joint,-1.57) p.setGravity(0,0,-10)
p.resetJointState(quadruped,knee_back_leftR_link,2.2)
p.resetJointState(quadruped,motor_back_leftL_joint,-1.57)
p.resetJointState(quadruped,knee_back_leftL_link,2.2)
p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.setJointMotorControl(quadruped,motor_back_leftR_joint,p.POSITION_CONTROL,-1.57,maxForce)
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,motor_back_leftL_joint,p.POSITION_CONTROL,-1.57,maxForce)
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
motordir=[-1,-1,-1,-1,1,1,1,1] legnumbering=[
legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint] 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]
for i in range (8):
print (legnumbering[i])
#use the Minitaur leg numbering #use the Minitaur leg numbering
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
@@ -108,9 +132,12 @@ p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMo
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
#stand still
p.setRealTimeSimulation(useRealTime)
#while (True):
p.stepSimulation() # time.sleep(0.01)
#p.stepSimulation()
print("quadruped Id = ") print("quadruped Id = ")
@@ -118,15 +145,14 @@ print(quadruped)
p.saveWorld("quadru.py") p.saveWorld("quadru.py")
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.txt",[quadruped]) logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.txt",[quadruped])
p.setGravity(0,0,-10)
#stand still
p.setRealTimeSimulation(useRealTime)
#jump #jump
t = 0.0 t = 0.0
t_end = t + 10 t_end = t + 100
i=0 i=0
ref_time = time.time() ref_time = time.time()
@@ -135,26 +161,19 @@ while t < t_end:
t = time.time()-ref_time t = time.time()-ref_time
else: else:
t = t+fixedTimeStep t = t+fixedTimeStep
if (True):
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
target = math.sin(t*speed)*jump_amp+1.57;
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*target,positionGain=kp, velocityGain=kd, force=maxForce)
target = math.sin(t*speed)*jump_amp+1.57;
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*target,positionGain=kp, velocityGain=kd, force=maxForce)
if (useRealTime==0): if (useRealTime==0):
p.stepSimulation() p.stepSimulation()
time.sleep(fixedTimeStep)

34
src/Bullet3Common/b3Quaternion.h
View File

@@ -126,15 +126,16 @@ public:
sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
} }
/**@brief Set the quaternion using euler angles
/**@brief Set the quaternion using euler angles
* @param yaw Angle around Z * @param yaw Angle around Z
* @param pitch Angle around Y * @param pitch Angle around Y
* @param roll Angle around X */ * @param roll Angle around X */
void setEulerZYX(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll) void setEulerZYX(const b3Scalar& yawZ, const b3Scalar& pitchY, const b3Scalar& rollX)
{ {
b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5); b3Scalar halfYaw = b3Scalar(yawZ) * b3Scalar(0.5);
b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5); b3Scalar halfPitch = b3Scalar(pitchY) * b3Scalar(0.5);
b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5); b3Scalar halfRoll = b3Scalar(rollX) * b3Scalar(0.5);
b3Scalar cosYaw = b3Cos(halfYaw); b3Scalar cosYaw = b3Cos(halfYaw);
b3Scalar sinYaw = b3Sin(halfYaw); b3Scalar sinYaw = b3Sin(halfYaw);
b3Scalar cosPitch = b3Cos(halfPitch); b3Scalar cosPitch = b3Cos(halfPitch);
@@ -145,7 +146,30 @@ public:
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
normalize();
} }
/**@brief Get the euler angles from this quaternion
* @param yaw Angle around Z
* @param pitch Angle around Y
* @param roll Angle around X */
void getEulerZYX(b3Scalar& yawZ, b3Scalar& pitchY, b3Scalar& rollX) const
{
b3Scalar squ;
b3Scalar sqx;
b3Scalar sqy;
b3Scalar sqz;
b3Scalar sarg;
sqx = m_floats[0] * m_floats[0];
sqy = m_floats[1] * m_floats[1];
sqz = m_floats[2] * m_floats[2];
squ = m_floats[3] * m_floats[3];
rollX = b3Atan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
sarg = b3Scalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
pitchY = sarg <= b3Scalar(-1.0) ? b3Scalar(-0.5) * B3_PI: (sarg >= b3Scalar(1.0) ? b3Scalar(0.5) * B3_PI : b3Asin(sarg));
yawZ = b3Atan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
}
/**@brief Add two quaternions /**@brief Add two quaternions
* @param q The quaternion to add to this one */ * @param q The quaternion to add to this one */
B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q) B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q)

12
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -48,9 +48,11 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
} }
//solve featherstone normal contact //solve featherstone normal contact
for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++) for (int j0=0;j0<m_multiBodyNormalContactConstraints.size();j0++)
{ {
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j]; int index = iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index];
btScalar residual = 0.f; btScalar residual = 0.f;
if (iteration < infoGlobal.m_numIterations) if (iteration < infoGlobal.m_numIterations)
@@ -68,11 +70,13 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
//solve featherstone frictional contact //solve featherstone frictional contact
for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++) for (int j1=0;j1<this->m_multiBodyFrictionContactConstraints.size();j1++)
{ {
if (iteration < infoGlobal.m_numIterations) if (iteration < infoGlobal.m_numIterations)
{ {
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; int index = iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
//adjust friction limits here //adjust friction limits here
if (totalImpulse>btScalar(0)) if (totalImpulse>btScalar(0))