Merge pull request #2273 from erwincoumans/master
simplify previous commit do/while
This commit is contained in:
erwincoumans
GitHub
@@ -1457,16 +1457,10 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger,
|
||||
int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r");
|
||||
|
||||
char destBuffer[8192];
|
||||
char* line = 0;
|
||||
do
|
||||
while (m_data->m_fileIO->readLine(fileId, destBuffer, 8192))
|
||||
{
|
||||
line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192);
|
||||
if (line)
|
||||
{
|
||||
xml_string += (std::string(destBuffer) + "\n");
|
||||
}
|
||||
xml_string += (std::string(destBuffer) + "\n");
|
||||
}
|
||||
while (line);
|
||||
m_data->m_fileIO->fileClose(fileId);
|
||||
|
||||
if (parseMJCFString(xml_string.c_str(), logger))
|
||||
|
||||
10259
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_abad.obj
Normal file
10259
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_abad.obj
Normal file
File diff suppressed because it is too large
Load Diff
37168
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_body.obj
Normal file
37168
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_body.obj
Normal file
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,347 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="mini_cheetah" xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
<link name="body">
|
||||
<inertial>
|
||||
<mass value="3.3"/>
|
||||
<origin xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.011253" ixy="0" ixz="0" iyy="0.362030" iyz="0" izz="0.042673"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_body.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_body.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!--!!!!!!!!!!!! Front Right Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
|
||||
<joint name="torso_to_abduct_fr_j" type="continuous">
|
||||
<axis xyz="1 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="0.19 -0.049 0.0"/>
|
||||
<parent link="body"/>
|
||||
<child link="abduct_fr"/>
|
||||
</joint>
|
||||
<link name="abduct_fr">
|
||||
<inertial>
|
||||
<mass value="0.54"/>
|
||||
<origin xyz="0.0 0.036 0."/>
|
||||
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
|
||||
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="3.141592 0.0 1.5708" xyz="-0.055 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="3.141592 0 1.5708" xyz="-0.055 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="abduct_fr_to_thigh_fr_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
|
||||
<parent link="abduct_fr"/>
|
||||
<child link="thigh_fr"/>
|
||||
</joint>
|
||||
<link name="thigh_fr">
|
||||
<inertial>
|
||||
<mass value="0.634"/>
|
||||
<origin xyz="0.0 0.016 -0.02"/>
|
||||
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
|
||||
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="thigh_fr_to_knee_fr_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
|
||||
<parent link="thigh_fr"/>
|
||||
<child link="shank_fr"/>
|
||||
</joint>
|
||||
<link name="shank_fr">
|
||||
<inertial>
|
||||
<mass value="0.064"/>
|
||||
<origin xyz="0.0 0.0 -0.209"/>
|
||||
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!--!!!!!!!!!!!! Front Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
|
||||
<joint name="torso_to_abduct_fl_j" type="continuous">
|
||||
<axis xyz="1 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="0.19 0.049 0.0"/>
|
||||
<parent link="body"/>
|
||||
<child link="abduct_fl"/>
|
||||
</joint>
|
||||
<link name="abduct_fl">
|
||||
<inertial>
|
||||
<mass value="0.54"/>
|
||||
<origin xyz="0.0 0.036 0."/>
|
||||
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
|
||||
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0. 0. -1.5708" xyz="-0.055 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 -1.5708" xyz="-0.055 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="abduct_fl_to_thigh_fl_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
|
||||
<parent link="abduct_fl"/>
|
||||
<child link="thigh_fl"/>
|
||||
</joint>
|
||||
<link name="thigh_fl">
|
||||
<inertial>
|
||||
<mass value="0.634"/>
|
||||
<origin xyz="0.0 0.016 -0.02"/>
|
||||
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
|
||||
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="thigh_fl_to_knee_fl_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
|
||||
<parent link="thigh_fl"/>
|
||||
<child link="shank_fl"/>
|
||||
</joint>
|
||||
<link name="shank_fl">
|
||||
<inertial>
|
||||
<mass value="0.064"/>
|
||||
<origin xyz="0.0 0.0 -0.209"/>
|
||||
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!--!!!!!!!!!!!! Hind Right Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
|
||||
<joint name="torso_to_abduct_hr_j" type="continuous">
|
||||
<axis xyz="1 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="-0.19 -0.049 0.0"/>
|
||||
<parent link="body"/>
|
||||
<child link="abduct_hr"/>
|
||||
</joint>
|
||||
<link name="abduct_hr">
|
||||
<inertial>
|
||||
<mass value="0.54"/>
|
||||
<origin xyz="0.0 0.036 0."/>
|
||||
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
|
||||
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0.0 1.5708" xyz="0.055 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 1.5708" xyz="0.055 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="abduct_hr_to_thigh_hr_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
|
||||
<parent link="abduct_hr"/>
|
||||
<child link="thigh_hr"/>
|
||||
</joint>
|
||||
<link name="thigh_hr">
|
||||
<inertial>
|
||||
<mass value="0.634"/>
|
||||
<origin xyz="0.0 0.016 -0.02"/>
|
||||
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
|
||||
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="thigh_hr_to_knee_hr_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
|
||||
<parent link="thigh_hr"/>
|
||||
<child link="shank_hr"/>
|
||||
</joint>
|
||||
<link name="shank_hr">
|
||||
<inertial>
|
||||
<mass value="0.064"/>
|
||||
<origin xyz="0.0 0.0 -0.209"/>
|
||||
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!--!!!!!!!!!!!! Hind Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
|
||||
<joint name="torso_to_abduct_hl_j" type="continuous">
|
||||
<axis xyz="1 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="-0.19 0.049 0.0"/>
|
||||
<parent link="body"/>
|
||||
<child link="abduct_hl"/>
|
||||
</joint>
|
||||
<link name="abduct_hl">
|
||||
<inertial>
|
||||
<mass value="0.54"/>
|
||||
<origin xyz="0.0 0.036 0."/>
|
||||
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
|
||||
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="3.141592 0.0 -1.5708" xyz="0.055 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="3.141592 0 -1.5708" xyz="0.055 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="abduct_hl_to_thigh_hl_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
|
||||
<parent link="abduct_hl"/>
|
||||
<child link="thigh_hl"/>
|
||||
</joint>
|
||||
<link name="thigh_hl">
|
||||
<inertial>
|
||||
<mass value="0.634"/>
|
||||
<origin xyz="0.0 0.016 -0.02"/>
|
||||
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
|
||||
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="thigh_hl_to_knee_hl_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
|
||||
<parent link="thigh_hl"/>
|
||||
<child link="shank_hl"/>
|
||||
</joint>
|
||||
<link name="shank_hl">
|
||||
<inertial>
|
||||
<mass value="0.064"/>
|
||||
<origin xyz="0.0 0.0 -0.209"/>
|
||||
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
</robot>
|
||||
Binary file not shown.
@@ -1,29 +0,0 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="cube.urdf">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="0.3"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.13"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.019 -0.019 -0.1502"/>
|
||||
<geometry>
|
||||
<mesh filename="channel.stl" scale="1.0 1.0 1.0"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.038 0.038 0.305"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
Binary file not shown.
@@ -1,29 +0,0 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="cube.urdf">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="0.3"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.081"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0125"/>
|
||||
<geometry>
|
||||
<mesh filename="d435i.stl" scale="1.0 1.0 1.0"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.09 0.025 0.025"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@@ -1,29 +0,0 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="cube.urdf">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="0.3"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.13"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.037 0.0045 0"/>
|
||||
<geometry>
|
||||
<mesh filename="plate.stl" scale="1.0 1.0 1.0"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.074 0.009 0.138"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
@@ -1,22 +0,0 @@
|
||||
import pybullet as p
|
||||
cin = p.connect(p.SHARED_MEMORY)
|
||||
if (cin < 0):
|
||||
cin = p.connect(p.GUI)
|
||||
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
|
||||
objects = [p.loadURDF("quadruped/microtaur/microtaur.urdf", 0.858173,-0.698485,0.227967,-0.002864,0.000163,0.951778,0.306776)]
|
||||
ob = objects[0]
|
||||
jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.568555, 0.000000, -2.177277, 1.570089, 0.000000, -2.184705, 1.570229, 0.000000, -2.182261, 1.570008, 0.000000, -2.184197, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, -1.569978, 0.000000, 2.184092, -1.569669, 0.000000, 2.186906, -1.570584, 0.000000, 2.181503, -1.568404, 0.000000, 2.178427 ]
|
||||
for jointIndex in range (p.getNumJoints(ob)):
|
||||
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
|
||||
|
||||
cid0 = p.createConstraint(1,35,1,32,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid0,maxForce=1000.000000)
|
||||
cid1 = p.createConstraint(1,7,1,10,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid1,maxForce=1000.000000)
|
||||
cid2 = p.createConstraint(1,41,1,38,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid2,maxForce=1000.000000)
|
||||
cid3 = p.createConstraint(1,13,1,16,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid3,maxForce=1000.000000)
|
||||
p.setGravity(0.000000,0.000000,-10.000000)
|
||||
p.stepSimulation()
|
||||
p.disconnect()
|
||||
Binary file not shown.
@@ -1,29 +0,0 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="xavier">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="0.3"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.64"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.045 -0.045 -0.018"/>
|
||||
<geometry>
|
||||
<mesh filename="xavier.stl" scale="1.0 1.0 1.0"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.092 0.105 0.045"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
Binary file not shown.
@@ -1,29 +0,0 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="cube.urdf">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="0.3"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.082"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0.011 0"/>
|
||||
<geometry>
|
||||
<mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba=".3 .3 .3 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.024 0.047 0.034"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
@@ -1,467 +0,0 @@
|
||||
import pybullet as p
|
||||
import pybullet_data as pd
|
||||
|
||||
import time
|
||||
import math
|
||||
|
||||
|
||||
def drawInertiaBox(parentUid, parentLinkIndex, color):
|
||||
return
|
||||
dyn = p.getDynamicsInfo(parentUid, parentLinkIndex)
|
||||
mass = dyn[0]
|
||||
frictionCoeff = dyn[1]
|
||||
inertia = dyn[2]
|
||||
if (mass > 0):
|
||||
Ixx = inertia[0]
|
||||
Iyy = inertia[1]
|
||||
Izz = inertia[2]
|
||||
boxScaleX = 0.5 * math.sqrt(6 * (Izz + Iyy - Ixx) / mass)
|
||||
boxScaleY = 0.5 * math.sqrt(6 * (Izz + Ixx - Iyy) / mass)
|
||||
boxScaleZ = 0.5 * math.sqrt(6 * (Ixx + Iyy - Izz) / mass)
|
||||
|
||||
halfExtents = [boxScaleX, boxScaleY, boxScaleZ]
|
||||
pts = [[halfExtents[0], halfExtents[1], halfExtents[2]],
|
||||
[-halfExtents[0], halfExtents[1], halfExtents[2]],
|
||||
[halfExtents[0], -halfExtents[1], halfExtents[2]],
|
||||
[-halfExtents[0], -halfExtents[1], halfExtents[2]],
|
||||
[halfExtents[0], halfExtents[1], -halfExtents[2]],
|
||||
[-halfExtents[0], halfExtents[1], -halfExtents[2]],
|
||||
[halfExtents[0], -halfExtents[1], -halfExtents[2]],
|
||||
[-halfExtents[0], -halfExtents[1], -halfExtents[2]]]
|
||||
|
||||
p.addUserDebugLine(pts[0],
|
||||
pts[1],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[1],
|
||||
pts[3],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[3],
|
||||
pts[2],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[2],
|
||||
pts[0],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
|
||||
p.addUserDebugLine(pts[0],
|
||||
pts[4],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[1],
|
||||
pts[5],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[2],
|
||||
pts[6],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[3],
|
||||
pts[7],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
|
||||
p.addUserDebugLine(pts[4 + 0],
|
||||
pts[4 + 1],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[4 + 1],
|
||||
pts[4 + 3],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[4 + 3],
|
||||
pts[4 + 2],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[4 + 2],
|
||||
pts[4 + 0],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
|
||||
|
||||
toeConstraint = True
|
||||
useMaximalCoordinates = False
|
||||
useRealTime = 1
|
||||
|
||||
#the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance
|
||||
fixedTimeStep = 1. / 100
|
||||
numSolverIterations = 50
|
||||
|
||||
if (useMaximalCoordinates):
|
||||
fixedTimeStep = 1. / 500
|
||||
numSolverIterations = 200
|
||||
|
||||
speed = 10
|
||||
amplitude = 0.8
|
||||
jump_amp = 0.5
|
||||
maxForce = 3.5
|
||||
kneeFrictionForce = 0
|
||||
kp = 1
|
||||
kd = .5
|
||||
maxKneeForce = 1000
|
||||
|
||||
physId = p.connect(p.SHARED_MEMORY)
|
||||
|
||||
if (physId < 0):
|
||||
p.connect(p.GUI)
|
||||
#p.resetSimulation()
|
||||
p.setAdditionalSearchPath(pd.getDataPath())
|
||||
angle = 0 # pick in range 0..0.2 radians
|
||||
orn = p.getQuaternionFromEuler([0, angle, 0])
|
||||
p.loadURDF("plane.urdf", [0, 0, 0], orn)
|
||||
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
|
||||
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
|
||||
"genericlogdata.bin",
|
||||
maxLogDof=16,
|
||||
logFlags=p.STATE_LOG_JOINT_TORQUES)
|
||||
p.setTimeOut(4000000)
|
||||
|
||||
p.setGravity(0, 0, 0)
|
||||
p.setTimeStep(fixedTimeStep)
|
||||
|
||||
orn = p.getQuaternionFromEuler([0, 0, 0.4])
|
||||
p.setRealTimeSimulation(0)
|
||||
quadruped = p.loadURDF("quadruped/microtaur/microtaur.urdf", [1, -1, .3],
|
||||
orn,
|
||||
useFixedBase=False,
|
||||
useMaximalCoordinates=useMaximalCoordinates,
|
||||
flags=p.URDF_USE_IMPLICIT_CYLINDER)
|
||||
nJoints = p.getNumJoints(quadruped)
|
||||
|
||||
jointNameToId = {}
|
||||
for i in range(nJoints):
|
||||
jointInfo = p.getJointInfo(quadruped, i)
|
||||
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
|
||||
|
||||
motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
|
||||
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
|
||||
knee_front_rightL_joint = jointNameToId['knee_front_rightL_joint']
|
||||
hip_front_rightR_joint = jointNameToId['hip_front_rightR_joint']
|
||||
knee_front_rightR_joint = jointNameToId['knee_front_rightR_joint']
|
||||
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
|
||||
motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
|
||||
hip_front_leftR_joint = jointNameToId['hip_front_leftR_joint']
|
||||
knee_front_leftR_joint = jointNameToId['knee_front_leftR_joint']
|
||||
motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
|
||||
motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
|
||||
knee_front_leftL_joint = jointNameToId['knee_front_leftL_joint']
|
||||
motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint']
|
||||
knee_back_rightR_joint = jointNameToId['knee_back_rightR_joint']
|
||||
motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
|
||||
motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
|
||||
knee_back_rightL_joint = jointNameToId['knee_back_rightL_joint']
|
||||
motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint']
|
||||
knee_back_leftR_joint = jointNameToId['knee_back_leftR_joint']
|
||||
motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
|
||||
motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
|
||||
knee_back_leftL_joint = jointNameToId['knee_back_leftL_joint']
|
||||
|
||||
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
|
||||
|
||||
motordir = [-1, -1, -1, -1, 1, 1, 1, 1]
|
||||
halfpi = 1.57079632679
|
||||
twopi = 4 * halfpi
|
||||
kneeangle = -2.1834
|
||||
|
||||
dyn = p.getDynamicsInfo(quadruped, -1)
|
||||
mass = dyn[0]
|
||||
friction = dyn[1]
|
||||
localInertiaDiagonal = dyn[2]
|
||||
|
||||
print("localInertiaDiagonal", localInertiaDiagonal)
|
||||
|
||||
#this is a no-op, just to show the API
|
||||
p.changeDynamics(quadruped, -1, localInertiaDiagonal=localInertiaDiagonal)
|
||||
|
||||
#for i in range (nJoints):
|
||||
# p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
|
||||
|
||||
drawInertiaBox(quadruped, -1, [1, 0, 0])
|
||||
#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
|
||||
|
||||
for i in range(nJoints):
|
||||
drawInertiaBox(quadruped, i, [0, 1, 0])
|
||||
|
||||
if (useMaximalCoordinates):
|
||||
steps = 400
|
||||
for aa in range(steps):
|
||||
p.setJointMotorControl2(quadruped, motor_front_leftL_joint, p.POSITION_CONTROL,
|
||||
motordir[0] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_front_leftR_joint, p.POSITION_CONTROL,
|
||||
motordir[1] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_back_leftL_joint, p.POSITION_CONTROL,
|
||||
motordir[2] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_back_leftR_joint, p.POSITION_CONTROL,
|
||||
motordir[3] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_front_rightL_joint, p.POSITION_CONTROL,
|
||||
motordir[4] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_front_rightR_joint, p.POSITION_CONTROL,
|
||||
motordir[5] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_back_rightL_joint, p.POSITION_CONTROL,
|
||||
motordir[6] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_back_rightR_joint, p.POSITION_CONTROL,
|
||||
motordir[7] * halfpi * float(aa) / steps)
|
||||
|
||||
p.setJointMotorControl2(quadruped, knee_front_leftL_joint, p.POSITION_CONTROL,
|
||||
motordir[0] * (kneeangle + twopi) * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_front_leftR_joint, p.POSITION_CONTROL,
|
||||
motordir[1] * kneeangle * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_back_leftL_joint, p.POSITION_CONTROL,
|
||||
motordir[2] * kneeangle * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_back_leftR_joint, p.POSITION_CONTROL,
|
||||
motordir[3] * (kneeangle + twopi) * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_front_rightL_joint, p.POSITION_CONTROL,
|
||||
motordir[4] * (kneeangle) * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_front_rightR_joint, p.POSITION_CONTROL,
|
||||
motordir[5] * (kneeangle + twopi) * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_back_rightL_joint, p.POSITION_CONTROL,
|
||||
motordir[6] * (kneeangle + twopi) * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_back_rightR_joint, p.POSITION_CONTROL,
|
||||
motordir[7] * kneeangle * float(aa) / steps)
|
||||
|
||||
p.stepSimulation()
|
||||
#time.sleep(fixedTimeStep)
|
||||
else:
|
||||
|
||||
p.resetJointState(quadruped, motor_front_leftL_joint, motordir[0] * halfpi)
|
||||
p.resetJointState(quadruped, knee_front_leftL_joint, motordir[0] * kneeangle)
|
||||
p.resetJointState(quadruped, motor_front_leftR_joint, motordir[1] * halfpi)
|
||||
p.resetJointState(quadruped, knee_front_leftR_joint, motordir[1] * kneeangle)
|
||||
|
||||
p.resetJointState(quadruped, motor_back_leftL_joint, motordir[2] * halfpi)
|
||||
p.resetJointState(quadruped, knee_back_leftL_joint, motordir[2] * kneeangle)
|
||||
p.resetJointState(quadruped, motor_back_leftR_joint, motordir[3] * halfpi)
|
||||
p.resetJointState(quadruped, knee_back_leftR_joint, motordir[3] * kneeangle)
|
||||
|
||||
p.resetJointState(quadruped, motor_front_rightL_joint, motordir[4] * halfpi)
|
||||
p.resetJointState(quadruped, knee_front_rightL_joint, motordir[4] * kneeangle)
|
||||
p.resetJointState(quadruped, motor_front_rightR_joint, motordir[5] * halfpi)
|
||||
p.resetJointState(quadruped, knee_front_rightR_joint, motordir[5] * kneeangle)
|
||||
|
||||
p.resetJointState(quadruped, motor_back_rightL_joint, motordir[6] * halfpi)
|
||||
p.resetJointState(quadruped, knee_back_rightL_joint, motordir[6] * kneeangle)
|
||||
p.resetJointState(quadruped, motor_back_rightR_joint, motordir[7] * halfpi)
|
||||
p.resetJointState(quadruped, knee_back_rightR_joint, motordir[7] * kneeangle)
|
||||
|
||||
#p.getNumJoints(1)
|
||||
|
||||
if (toeConstraint):
|
||||
cid = p.createConstraint(quadruped, knee_front_leftR_joint, quadruped, knee_front_leftL_joint,
|
||||
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
|
||||
p.changeConstraint(cid, maxForce=maxKneeForce)
|
||||
cid = p.createConstraint(quadruped, knee_front_rightR_joint, quadruped, knee_front_rightL_joint,
|
||||
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
|
||||
p.changeConstraint(cid, maxForce=maxKneeForce)
|
||||
cid = p.createConstraint(quadruped, knee_back_leftR_joint, quadruped, knee_back_leftL_joint,
|
||||
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
|
||||
p.changeConstraint(cid, maxForce=maxKneeForce)
|
||||
cid = p.createConstraint(quadruped, knee_back_rightR_joint, quadruped, knee_back_rightL_joint,
|
||||
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
|
||||
p.changeConstraint(cid, maxForce=maxKneeForce)
|
||||
|
||||
if (1):
|
||||
p.setJointMotorControl(quadruped, knee_front_leftL_joint, p.VELOCITY_CONTROL, 0,
|
||||
kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_front_leftR_joint, p.VELOCITY_CONTROL, 0,
|
||||
kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_front_rightL_joint, p.VELOCITY_CONTROL, 0,
|
||||
kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_front_rightR_joint, p.VELOCITY_CONTROL, 0,
|
||||
kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_back_leftL_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_back_leftR_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_back_leftL_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_back_leftR_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_back_rightL_joint, p.VELOCITY_CONTROL, 0,
|
||||
kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_back_rightR_joint, p.VELOCITY_CONTROL, 0,
|
||||
kneeFrictionForce)
|
||||
|
||||
p.setGravity(0, 0, -10)
|
||||
|
||||
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)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[2],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[2] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[3],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[3] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[4],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[4] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[5],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[5] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[6],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[6] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[7],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[7] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
#stand still
|
||||
p.setRealTimeSimulation(useRealTime)
|
||||
|
||||
t = 0.0
|
||||
t_end = t + 5
|
||||
ref_time = time.time()
|
||||
while (t < t_end):
|
||||
p.setGravity(0, 0, -10)
|
||||
if (useRealTime):
|
||||
t = time.time() - ref_time
|
||||
else:
|
||||
t = t + fixedTimeStep
|
||||
if (useRealTime == 0):
|
||||
p.stepSimulation()
|
||||
time.sleep(fixedTimeStep)
|
||||
|
||||
print("quadruped Id = ")
|
||||
print(quadruped)
|
||||
p.saveWorld("quadru.py")
|
||||
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "quadrupedLog.bin", [quadruped])
|
||||
|
||||
#jump
|
||||
t = 0.0
|
||||
t_end = t + 100
|
||||
i = 0
|
||||
ref_time = time.time()
|
||||
|
||||
while (1):
|
||||
if (useRealTime):
|
||||
t = time.time() - ref_time
|
||||
else:
|
||||
t = t + fixedTimeStep
|
||||
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)
|
||||
@@ -0,0 +1,23 @@
|
||||
import pybullet as p
|
||||
import pybullet_data as pd
|
||||
import time
|
||||
|
||||
p.connect(p.GUI)
|
||||
p.setGravity(0,0,-9.8)
|
||||
p.setAdditionalSearchPath(pd.getDataPath())
|
||||
floor = p.loadURDF("plane.urdf")
|
||||
startPos = [0,0,0.5]
|
||||
robot = p.loadURDF("mini_cheetah/mini_cheetah.urdf",startPos)
|
||||
numJoints = p.getNumJoints(robot)
|
||||
p.changeVisualShape(robot,-1,rgbaColor=[1,1,1,1])
|
||||
for j in range (numJoints):
|
||||
p.changeVisualShape(robot,j,rgbaColor=[1,1,1,1])
|
||||
force=200
|
||||
pos=0
|
||||
p.setJointMotorControl2(robot,j,p.POSITION_CONTROL,pos,force=force)
|
||||
dt = 1./240.
|
||||
p.setTimeStep(dt)
|
||||
while (1):
|
||||
p.stepSimulation()
|
||||
time.sleep(dt)
|
||||
|
||||
Reference in New Issue
Block a user