Merge pull request #1023 from erwincoumans/master
update minitaur.py and RobotSimulator to use data/quadruped/minitaur.urdf
This commit is contained in:
erwincoumans
GitHub
@@ -49,30 +49,30 @@
|
||||
</visual>
|
||||
|
||||
<asset>
|
||||
<mesh name="index0" file="index0_collision.stl"/>
|
||||
<mesh name="index1" file="index1_collision.stl"/>
|
||||
<mesh name="index2" file="index2_collision.stl"/>
|
||||
<mesh name="index3" file="index3_collision.stl"/>
|
||||
<mesh name="middle0" file="middle0_collision.stl"/>
|
||||
<mesh name="middle1" file="middle1_collision.stl"/>
|
||||
<mesh name="middle2" file="middle2_collision.stl"/>
|
||||
<mesh name="middle3" file="middle3_collision.stl"/>
|
||||
<mesh name="palm" file="palm.stl"/>
|
||||
<mesh name="pinky0" file="pinky0_collision.stl"/>
|
||||
<mesh name="pinky1" file="pinky1_collision.stl"/>
|
||||
<mesh name="pinky2" file="pinky2_collision.stl"/>
|
||||
<mesh name="pinky3" file="pinky3_collision.stl"/>
|
||||
<mesh name="ring0" file="ring0_collision.stl"/>
|
||||
<mesh name="ring1" file="ring1_collision.stl"/>
|
||||
<mesh name="ring2" file="ring2_collision.stl"/>
|
||||
<mesh name="ring3" file="ring3_collision.stl"/>
|
||||
<mesh name="thumb0" file="thumb0_collision.stl"/>
|
||||
<mesh name="thumb1" file="thumb1_collision.stl"/>
|
||||
<mesh name="thumb2" file="thumb2_collision.stl"/>
|
||||
<mesh name="thumb3" file="thumb3_collision.stl"/>
|
||||
<mesh name="wristx" file="wristx_collision.stl"/>
|
||||
<mesh name="wristy" file="wristy_collision.stl"/>
|
||||
<mesh name="wristz" file="wristz_collision.stl"/>
|
||||
<mesh name="index0" file="index0_collision.STL"/>
|
||||
<mesh name="index1" file="index1_collision.STL"/>
|
||||
<mesh name="index2" file="index2_collision.STL"/>
|
||||
<mesh name="index3" file="index3_collision.STL"/>
|
||||
<mesh name="middle0" file="middle0_collision.STL"/>
|
||||
<mesh name="middle1" file="middle1_collision.STL"/>
|
||||
<mesh name="middle2" file="middle2_collision.STL"/>
|
||||
<mesh name="middle3" file="middle3_collision.STL"/>
|
||||
<mesh name="palm" file="palm.STL"/>
|
||||
<mesh name="pinky0" file="pinky0_collision.STL"/>
|
||||
<mesh name="pinky1" file="pinky1_collision.STL"/>
|
||||
<mesh name="pinky2" file="pinky2_collision.STL"/>
|
||||
<mesh name="pinky3" file="pinky3_collision.STL"/>
|
||||
<mesh name="ring0" file="ring0_collision.STL"/>
|
||||
<mesh name="ring1" file="ring1_collision.STL"/>
|
||||
<mesh name="ring2" file="ring2_collision.STL"/>
|
||||
<mesh name="ring3" file="ring3_collision.STL"/>
|
||||
<mesh name="thumb0" file="thumb0_collision.STL"/>
|
||||
<mesh name="thumb1" file="thumb1_collision.STL"/>
|
||||
<mesh name="thumb2" file="thumb2_collision.STL"/>
|
||||
<mesh name="thumb3" file="thumb3_collision.STL"/>
|
||||
<mesh name="wristx" file="wristx_collision.STL"/>
|
||||
<mesh name="wristy" file="wristy_collision.STL"/>
|
||||
<mesh name="wristz" file="wristz_collision.STL"/>
|
||||
|
||||
<texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
|
||||
width="100" height="100"/>
|
||||
|
||||
@@ -427,7 +427,9 @@
|
||||
|
||||
<link name="lower_leg_front_rightR_link">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
<stiffness value="10000"/>
|
||||
<damping value="10"/>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
|
||||
<visual>
|
||||
@@ -493,7 +495,9 @@
|
||||
|
||||
<link name="lower_leg_front_rightL_link">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
<stiffness value="10000"/>
|
||||
<damping value="10"/>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
|
||||
<visual>
|
||||
@@ -557,7 +561,9 @@
|
||||
|
||||
<link name="lower_leg_front_leftR_link">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
<stiffness value="10000"/>
|
||||
<damping value="10"/>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
|
||||
<visual>
|
||||
@@ -622,7 +628,9 @@
|
||||
|
||||
<link name="lower_leg_front_leftL_link">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
<stiffness value="10000"/>
|
||||
<damping value="10"/>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
|
||||
<visual>
|
||||
@@ -684,7 +692,9 @@
|
||||
|
||||
<link name="lower_leg_back_rightR_link">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
<stiffness value="10000"/>
|
||||
<damping value="10"/>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
|
||||
<visual>
|
||||
@@ -750,9 +760,11 @@
|
||||
|
||||
<link name="lower_leg_back_rightL_link">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
<stiffness value="10000"/>
|
||||
<damping value="10"/>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
|
||||
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
|
||||
<geometry>
|
||||
@@ -814,7 +826,9 @@
|
||||
|
||||
<link name="lower_leg_back_leftR_link">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
<stiffness value="10000"/>
|
||||
<damping value="10"/>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
|
||||
<visual>
|
||||
@@ -877,7 +891,9 @@
|
||||
|
||||
<link name="lower_leg_back_leftL_link">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
<stiffness value="10000"/>
|
||||
<damping value="10"/>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
|
||||
<visual>
|
||||
|
||||
913
data/quadruped/minitaur_fixed_all.urdf
Normal file
913
data/quadruped/minitaur_fixed_all.urdf
Normal 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
929
data/quadruped/minitaur_fixed_knees.urdf
Normal 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>
|
||||
|
||||
@@ -49,57 +49,69 @@ void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim)
|
||||
sim->setJointMotorControl(m_data->m_quadrupedUniqueId,i,controlArgs);
|
||||
}
|
||||
|
||||
//right front leg
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightR_joint"],1.57);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],-2.2);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightL_joint"],-1.57);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],2.2);
|
||||
b3Scalar startAngle = B3_HALF_PI;
|
||||
b3Scalar upperLegLength = 11.5;
|
||||
b3Scalar lowerLegLength = 20;
|
||||
b3Scalar kneeAngle = B3_PI+b3Acos(upperLegLength/lowerLegLength);
|
||||
|
||||
b3Scalar motorDirs[8] = {-1,-1,-1,-1,1,1,1,1};
|
||||
b3JointInfo jointInfo;
|
||||
jointInfo.m_jointType = ePoint2PointType;
|
||||
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_rightR_link"],
|
||||
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],&jointInfo);
|
||||
setDesiredMotorAngle(sim,"motor_front_rightR_joint",1.57);
|
||||
setDesiredMotorAngle(sim,"motor_front_rightL_joint",-1.57);
|
||||
|
||||
//left front leg
|
||||
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;
|
||||
//left front leg
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftL_joint"],motorDirs[0] * startAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],motorDirs[0]*kneeAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftR_joint"],motorDirs[1] * startAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],motorDirs[1]*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_leftR_link"],
|
||||
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", -1.57);
|
||||
|
||||
//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);
|
||||
|
||||
setDesiredMotorAngle(sim,"motor_front_leftL_joint", motorDirs[0] * startAngle);
|
||||
setDesiredMotorAngle(sim,"motor_front_leftR_joint", motorDirs[1] * startAngle);
|
||||
|
||||
//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["knee_back_leftR_link"],-2.2);
|
||||
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["knee_back_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->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_leftL_link"],motorDirs[2] * kneeAngle);
|
||||
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_leftR_link"],motorDirs[3] * 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_leftR_link"],
|
||||
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", -1.57);
|
||||
setDesiredMotorAngle(sim,"motor_back_leftL_joint", motorDirs[2] * startAngle);
|
||||
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)
|
||||
@@ -109,7 +121,7 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3V
|
||||
args.m_startPosition = startPos;
|
||||
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);
|
||||
for (int i=0;i<numJoints;i++)
|
||||
|
||||
@@ -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
|
||||
sim->syncBodies();
|
||||
|
||||
sim->setTimeStep(1./240.);
|
||||
b3Scalar fixedTimeStep = 1./240.;
|
||||
|
||||
sim->setGravity(b3MakeVector3(0,0,-10));
|
||||
sim->setTimeStep(fixedTimeStep);
|
||||
|
||||
int blockId = sim->loadURDF("cube.urdf");
|
||||
b3BodyInfo bodyInfo;
|
||||
sim->getBodyInfo(blockId,&bodyInfo);
|
||||
b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3));
|
||||
b3Vector3 rpy;
|
||||
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");
|
||||
|
||||
@@ -34,15 +39,15 @@ int main(int argc, char* argv[])
|
||||
int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3));
|
||||
|
||||
|
||||
b3RobotSimulatorLoadUrdfFileArgs args;
|
||||
args.m_startPosition.setValue(2,0,1);
|
||||
int r2d2 = sim->loadURDF("r2d2.urdf",args);
|
||||
//b3RobotSimulatorLoadUrdfFileArgs args;
|
||||
//args.m_startPosition.setValue(2,0,1);
|
||||
//int r2d2 = sim->loadURDF("r2d2.urdf",args);
|
||||
|
||||
b3RobotSimulatorLoadFileResults sdfResults;
|
||||
if (!sim->loadSDF("two_cubes.sdf",sdfResults))
|
||||
{
|
||||
b3Warning("Can't load SDF!\n");
|
||||
}
|
||||
//b3RobotSimulatorLoadFileResults sdfResults;
|
||||
//if (!sim->loadSDF("two_cubes.sdf",sdfResults))
|
||||
//{
|
||||
// b3Warning("Can't load SDF!\n");
|
||||
//}
|
||||
|
||||
b3Clock clock;
|
||||
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);
|
||||
}
|
||||
}
|
||||
b3Clock::usleep(1000*1000);
|
||||
sim->stepSimulation();
|
||||
b3Clock::usleep(1000.*1000.*fixedTimeStep);
|
||||
}
|
||||
|
||||
printf("sim->disconnect\n");
|
||||
|
||||
@@ -1,17 +1,16 @@
|
||||
#include "b3RobotSimulatorClientAPI.h"
|
||||
|
||||
|
||||
//#include "SharedMemoryCommands.h"
|
||||
|
||||
#include "SharedMemory/PhysicsClientC_API.h"
|
||||
|
||||
#ifdef BT_ENABLE_ENET
|
||||
#include "SharedMemory/PhysicsClientUDP_C_API.h"
|
||||
#endif//PHYSICS_UDP
|
||||
#endif //PHYSICS_UDP
|
||||
|
||||
#ifdef BT_ENABLE_CLSOCKET
|
||||
#include "SharedMemory/PhysicsClientTCP_C_API.h"
|
||||
#endif//PHYSICS_TCP
|
||||
#endif //PHYSICS_TCP
|
||||
|
||||
#include "SharedMemory/PhysicsDirectC_API.h"
|
||||
|
||||
@@ -25,7 +24,7 @@ struct b3RobotSimulatorClientAPI_InternalData
|
||||
b3PhysicsClientHandle m_physicsClientHandle;
|
||||
|
||||
b3RobotSimulatorClientAPI_InternalData()
|
||||
:m_physicsClientHandle(0)
|
||||
: m_physicsClientHandle(0)
|
||||
{
|
||||
}
|
||||
};
|
||||
@@ -35,17 +34,16 @@ b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI()
|
||||
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)
|
||||
{
|
||||
if (m_data->m_physicsClientHandle)
|
||||
{
|
||||
b3Warning ("Already connected, disconnect first.");
|
||||
b3Warning("Already connected, disconnect first.");
|
||||
return false;
|
||||
}
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
@@ -54,89 +52,88 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
|
||||
int tcpPort = 6667;
|
||||
int key = SHARED_MEMORY_KEY;
|
||||
bool connected = false;
|
||||
|
||||
|
||||
switch (mode)
|
||||
switch (mode)
|
||||
{
|
||||
case eCONNECT_GUI:
|
||||
case eCONNECT_GUI:
|
||||
{
|
||||
int argc = 0;
|
||||
char* argv[1] = {0};
|
||||
#ifdef __APPLE__
|
||||
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
|
||||
#else
|
||||
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
|
||||
#endif
|
||||
break;
|
||||
int argc = 0;
|
||||
char* argv[1] = {0};
|
||||
#ifdef __APPLE__
|
||||
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
|
||||
#else
|
||||
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
case eCONNECT_DIRECT: {
|
||||
sm = b3ConnectPhysicsDirect();
|
||||
break;
|
||||
case eCONNECT_DIRECT:
|
||||
{
|
||||
sm = b3ConnectPhysicsDirect();
|
||||
break;
|
||||
}
|
||||
case eCONNECT_SHARED_MEMORY: {
|
||||
if (portOrKey>=0)
|
||||
case eCONNECT_SHARED_MEMORY:
|
||||
{
|
||||
if (portOrKey >= 0)
|
||||
{
|
||||
key = portOrKey;
|
||||
}
|
||||
sm = b3ConnectSharedMemory(key);
|
||||
break;
|
||||
sm = b3ConnectSharedMemory(key);
|
||||
break;
|
||||
}
|
||||
case eCONNECT_UDP:
|
||||
{
|
||||
if (portOrKey>=0)
|
||||
case eCONNECT_UDP:
|
||||
{
|
||||
if (portOrKey >= 0)
|
||||
{
|
||||
udpPort = portOrKey;
|
||||
}
|
||||
#ifdef BT_ENABLE_ENET
|
||||
#ifdef BT_ENABLE_ENET
|
||||
|
||||
sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort);
|
||||
#else
|
||||
b3Warning("UDP is not enabled in this build");
|
||||
#endif //BT_ENABLE_ENET
|
||||
#else
|
||||
b3Warning("UDP is not enabled in this build");
|
||||
#endif //BT_ENABLE_ENET
|
||||
|
||||
break;
|
||||
}
|
||||
case eCONNECT_TCP:
|
||||
}
|
||||
case eCONNECT_TCP:
|
||||
{
|
||||
if (portOrKey>=0)
|
||||
if (portOrKey >= 0)
|
||||
{
|
||||
tcpPort = portOrKey;
|
||||
}
|
||||
#ifdef BT_ENABLE_CLSOCKET
|
||||
|
||||
#ifdef BT_ENABLE_CLSOCKET
|
||||
|
||||
sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort);
|
||||
#else
|
||||
#else
|
||||
b3Warning("TCP is not enabled in this pybullet build");
|
||||
#endif //BT_ENABLE_CLSOCKET
|
||||
#endif //BT_ENABLE_CLSOCKET
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
default: {
|
||||
b3Warning("connectPhysicsServer unexpected argument");
|
||||
default:
|
||||
{
|
||||
b3Warning("connectPhysicsServer unexpected argument");
|
||||
}
|
||||
};
|
||||
|
||||
if (sm)
|
||||
{
|
||||
m_data->m_physicsClientHandle = sm;
|
||||
if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
|
||||
{
|
||||
disconnect();
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
if (sm)
|
||||
{
|
||||
m_data->m_physicsClientHandle = sm;
|
||||
if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
|
||||
{
|
||||
disconnect();
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::isConnected() const
|
||||
{
|
||||
return (m_data->m_physicsClientHandle!=0);
|
||||
return (m_data->m_physicsClientHandle != 0);
|
||||
}
|
||||
|
||||
|
||||
void b3RobotSimulatorClientAPI::disconnect()
|
||||
{
|
||||
if (!isConnected())
|
||||
@@ -157,15 +154,13 @@ void b3RobotSimulatorClientAPI::syncBodies()
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::resetSimulation()
|
||||
@@ -175,9 +170,9 @@ void b3RobotSimulatorClientAPI::resetSimulation()
|
||||
b3Warning("Not connected");
|
||||
return;
|
||||
}
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(
|
||||
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(
|
||||
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::canSubmitCommand() const
|
||||
@@ -186,7 +181,7 @@ bool b3RobotSimulatorClientAPI::canSubmitCommand() const
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return (b3CanSubmitCommand(m_data->m_physicsClientHandle));
|
||||
return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0);
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::stepSimulation()
|
||||
@@ -200,11 +195,11 @@ void b3RobotSimulatorClientAPI::stepSimulation()
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
|
||||
{
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED);
|
||||
}
|
||||
{
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
|
||||
}
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
|
||||
@@ -215,57 +210,26 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
|
||||
return;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]);
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]);
|
||||
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)
|
||||
{
|
||||
double phi, the, psi;
|
||||
phi = rollPitchYaw[0] / 2.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]);
|
||||
b3Quaternion q;
|
||||
q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]);
|
||||
return q;
|
||||
}
|
||||
|
||||
b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat)
|
||||
{
|
||||
double squ;
|
||||
double sqx;
|
||||
double sqy;
|
||||
double sqz;
|
||||
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;
|
||||
b3Scalar roll,pitch,yaw;
|
||||
quat.getEulerZYX(yaw,pitch,roll);
|
||||
b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw);
|
||||
return rpy2;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
|
||||
|
||||
|
||||
//setting the initial position, orientation and other arguments are optional
|
||||
|
||||
|
||||
b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
|
||||
args.m_startPosition[1],
|
||||
args.m_startPosition[2]);
|
||||
b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0]
|
||||
,args.m_startOrientation[1]
|
||||
,args.m_startOrientation[2]
|
||||
,args.m_startOrientation[3]);
|
||||
args.m_startPosition[1],
|
||||
args.m_startPosition[2]);
|
||||
b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
|
||||
if (args.m_forceOverrideFixedBase)
|
||||
{
|
||||
b3LoadUrdfCommandSetUseFixedBase(command,true);
|
||||
b3LoadUrdfCommandSetUseFixedBase(command, true);
|
||||
}
|
||||
b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
b3Assert(statusType==CMD_URDF_LOADING_COMPLETED);
|
||||
if (statusType==CMD_URDF_LOADING_COMPLETED)
|
||||
b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
|
||||
if (statusType == CMD_URDF_LOADING_COMPLETED)
|
||||
{
|
||||
robotUniqueId = b3GetStatusBodyIndex(statusHandle);
|
||||
}
|
||||
@@ -329,12 +288,12 @@ bool b3RobotSimulatorClientAPI::loadMJCF(const std::string& fileName, b3RobotSim
|
||||
b3Warning("Couldn't load .mjcf file.");
|
||||
return false;
|
||||
}
|
||||
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
|
||||
int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
|
||||
if (numBodies)
|
||||
{
|
||||
results.m_uniqueObjectIds.resize(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;
|
||||
@@ -359,12 +318,12 @@ bool b3RobotSimulatorClientAPI::loadBullet(const std::string& fileName, b3RobotS
|
||||
{
|
||||
return false;
|
||||
}
|
||||
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
|
||||
int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
|
||||
if (numBodies)
|
||||
{
|
||||
results.m_uniqueObjectIds.resize(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;
|
||||
@@ -383,19 +342,18 @@ bool b3RobotSimulatorClientAPI::loadSDF(const std::string& fileName, b3RobotSimu
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
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);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
b3Assert(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)
|
||||
{
|
||||
results.m_uniqueObjectIds.resize(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;
|
||||
}
|
||||
@@ -411,7 +369,7 @@ bool b3RobotSimulatorClientAPI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo*
|
||||
return false;
|
||||
}
|
||||
|
||||
int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
|
||||
int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
|
||||
return (result != 0);
|
||||
}
|
||||
|
||||
@@ -431,7 +389,8 @@ bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId,
|
||||
const int status_type = b3GetStatusType(status_handle);
|
||||
const double* actualStateQ;
|
||||
|
||||
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
|
||||
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -466,14 +425,72 @@ bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId
|
||||
|
||||
b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]);
|
||||
b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1],
|
||||
baseOrientation[2], baseOrientation[3]);
|
||||
baseOrientation[2], baseOrientation[3]);
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
|
||||
|
||||
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)
|
||||
{
|
||||
@@ -500,10 +517,9 @@ int b3RobotSimulatorClientAPI::getNumJoints(int bodyUniqueId) const
|
||||
b3Warning("Not connected");
|
||||
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)
|
||||
{
|
||||
if (!isConnected())
|
||||
@@ -511,7 +527,7 @@ bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b
|
||||
b3Warning("Not connected");
|
||||
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)
|
||||
@@ -521,55 +537,55 @@ void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parent
|
||||
b3Warning("Not connected");
|
||||
return;
|
||||
}
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3Assert(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));
|
||||
}
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3Assert(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));
|
||||
}
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state)
|
||||
bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
|
||||
{
|
||||
if (!isConnected())
|
||||
{
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
int statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
int statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
{
|
||||
if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int numJoints;
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int numJoints;
|
||||
|
||||
numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
if ((jointIndex >= numJoints) || (jointIndex < 0)) {
|
||||
return false;
|
||||
}
|
||||
numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
if ((jointIndex >= numJoints) || (jointIndex < 0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
|
||||
b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
|
||||
targetValue);
|
||||
b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
|
||||
targetValue);
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
|
||||
{
|
||||
if (!isConnected())
|
||||
@@ -582,39 +598,39 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
|
||||
{
|
||||
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;
|
||||
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
|
||||
int uIndex = jointInfo.m_uIndex;
|
||||
b3JointControlSetKd(command,uIndex,args.m_kd);
|
||||
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
|
||||
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
|
||||
b3JointControlSetKd(command, uIndex, args.m_kd);
|
||||
b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
|
||||
b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
break;
|
||||
}
|
||||
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;
|
||||
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
|
||||
int uIndex = jointInfo.m_uIndex;
|
||||
int qIndex = jointInfo.m_qIndex;
|
||||
|
||||
b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition);
|
||||
b3JointControlSetKp(command,uIndex,args.m_kp);
|
||||
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
|
||||
b3JointControlSetKd(command,uIndex,args.m_kd);
|
||||
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
|
||||
b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition);
|
||||
b3JointControlSetKp(command, uIndex, args.m_kp);
|
||||
b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
|
||||
b3JointControlSetKd(command, uIndex, args.m_kd);
|
||||
b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
break;
|
||||
}
|
||||
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;
|
||||
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
|
||||
int uIndex = jointInfo.m_uIndex;
|
||||
b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue);
|
||||
b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
break;
|
||||
}
|
||||
@@ -625,7 +641,6 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
|
||||
{
|
||||
if (!isConnected())
|
||||
@@ -633,12 +648,11 @@ void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
|
||||
b3Warning("Not connected");
|
||||
return;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3PhysicsParamSetNumSolverIterations(command, numIterations);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3PhysicsParamSetNumSolverIterations(command, numIterations);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
|
||||
@@ -654,10 +668,8 @@ void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
|
||||
int ret;
|
||||
ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
|
||||
{
|
||||
if (!isConnected())
|
||||
@@ -665,14 +677,13 @@ void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
|
||||
b3Warning("Not connected");
|
||||
return;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3PhysicsParamSetNumSubSteps(command, numSubSteps);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3PhysicsParamSetNumSubSteps(command, numSubSteps);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
|
||||
{
|
||||
if (!isConnected())
|
||||
@@ -680,55 +691,52 @@ bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotS
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3Assert(args.m_endEffectorLinkIndex>=0);
|
||||
b3Assert(args.m_bodyUniqueId>=0);
|
||||
|
||||
b3Assert(args.m_endEffectorLinkIndex >= 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))
|
||||
{
|
||||
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);
|
||||
} 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);
|
||||
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);
|
||||
}
|
||||
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);
|
||||
|
||||
|
||||
int numPos = 0;
|
||||
|
||||
int numPos=0;
|
||||
|
||||
bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
|
||||
&results.m_bodyUniqueId,
|
||||
&numPos,
|
||||
0)!=0;
|
||||
if (result && numPos)
|
||||
{
|
||||
results.m_calculatedJointPositions.resize(numPos);
|
||||
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
|
||||
&results.m_bodyUniqueId,
|
||||
&numPos,
|
||||
&results.m_calculatedJointPositions[0])!=0;
|
||||
|
||||
}
|
||||
&results.m_bodyUniqueId,
|
||||
&numPos,
|
||||
0) != 0;
|
||||
if (result && numPos)
|
||||
{
|
||||
results.m_calculatedJointPositions.resize(numPos);
|
||||
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
|
||||
&results.m_bodyUniqueId,
|
||||
&numPos,
|
||||
&results.m_calculatedJointPositions[0]) != 0;
|
||||
}
|
||||
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)
|
||||
{
|
||||
if (!isConnected())
|
||||
@@ -736,12 +744,12 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex,
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
|
||||
if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
|
||||
{
|
||||
b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
|
||||
b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
|
||||
if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
|
||||
{
|
||||
b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
@@ -754,14 +762,14 @@ bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
|
||||
if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
{
|
||||
b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
|
||||
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
|
||||
if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
{
|
||||
b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -787,9 +795,9 @@ void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData)
|
||||
return;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
|
||||
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
|
||||
b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData)
|
||||
@@ -802,32 +810,35 @@ void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboard
|
||||
return;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
|
||||
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 b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
|
||||
{
|
||||
int loggingUniqueId = -1;
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
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];
|
||||
b3StateLoggingAddLoggingObjectUniqueId(commandHandle,objectUid);
|
||||
b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid);
|
||||
}
|
||||
|
||||
if (maxLogDof > 0)
|
||||
{
|
||||
b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
|
||||
}
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType==CMD_STATE_LOGGING_START_COMPLETED)
|
||||
if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
|
||||
{
|
||||
loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
|
||||
}
|
||||
@@ -842,4 +853,3 @@ void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId)
|
||||
b3StateLoggingStop(commandHandle, stateLoggerUniqueId);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
}
|
||||
|
||||
|
||||
@@ -9,23 +9,18 @@
|
||||
|
||||
#include <string>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
struct b3RobotSimulatorLoadUrdfFileArgs
|
||||
{
|
||||
b3Vector3 m_startPosition;
|
||||
b3Quaternion m_startOrientation;
|
||||
bool m_forceOverrideFixedBase;
|
||||
bool m_useMultiBody;
|
||||
bool m_useMultiBody;
|
||||
|
||||
b3RobotSimulatorLoadUrdfFileArgs()
|
||||
:
|
||||
m_startPosition(b3MakeVector3(0,0,0)),
|
||||
m_startOrientation(b3Quaternion(0,0,0,1)),
|
||||
m_forceOverrideFixedBase(false),
|
||||
m_useMultiBody(true)
|
||||
: m_startPosition(b3MakeVector3(0, 0, 0)),
|
||||
m_startOrientation(b3Quaternion(0, 0, 0, 1)),
|
||||
m_forceOverrideFixedBase(false),
|
||||
m_useMultiBody(true)
|
||||
{
|
||||
}
|
||||
};
|
||||
@@ -33,17 +28,15 @@ struct b3RobotSimulatorLoadUrdfFileArgs
|
||||
struct b3RobotSimulatorLoadSdfFileArgs
|
||||
{
|
||||
bool m_forceOverrideFixedBase;
|
||||
bool m_useMultiBody;
|
||||
bool m_useMultiBody;
|
||||
|
||||
b3RobotSimulatorLoadSdfFileArgs()
|
||||
:
|
||||
m_forceOverrideFixedBase(false),
|
||||
m_useMultiBody(true)
|
||||
: m_forceOverrideFixedBase(false),
|
||||
m_useMultiBody(true)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct b3RobotSimulatorLoadFileResults
|
||||
{
|
||||
b3AlignedObjectArray<int> m_uniqueObjectIds;
|
||||
@@ -55,7 +48,7 @@ struct b3RobotSimulatorLoadFileResults
|
||||
struct b3RobotSimulatorJointMotorArgs
|
||||
{
|
||||
int m_controlMode;
|
||||
|
||||
|
||||
double m_targetPosition;
|
||||
double m_kp;
|
||||
|
||||
@@ -65,52 +58,52 @@ struct b3RobotSimulatorJointMotorArgs
|
||||
double m_maxTorqueValue;
|
||||
|
||||
b3RobotSimulatorJointMotorArgs(int controlMode)
|
||||
:m_controlMode(controlMode),
|
||||
m_targetPosition(0),
|
||||
m_kp(0.1),
|
||||
m_targetVelocity(0),
|
||||
m_kd(0.9),
|
||||
m_maxTorqueValue(1000)
|
||||
: m_controlMode(controlMode),
|
||||
m_targetPosition(0),
|
||||
m_kp(0.1),
|
||||
m_targetVelocity(0),
|
||||
m_kd(0.9),
|
||||
m_maxTorqueValue(1000)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
enum b3RobotSimulatorInverseKinematicsFlags
|
||||
{
|
||||
B3_HAS_IK_TARGET_ORIENTATION=1,
|
||||
B3_HAS_NULL_SPACE_VELOCITY=2,
|
||||
B3_HAS_JOINT_DAMPING=4,
|
||||
B3_HAS_IK_TARGET_ORIENTATION = 1,
|
||||
B3_HAS_NULL_SPACE_VELOCITY = 2,
|
||||
B3_HAS_JOINT_DAMPING = 4,
|
||||
};
|
||||
|
||||
struct b3RobotSimulatorInverseKinematicArgs
|
||||
{
|
||||
int m_bodyUniqueId;
|
||||
// double* m_currentJointPositions;
|
||||
// int m_numPositions;
|
||||
// double* m_currentJointPositions;
|
||||
// int m_numPositions;
|
||||
double m_endEffectorTargetPosition[3];
|
||||
double m_endEffectorTargetOrientation[4];
|
||||
int m_endEffectorLinkIndex;
|
||||
int m_endEffectorLinkIndex;
|
||||
int m_flags;
|
||||
int m_numDegreeOfFreedom;
|
||||
b3AlignedObjectArray<double> m_lowerLimits;
|
||||
b3AlignedObjectArray<double> m_upperLimits;
|
||||
b3AlignedObjectArray<double> m_jointRanges;
|
||||
b3AlignedObjectArray<double> m_restPoses;
|
||||
b3AlignedObjectArray<double> m_jointDamping;
|
||||
int m_numDegreeOfFreedom;
|
||||
b3AlignedObjectArray<double> m_lowerLimits;
|
||||
b3AlignedObjectArray<double> m_upperLimits;
|
||||
b3AlignedObjectArray<double> m_jointRanges;
|
||||
b3AlignedObjectArray<double> m_restPoses;
|
||||
b3AlignedObjectArray<double> m_jointDamping;
|
||||
|
||||
b3RobotSimulatorInverseKinematicArgs()
|
||||
:m_bodyUniqueId(-1),
|
||||
m_endEffectorLinkIndex(-1),
|
||||
m_flags(0)
|
||||
: m_bodyUniqueId(-1),
|
||||
m_endEffectorLinkIndex(-1),
|
||||
m_flags(0)
|
||||
{
|
||||
m_endEffectorTargetPosition[0]=0;
|
||||
m_endEffectorTargetPosition[1]=0;
|
||||
m_endEffectorTargetPosition[2]=0;
|
||||
m_endEffectorTargetPosition[0] = 0;
|
||||
m_endEffectorTargetPosition[1] = 0;
|
||||
m_endEffectorTargetPosition[2] = 0;
|
||||
|
||||
m_endEffectorTargetOrientation[0]=0;
|
||||
m_endEffectorTargetOrientation[1]=0;
|
||||
m_endEffectorTargetOrientation[2]=0;
|
||||
m_endEffectorTargetOrientation[3]=1;
|
||||
m_endEffectorTargetOrientation[0] = 0;
|
||||
m_endEffectorTargetOrientation[1] = 0;
|
||||
m_endEffectorTargetOrientation[2] = 0;
|
||||
m_endEffectorTargetOrientation[3] = 1;
|
||||
}
|
||||
};
|
||||
|
||||
@@ -120,20 +113,19 @@ struct b3RobotSimulatorInverseKinematicsResults
|
||||
b3AlignedObjectArray<double> m_calculatedJointPositions;
|
||||
};
|
||||
|
||||
|
||||
///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
|
||||
///as documented in the pybullet Quickstart Guide
|
||||
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
|
||||
class b3RobotSimulatorClientAPI
|
||||
{
|
||||
struct b3RobotSimulatorClientAPI_InternalData* m_data;
|
||||
|
||||
|
||||
public:
|
||||
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();
|
||||
|
||||
bool isConnected() const;
|
||||
@@ -145,8 +137,8 @@ public:
|
||||
b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw);
|
||||
b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat);
|
||||
|
||||
int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args=b3RobotSimulatorLoadUrdfFileArgs());
|
||||
bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs());
|
||||
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 loadMJCF(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 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;
|
||||
|
||||
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);
|
||||
|
||||
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args);
|
||||
@@ -170,29 +165,28 @@ public:
|
||||
void stepSimulation();
|
||||
|
||||
bool canSubmitCommand() const;
|
||||
|
||||
|
||||
void setRealTimeSimulation(bool enableRealTimeSimulation);
|
||||
|
||||
void setGravity(const b3Vector3& gravityAcceleration);
|
||||
|
||||
|
||||
void setTimeStep(double timeStepInSeconds);
|
||||
void setNumSimulationSubSteps(int numSubSteps);
|
||||
void setNumSimulationSubSteps(int numSubSteps);
|
||||
void setNumSolverIterations(int numIterations);
|
||||
|
||||
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);
|
||||
|
||||
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 getVREvents(b3VREventsData* vrEventsData);
|
||||
void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData);
|
||||
|
||||
};
|
||||
|
||||
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H
|
||||
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H
|
||||
|
||||
@@ -218,8 +218,18 @@ updateVertex(
|
||||
}
|
||||
|
||||
if (i.vt_idx >= 0) {
|
||||
texcoords.push_back(in_texcoords[2*i.vt_idx+0]);
|
||||
texcoords.push_back(in_texcoords[2*i.vt_idx+1]);
|
||||
int numTexCoords = in_texcoords.size();
|
||||
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;
|
||||
|
||||
@@ -16,7 +16,7 @@ if (c<0):
|
||||
p.connect(p.GUI)
|
||||
|
||||
#load the MuJoCo MJCF hand
|
||||
objects = p.loadMJCF("MPL/mpl.xml")
|
||||
objects = p.loadMJCF("MPL/MPL.xml")
|
||||
|
||||
hand=objects[0]
|
||||
#clamp in range 400-600
|
||||
@@ -75,4 +75,4 @@ if (ser.isOpen()):
|
||||
#print(middle)
|
||||
#print(pink)
|
||||
#print(index)
|
||||
#print(thumb)
|
||||
#print(thumb)
|
||||
|
||||
@@ -17,10 +17,10 @@ class Minitaur:
|
||||
p.stepSimulation()
|
||||
|
||||
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_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_leftR_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_back_rightL_joint'])
|
||||
@@ -28,21 +28,17 @@ class Minitaur:
|
||||
|
||||
|
||||
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.kd = 0.1
|
||||
self.maxForce = 3.5
|
||||
self.nMotors = 8
|
||||
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.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):
|
||||
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)
|
||||
|
||||
def resetPose(self):
|
||||
#right front leg
|
||||
self.disableAllMotors()
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57)
|
||||
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)
|
||||
kneeFrictionForce = 0
|
||||
halfpi = 1.57079632679
|
||||
kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)
|
||||
|
||||
#left front leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.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.01,0.2],[0,0.015,0.2])
|
||||
self.setMotorAngleByName('motor_front_leftR_joint', 1.57)
|
||||
self.setMotorAngleByName('motor_front_leftL_joint',-1.57)
|
||||
|
||||
#right back leg
|
||||
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)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi)
|
||||
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.005,0.2],[0,0.01,0.2])
|
||||
self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi)
|
||||
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)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
|
||||
#left back leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.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.01,0.2],[0,0.015,0.2])
|
||||
self.setMotorAngleByName('motor_back_leftR_joint', 1.57)
|
||||
self.setMotorAngleByName('motor_back_leftL_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_leftL_link'],self.motorDir[2]*kneeangle)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi)
|
||||
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.005,0.2],[0,0.01,0.2])
|
||||
self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi)
|
||||
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):
|
||||
position, orientation = p.getBasePositionAndOrientation(self.quadruped)
|
||||
|
||||
@@ -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')
|
||||
beforeTime = time.time()
|
||||
p.resetSimulation()
|
||||
|
||||
@@ -1,3 +1,7 @@
|
||||
import sys
|
||||
#some python interpreters need '.' added
|
||||
sys.path.append(".")
|
||||
|
||||
import pybullet as p
|
||||
from minitaur import Minitaur
|
||||
from minitaur_evaluate import *
|
||||
|
||||
21
examples/pybullet/mylittleminitaur.py
Normal file
21
examples/pybullet/mylittleminitaur.py
Normal 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)
|
||||
|
||||
@@ -3,28 +3,31 @@ import time
|
||||
import math
|
||||
|
||||
useRealTime = 0
|
||||
fixedTimeStep = 0.001
|
||||
fixedTimeStep = 0.01
|
||||
speed = 10
|
||||
amplitude = 0.8
|
||||
jump_amp = 0.5
|
||||
maxForce = 3.5
|
||||
kneeFrictionForce = 0.00
|
||||
kp = .05
|
||||
kd = .5
|
||||
kp = 1
|
||||
kd = .1
|
||||
|
||||
|
||||
physId = p.connect(p.SHARED_MEMORY)
|
||||
if (physId<0):
|
||||
p.connect(p.GUI)
|
||||
#p.resetSimulation()
|
||||
p.loadURDF("plane.urdf",0,0,0)
|
||||
p.setPhysicsEngineParameter(numSolverIterations=50)
|
||||
|
||||
p.setTimeOut(4)
|
||||
p.loadURDF("plane.urdf",0,0,0.1)
|
||||
#p.loadSDF("kitchens/1.sdf")
|
||||
|
||||
p.setGravity(0,0,0)
|
||||
p.setTimeStep(fixedTimeStep)
|
||||
|
||||
orn = p.getQuaternionFromEuler([0,0,0.4])
|
||||
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)
|
||||
|
||||
jointNameToId = {}
|
||||
@@ -58,47 +61,68 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
|
||||
motor_back_leftL_link = jointNameToId['motor_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.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,knee_front_rightL_link,-2.2)
|
||||
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.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
|
||||
p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi)
|
||||
p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle)
|
||||
p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi)
|
||||
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_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
p.resetJointState(quadruped,motor_front_leftR_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_front_leftR_link,2.2)
|
||||
p.resetJointState(quadruped,motor_front_leftL_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_front_leftL_link,2.2)
|
||||
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.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi)
|
||||
p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle)
|
||||
p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi)
|
||||
p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle)
|
||||
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.changeConstraint(cid,maxForce=10000)
|
||||
|
||||
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_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
p.resetJointState(quadruped,motor_back_leftR_joint,-1.57)
|
||||
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)
|
||||
p.setGravity(0,0,-10)
|
||||
|
||||
|
||||
|
||||
motordir=[-1,-1,-1,-1,1,1,1,1]
|
||||
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]
|
||||
legnumbering=[
|
||||
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
|
||||
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)
|
||||
@@ -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[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)
|
||||
#stand still
|
||||
p.setRealTimeSimulation(useRealTime)
|
||||
|
||||
|
||||
p.stepSimulation()
|
||||
#while (True):
|
||||
# time.sleep(0.01)
|
||||
#p.stepSimulation()
|
||||
|
||||
|
||||
print("quadruped Id = ")
|
||||
@@ -118,15 +145,14 @@ print(quadruped)
|
||||
p.saveWorld("quadru.py")
|
||||
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.txt",[quadruped])
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
|
||||
|
||||
#stand still
|
||||
p.setRealTimeSimulation(useRealTime)
|
||||
|
||||
|
||||
|
||||
#jump
|
||||
t = 0.0
|
||||
t_end = t + 10
|
||||
t_end = t + 100
|
||||
i=0
|
||||
ref_time = time.time()
|
||||
|
||||
@@ -135,26 +161,19 @@ while t < t_end:
|
||||
t = time.time()-ref_time
|
||||
else:
|
||||
t = t+fixedTimeStep
|
||||
|
||||
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)
|
||||
if (True):
|
||||
|
||||
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):
|
||||
p.stepSimulation()
|
||||
time.sleep(fixedTimeStep)
|
||||
|
||||
|
||||
@@ -126,15 +126,16 @@ public:
|
||||
sinRoll * cosPitch * cosYaw - cosRoll * 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 pitch Angle around Y
|
||||
* @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 halfPitch = b3Scalar(pitch) * b3Scalar(0.5);
|
||||
b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5);
|
||||
b3Scalar halfYaw = b3Scalar(yawZ) * b3Scalar(0.5);
|
||||
b3Scalar halfPitch = b3Scalar(pitchY) * b3Scalar(0.5);
|
||||
b3Scalar halfRoll = b3Scalar(rollX) * b3Scalar(0.5);
|
||||
b3Scalar cosYaw = b3Cos(halfYaw);
|
||||
b3Scalar sinYaw = b3Sin(halfYaw);
|
||||
b3Scalar cosPitch = b3Cos(halfPitch);
|
||||
@@ -145,7 +146,30 @@ public:
|
||||
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
|
||||
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
|
||||
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
|
||||
* @param q The quaternion to add to this one */
|
||||
B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q)
|
||||
|
||||
@@ -48,9 +48,11 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
}
|
||||
|
||||
//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;
|
||||
|
||||
if (iteration < infoGlobal.m_numIterations)
|
||||
@@ -68,11 +70,13 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
|
||||
//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)
|
||||
{
|
||||
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;
|
||||
//adjust friction limits here
|
||||
if (totalImpulse>btScalar(0))
|
||||
|
||||
Reference in New Issue
Block a user