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

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

Browse Source
This commit is contained in:
Erwin Coumans
2017-01-22 21:26:15 -08:00
parent 2bc787626e 73e83d6e84
commit f2da5d41c9
55 changed files with 2037 additions and 293 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
VERSION
View File

@@ -1 +1 @@
2.85 2.86

19
data/MPL/LICENSE.txt Normal file
View File

@@ -0,0 +1,19 @@
<!--
This file and the .stl mesh files referenced from it have been derived by Roboti LLC from the model of the Modular Prosthetic Limb developed by The Johns Hopkins University / Applied Physics Laboratory. The modifications are as follows: the original meshes have been replaced with their convex hulls; the original URDF model has been converted to the MJCF format and a number of MJCF-specific fields have been added.
The Johns Hopkins University / Applied Physics Laboratory has given Roboti LLC permission to distribute the modified model under the following license:
=========================
(C) 2013 The Johns Hopkins University / Applied Physics Laboratory All Rights Reserved.
Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
========================
The modifications made by Roboti LLC are also licensed under the Apache License version 2.0.
-->

473
data/MPL/MPL.xml Normal file
View File

@@ -0,0 +1,473 @@
<mujoco model="MPL">
<!--
This file and the .stl mesh files referenced from it have been derived by Roboti LLC from the model of the Modular Prosthetic Limb developed by The Johns Hopkins University / Applied Physics Laboratory. The modifications are as follows: the original meshes have been replaced with their convex hulls; the original URDF model has been converted to the MJCF format and a number of MJCF-specific fields have been added.
The Johns Hopkins University / Applied Physics Laboratory has given Roboti LLC permission to distribute the modified model under the following license:
=========================
(C) 2013 The Johns Hopkins University / Applied Physics Laboratory All Rights Reserved.
Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
========================
The modifications made by Roboti LLC are also licensed under the Apache License version 2.0.
-->
<compiler angle="radian" meshdir="mesh/" texturedir="texture/"/>
<option timestep="0.002" iterations="50" apirate="50"/>
<size njmax="600" nconmax="150" nstack="300000"/>
<default>
<default class="MPL">
<geom material="MatMesh" contype="1" conaffinity="1" condim="4" margin="0.001"/>
<joint limited="true" damping="0.2" armature=".01"/>
<site material="MatTouch" type="ellipsoid" group="3"/>
<position ctrllimited="true" kp="10"/>
</default>
<default class="IMU">
<site material="MatIMU" type="box" group="4"/>
</default>
<default class="free">
<joint type="free" damping="0" armature="0" limited="false"/>
</default>
</default>
<statistic extent="1" center="0 -0.2 0.2"/>
<visual>
<quality shadowsize="2048"/>
<map fogstart="6" fogend="10"/>
<headlight diffuse=".6 .6 .6" specular="0 0 0"/>
</visual>
<asset>
<mesh name="index0" file="index0.stl"/>
<mesh name="index1" file="index1.stl"/>
<mesh name="index2" file="index2.stl"/>
<mesh name="index3" file="index3.stl"/>
<mesh name="middle0" file="middle0.stl"/>
<mesh name="middle1" file="middle1.stl"/>
<mesh name="middle2" file="middle2.stl"/>
<mesh name="middle3" file="middle3.stl"/>
<mesh name="palm" file="palm.stl"/>
<mesh name="pinky0" file="pinky0.stl"/>
<mesh name="pinky1" file="pinky1.stl"/>
<mesh name="pinky2" file="pinky2.stl"/>
<mesh name="pinky3" file="pinky3.stl"/>
<mesh name="ring0" file="ring0.stl"/>
<mesh name="ring1" file="ring1.stl"/>
<mesh name="ring2" file="ring2.stl"/>
<mesh name="ring3" file="ring3.stl"/>
<mesh name="thumb0" file="thumb0.stl"/>
<mesh name="thumb1" file="thumb1.stl"/>
<mesh name="thumb2" file="thumb2.stl"/>
<mesh name="thumb3" file="thumb3.stl"/>
<mesh name="wristx" file="wristx.stl"/>
<mesh name="wristy" file="wristy.stl"/>
<mesh name="wristz" file="wristz.stl"/>
<texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
width="100" height="100"/>
<texture name="groundplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 .2 .3"
width="100" height="100"/>
<texture name="skin" type="cube" file="skin.png"/>
<texture name="marble2d" type="2d" file="marble.png"/>
<texture name="marblecube" type="cube" file="marble.png"/>
<material name="groundplane" texture="groundplane" texrepeat="10 10"/>
<material name="table2d" texture="marble2d" reflectance="0.3" rgba=".8 .8 .8 1"/>
<material name="tablecube" texture="marblecube" rgba=".8 .8 .8 1"/>
<material name="MatTouch" rgba=".3 .9 .3 .3"/>
<material name="MatIMU" rgba=".1 .1 .9 1"/>
<material name="MatMesh" texture="skin"/>
</asset>
<contact>
<exclude body1="wristz" body2="wristy"/>
<exclude body1="wristx" body2="thumb0"/>
<exclude body1="palm" body2="thumb1"/>
<exclude body1="palm" body2="index1"/>
<exclude body1="palm" body2="middle1"/>
<exclude body1="palm" body2="ring1"/>
<exclude body1="palm" body2="pinky1"/>
</contact>
<worldbody>
<!-- ======= ROBOT ======= -->
<body childclass="MPL" name="forearm" pos="0 -0.35 0.2" axisangle="0 0 1 3.141592">
<geom type="cylinder" size="0.02 0.01" zaxis="0 1 0"/>
<body name="wristy" pos="0 0 0">
<inertial pos="-7.08369e-005 -0.0217787 -0.000286168" quat="0.707488 0.00581744 -0.0107421 0.70662" mass="0.0272932" diaginertia="2.46813e-005 1.77029e-005 1.71079e-005" />
<geom type="mesh" mesh="wristy"/>
<joint name="wrist_PRO" type="hinge" damping="0.4" pos="0 0 0" axis="0 1 0" range="-1.57 1.57"/>
<body name="wristx" pos="-3.36826e-005 -0.0476452 0.00203763">
<inertial pos="0.00139174 -0.00975189 -0.00252668" quat="-0.0729226 0.705959 0.0352732 0.703605" mass="0.010691" diaginertia="5.04455e-006 4.25035e-006 3.25677e-006" />
<joint name="wrist_UDEV" damping="0.4" type="hinge" pos="0 0 0" axis="0 0 -1" range="-0.261 0.785"/>
<geom type="mesh" mesh="wristx"/>
<body name="wristz" pos="0.0001872 -0.03 -0.002094">
<inertial pos="0.000579016 -0.00125952 0.000455968" quat="0.527723 0.475346 0.521597 0.472749" mass="0.00602247" diaginertia="1.58133e-006 1.43102e-006 1.26861e-006" />
<joint name="wrist_FLEX" damping="0.4" type="hinge" pos="0 0 0" axis="1 0 0" range="-1.04 1.04"/>
<geom type="mesh" mesh="wristz"/>
<!-- ======= PALM ======= -->
<body name="palm" pos="0.025625 0 0">
<inertial pos="-0.0217876 -0.0376147 0.00276997" quat="-0.146373 0.723094 0.0985561 0.66783" mass="0.119867" diaginertia="0.000123088 0.000100082 6.89467e-005" />
<geom type="mesh" mesh="palm"/>
<site name="palm_thumb" pos="-0.0052 -0.0438 -0.0182" size=".017 .03 .01"/>
<site name="palm_pinky" pos="-0.0358 -0.0401 -0.0183" size=".017 .03 .01"/>
<site name="palm_side" pos="-0.0604 -0.0329 -0.0048" size=".01 .03 .015"/>
<site name="palm_back" pos="-0.0246 -0.0446 0.018" size=".03 .035 .015"/>
<!-- ======= THUMB ======= -->
<body name="thumb0" pos="0.00835752 -0.0206978 -0.010093" quat="0.990237 0.0412644 -0.0209178 -0.13149">
<inertial pos="0.00863339 -0.000156884 -0.000945846" quat="0.408795 0.551643 0.541079 0.485602" mass="0.00336696" diaginertia="4.50769e-007 4.48758e-007 2.35017e-007" />
<joint name="thumb_ABD" type="hinge" pos="0 0 0" axis="0 1 0" range="0 2.07"/>
<geom type="mesh" mesh="thumb0"/>
<body name="thumb1" pos="0.0209172 -0.00084 0.0014476">
<inertial pos="0.019024 0.000361131 -0.000186763" quat="0.5208 0.469572 0.484571 0.522934" mass="0.00596213" diaginertia="9.88001e-007 9.45125e-007 5.32989e-007" />
<joint name="thumb_MCP" type="hinge" pos="0 0 0" axis="0 0 -1" range="0 1.03"/>
<geom type="mesh" mesh="thumb1"/>
<site name="thumb_proximal" pos="0.0173 -0.008 0.0009" size=".015 .0075 .01"/>
<body name="thumb2" pos="0.0335 0 -0.0007426">
<inertial pos="0.0188965 0.000375725 0.00065381" quat="0.502274 0.484638 0.475673 0.535333" mass="0.00599792" diaginertia="9.96692e-007 9.64948e-007 5.14416e-007" />
<joint name="thumb_PIP" type="hinge" pos="0 0 0" axis="0 0 -1" range="0 1.03"/>
<geom type="mesh" mesh="thumb2"/>
<site name="thumb_medial" pos="0.0182 -0.008 0.0015" size=".015 .0075 .01"/>
<body name="thumb3" pos="0.0335 0 0.0010854">
<inertial pos="0.0188965 0.000375725 0.00065381" quat="0.502274 0.484638 0.475673 0.535333" mass="0.00599792" diaginertia="9.96692e-007 9.64948e-007 5.14416e-007" />
<joint name="thumb_DIP" type="hinge" pos="0 0 0" axis="0 0 -1" range="-0.819 1.28"/>
<geom type="mesh" mesh="thumb3"/>
<site name="thumb_distal" pos="0.0156 -0.007 0.0003" size=".015 .0075 .01" axisangle="0 0 1 0.2"/>
<site class="IMU" name="thumb_IMU" pos="0.0099 -0.00052 0" quat=".5 .5 .5 .5" size=".003 .003 .003"/>
</body>
</body>
</body>
</body>
<!-- ======= INDEX ======= -->
<body name="index0" pos="0.00986485 -0.0658 0.00101221" quat="0.996195 0 0.0871557 0">
<inertial pos="-0.000142572 -0.00548494 0.000206145" quat="0.699132 0.714861 -0.000723869 0.013694" mass="0.00295579" diaginertia="4.22462e-007 4.02281e-007 1.93868e-007" />
<joint name="index_ABD" type="hinge" pos="0 0 0" axis="0 0 1" range="0 0.345"/>
<geom type="mesh" mesh="index0"/>
<body name="index1" pos="6.26e-005 -0.018 0">
<inertial pos="0.000406487 -0.0213125 0.000655609" quat="0.698452 0.715642 -0.00415384 0.0023049" mass="0.00478235" diaginertia="8.18599e-007 7.95693e-007 3.06254e-007" />
<joint name="index_MCP" type="hinge" pos="0 0 0" axis="1 0 0" range="-0.785 1.57"/>
<geom type="mesh" mesh="index1"/>
<site name="index_proximal" pos="0 -0.0235 -0.007" size=".009 .015 .0075"/>
<body name="index2" pos="0.001086 -0.0435 0.0005">
<inertial pos="-0.000841462 -0.012689 0.000572665" quat="0.734882 0.677481 -0.028511 0.0124827" mass="0.00344764" diaginertia="3.63962e-007 3.59059e-007 1.05304e-007" />
<joint name="index_PIP" type="hinge" pos="0 0 0" axis="1 0 0" range="0 1.72"/>
<geom type="mesh" mesh="index2"/>
<site name="index_medial" pos="0 -0.009 -0.0047" size=".0075 .01 .006"/>
<body name="index3" pos="-0.000635 -0.0245 0">
<inertial pos="4.32004e-005 -0.0125318 0.000903476" quat="0.516251 0.4829 -0.483241 0.516498" mass="0.00274415" diaginertia="1.19635e-007 1.09202e-007 7.77873e-008" />
<joint name="index_DIP" type="hinge" pos="0 0 0" axis="1 0 0" range="0 1.38"/>
<geom type="mesh" mesh="index3"/>
<site name="index_distal" pos="0 -0.0132 -0.0038" size=".0075 .01 .006"/>
<site class="IMU" name="index_IMU" pos="0 -0.0093 0.00063" quat=".5 .5 -.5 .5" size=".003 .003 .003"/>
</body>
</body>
</body>
</body>
<!-- ======= MIDDLE ======= -->
<body name="middle0" pos="-0.012814 -0.0779014 0.00544608" quat="-3.14 0.0436194 0 0">
<inertial pos="-0.000142567 -0.00548493 0.000206162" quat="0.699131 0.714862 -0.000723874 0.013694" mass="0.00295579" diaginertia="4.22461e-007 4.02281e-007 1.93868e-007" />
<!--<joint name="middle0" type="hinge" pos="0 0 0" range="-0.345 0" axis="0 0 -1" />-->
<geom type="mesh" mesh="middle0"/>
<body name="middle1" pos="6.26e-005 -0.018 0">
<inertial pos="0.000406411 -0.0213125 0.00065565" quat="0.698451 0.715642 -0.00415503 0.00230486" mass="0.00478229" diaginertia="8.18595e-007 7.9569e-007 3.06253e-007" />
<joint name="middle_MCP" type="hinge" pos="0 0 0" axis="1 0 0" range="-0.785 1.57"/>
<geom type="mesh" mesh="middle1"/>
<site name="middle_proximal" pos="0 -0.025 -0.007" size=".009 .015 .0075"/>
<body name="middle2" pos="0.001086 -0.0435 0.0005">
<inertial pos="-0.000841444 -0.012689 0.00057266" quat="0.734883 0.677482 -0.0284727 0.0124412" mass="0.00344765" diaginertia="3.63962e-007 3.5906e-007 1.05304e-007" />
<joint name="middle_PIP" type="hinge" pos="0 0 0" axis="1 0 0" range="0 1.73"/>
<geom type="mesh" mesh="middle2"/>
<site name="middle_medial" pos="0 -0.0146 -0.0047" size=".0075 .01 .006"/>
<body name="middle3" pos="-0.000635 -0.0245 0">
<inertial pos="4.31236e-005 -0.0125318 0.000903446" quat="0.516263 0.482913 -0.483228 0.516487" mass="0.00274417" diaginertia="1.19635e-007 1.09202e-007 7.77884e-008" />
<joint name="middle_DIP" type="hinge" pos="0 0 0" axis="1 0 0" range="0 1.38"/>
<geom type="mesh" mesh="middle3"/>
<site name="middle_distal" pos="0 -0.0129 -0.0038" size=".0075 .01 .006"/>
<site class="IMU" name="middle_IMU" pos="0 -0.0093 0.00063" quat=".5 .5 -.5 .5" size=".003 .003 .003"/>
</body>
</body>
</body>
</body>
<!-- ======= RING ======= -->
<body name="ring0" pos="-0.0354928 -0.0666999 0.00151221" quat="0.996195 0 -0.0871557 0">
<inertial pos="-0.000142559 -0.00548494 0.000206147" quat="0.699132 0.714861 -0.000720946 0.013691" mass="0.00295579" diaginertia="4.22462e-007 4.02281e-007 1.93868e-007" />
<joint name="ring_ABD" type="hinge" pos="0 0 0" axis="0 0 -1" range="0 0.345"/>
<geom type="mesh" mesh="ring0"/>
<body name="ring1" pos="6.26e-005 -0.018 0">
<inertial pos="0.000406447 -0.0213125 0.00065563" quat="0.698451 0.715642 -0.00415675 0.00230715" mass="0.00478232" diaginertia="8.18597e-007 7.95692e-007 3.06254e-007" />
<joint name="ring_MCP" type="hinge" pos="0 0 0" axis="1 0 0" range="-0.785 1.57"/>
<geom type="mesh" mesh="ring1"/>
<site name="ring_proximal" pos="0 -0.0259 -0.007" size=".009 .015 .0075"/>
<body name="ring2" pos="0.001086 -0.0435 0.0005">
<inertial pos="-0.000841518 -0.012689 0.000572674" quat="0.73488 0.677478 -0.0285773 0.0125557" mass="0.00344767" diaginertia="3.63963e-007 3.59061e-007 1.05305e-007" />
<joint name="ring_PIP" type="hinge" pos="0 0 0" axis="1 0 0" range="0 1.72"/>
<geom type="mesh" mesh="ring2"/>
<site name="ring_medial" pos="0 -0.0137 -0.0047" size=".0075 .01 .006"/>
<body name="ring3" pos="-0.000635 -0.0245 0">
<inertial pos="4.31973e-005 -0.0125318 0.000903457" quat="0.516255 0.482902 -0.483238 0.516495" mass="0.00274416" diaginertia="1.19635e-007 1.09202e-007 7.77877e-008" />
<joint name="ring_DIP" type="hinge" pos="0 0 0" axis="1 0 0" range="0 1.38"/>
<geom type="mesh" mesh="ring3"/>
<site name="ring_distal" pos="0 -0.0117 -0.0038" size=".0075 .01 .006"/>
<site class="IMU" name="ring_IMU" pos="0 -0.0093 0.00063" quat=".5 .5 -.5 .5" size=".003 .003 .003"/>
</body>
</body>
</body>
</body>
<!-- ======= LITTLE ======= -->
<body name="pinky0" pos="-0.0562459 -0.0554001 -0.00563858" quat="0.996195 0 -0.0871557 0">
<inertial pos="-0.000142559 -0.00538484 0.000206147" quat="0.699132 0.714861 -0.000721037 0.0136911" mass="0.00295579" diaginertia="4.22462e-007 4.02281e-007 1.93868e-007" />
<joint name="pinky_ABD" type="hinge" pos="0 0 0" axis="0 0 -1" range="0 0.345"/>
<geom type="mesh" mesh="pinky0"/>
<body name="pinky1" pos="6.26e-005 -0.0178999 0">
<inertial pos="0.000458624 -0.0160478 0.000924735" quat="0.685529 0.72723 0.021252 -0.0270914" mass="0.0034099" diaginertia="4.03391e-007 3.84061e-007 2.19866e-007" />
<joint name="pinky_MCP" type="hinge" pos="0 0 0" axis="1 0 0" range="-0.785 1.57"/>
<geom type="mesh" mesh="pinky1"/>
<site name="pinky_proximal" pos="0 -0.021 -0.0066" size=".009 .013 .0075"/>
<body name="pinky2" pos="0.000578 -0.033 0.0005">
<inertial pos="-0.000270832 -0.00914628 0.000738493" quat="0.746786 0.664476 -4.11065e-005 -0.0279675" mass="0.00250622" diaginertia="1.79089e-007 1.75934e-007 7.44543e-008" />
<joint name="pinky_PIP" type="hinge" pos="0 0 0" axis="1 0 0" range="0 1.72"/>
<geom type="mesh" mesh="pinky2"/>
<site name="pinky_medial" pos="0 -0.0117 -0.0047" size=".0075 .01 .006"/>
<body name="pinky3" pos="-4.78e-005 -0.0175 0">
<inertial pos="3.85026e-005 -0.0125047 0.000912295" quat="0.516037 0.484447 -0.483043 0.515448" mass="0.00273265" diaginertia="1.19141e-007 1.08629e-007 7.77271e-008" />
<joint name="pinky_DIP" type="hinge" pos="0 0 0" axis="1 0 0" range="0 1.38"/>
<geom type="mesh" mesh="pinky3"/>
<site name="pinky_distal" pos="0 -0.0121 -0.0038" size=".0075 .01 .006"/>
<site class="IMU" name="pinky_IMU" pos="0 -0.0093 0.00063" quat=".5 .5 -.5 .5" size=".003 .003 .003"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</worldbody>
<sensor>
<jointpos name="Sjp_wrist_PRO" joint="wrist_PRO"/>
<jointpos name="Sjp_wrist_UDEV" joint="wrist_UDEV"/>
<jointpos name="Sjp_wrist_FLEX" joint="wrist_FLEX"/>
<jointpos name="Sjp_thumb_ABD" joint="thumb_ABD"/>
<jointpos name="Sjp_thumb_MCP" joint="thumb_MCP"/>
<jointpos name="Sjp_thumb_PIP" joint="thumb_PIP"/>
<jointpos name="Sjp_thumb_DIP" joint="thumb_DIP"/>
<jointpos name="Sjp_index_ABD" joint="index_ABD"/>
<jointpos name="Sjp_index_MCP" joint="index_MCP"/>
<jointpos name="Sjp_index_PIP" joint="index_PIP"/>
<jointpos name="Sjp_index_DIP" joint="index_DIP"/>
<jointpos name="Sjp_middle_MCP" joint="middle_MCP"/>
<jointpos name="Sjp_middle_PIP" joint="middle_PIP"/>
<jointpos name="Sjp_middle_DIP" joint="middle_DIP"/>
<jointpos name="Sjp_ring_ABD" joint="ring_ABD"/>
<jointpos name="Sjp_ring_MCP" joint="ring_MCP"/>
<jointpos name="Sjp_ring_PIP" joint="ring_PIP"/>
<jointpos name="Sjp_ring_DIP" joint="ring_DIP"/>
<jointpos name="Sjp_pinky_ABD" joint="pinky_ABD"/>
<jointpos name="Sjp_pinky_MCP" joint="pinky_MCP"/>
<jointpos name="Sjp_pinky_PIP" joint="pinky_PIP"/>
<jointpos name="Sjp_pinky_DIP" joint="pinky_DIP"/>
<jointvel name="Sjv_wrist_PRO" joint="wrist_PRO"/>
<jointvel name="Sjv_wrist_UDEV" joint="wrist_UDEV"/>
<jointvel name="Sjv_wrist_FLEX" joint="wrist_FLEX"/>
<jointvel name="Sjv_thumb_ABD" joint="thumb_ABD"/>
<jointvel name="Sjv_thumb_MCP" joint="thumb_MCP"/>
<jointvel name="Sjv_thumb_PIP" joint="thumb_PIP"/>
<jointvel name="Sjv_thumb_DIP" joint="thumb_DIP"/>
<jointvel name="Sjv_index_ABD" joint="index_ABD"/>
<jointvel name="Sjv_index_MCP" joint="index_MCP"/>
<jointvel name="Sjv_index_PIP" joint="index_PIP"/>
<jointvel name="Sjv_index_DIP" joint="index_DIP"/>
<jointvel name="Sjv_middle_MCP" joint="middle_MCP"/>
<jointvel name="Sjv_middle_PIP" joint="middle_PIP"/>
<jointvel name="Sjv_middle_DIP" joint="middle_DIP"/>
<jointvel name="Sjv_ring_ABD" joint="ring_ABD"/>
<jointvel name="Sjv_ring_MCP" joint="ring_MCP"/>
<jointvel name="Sjv_ring_PIP" joint="ring_PIP"/>
<jointvel name="Sjv_ring_DIP" joint="ring_DIP"/>
<jointvel name="Sjv_pinky_ABD" joint="pinky_ABD"/>
<jointvel name="Sjv_pinky_MCP" joint="pinky_MCP"/>
<jointvel name="Sjv_pinky_PIP" joint="pinky_PIP"/>
<jointvel name="Sjv_pinky_DIP" joint="pinky_DIP"/>
<actuatorpos name="Sap_wrist_PRO" actuator="A_wrist_PRO" />
<actuatorpos name="Sap_wrist_UDEV" actuator="A_wrist_UDEV" />
<actuatorpos name="Sap_wrist_FLEX" actuator="A_wrist_FLEX" />
<actuatorpos name="Sap_thumb_ABD" actuator="A_thumb_ABD" />
<actuatorpos name="Sap_thumb_MCP" actuator="A_thumb_MCP" />
<actuatorpos name="Sap_thumb_PIP" actuator="A_thumb_PIP" />
<actuatorpos name="Sap_thumb_DIP" actuator="A_thumb_DIP" />
<actuatorpos name="Sap_index_ABD" actuator="A_index_ABD" />
<actuatorpos name="Sap_index_MCP" actuator="A_index_MCP" />
<actuatorpos name="Sap_middle_MCP" actuator="A_middle_MCP"/>
<actuatorpos name="Sap_ring_MCP" actuator="A_ring_MCP" />
<actuatorpos name="Sap_pinky_ABD" actuator="A_pinky_ABD" />
<actuatorpos name="Sap_pinky_MCP" actuator="A_pinky_MCP" />
<actuatorvel name="Sav_wrist_PRO" actuator="A_wrist_PRO" />
<actuatorvel name="Sav_wrist_UDEV" actuator="A_wrist_UDEV"/>
<actuatorvel name="Sav_wrist_FLEX" actuator="A_wrist_FLEX"/>
<actuatorvel name="Sav_thumb_ABD" actuator="A_thumb_ABD" />
<actuatorvel name="Sav_thumb_MCP" actuator="A_thumb_MCP" />
<actuatorvel name="Sav_thumb_PIP" actuator="A_thumb_PIP" />
<actuatorvel name="Sav_thumb_DIP" actuator="A_thumb_DIP" />
<actuatorvel name="Sav_index_ABD" actuator="A_index_ABD" />
<actuatorvel name="Sav_index_MCP" actuator="A_index_MCP" />
<actuatorvel name="Sav_middle_MCP" actuator="A_middle_MCP"/>
<actuatorvel name="Sav_ring_MCP" actuator="A_ring_MCP" />
<actuatorvel name="Sav_pinky_ABD" actuator="A_pinky_ABD" />
<actuatorvel name="Sav_pinky_MCP" actuator="A_pinky_MCP" />
<actuatorfrc name="Saf_wrist_PRO" actuator="A_wrist_PRO"/>
<actuatorfrc name="Saf_wrist_UDEV" actuator="A_wrist_UDEV"/>
<actuatorfrc name="Saf_wrist_FLEX" actuator="A_wrist_FLEX"/>
<actuatorfrc name="Saf_thumb_ABD" actuator="A_thumb_ABD"/>
<actuatorfrc name="Saf_thumb_MCP" actuator="A_thumb_MCP"/>
<actuatorfrc name="Saf_thumb_PIP" actuator="A_thumb_PIP"/>
<actuatorfrc name="Saf_thumb_DIP" actuator="A_thumb_DIP"/>
<actuatorfrc name="Saf_index_ABD" actuator="A_index_ABD"/>
<actuatorfrc name="Saf_index_MCP" actuator="A_index_MCP"/>
<actuatorfrc name="Saf_middle_MCP" actuator="A_middle_MCP"/>
<actuatorfrc name="Saf_ring_MCP" actuator="A_ring_MCP"/>
<actuatorfrc name="Saf_pinky_ABD" actuator="A_pinky_ABD"/>
<actuatorfrc name="Saf_pinky_MCP" actuator="A_pinky_MCP"/>
<accelerometer name="S_thumb_IMU" site="thumb_IMU"/>
<accelerometer name="S_index_IMU" site="index_IMU"/>
<accelerometer name="S_middle_IMU" site="middle_IMU"/>
<accelerometer name="S_ring_IMU" site="ring_IMU"/>
<accelerometer name="S_pinky_IMU" site="pinky_IMU"/>
<gyro site="thumb_IMU"/>
<gyro site="index_IMU"/>
<gyro site="middle_IMU"/>
<gyro site="ring_IMU"/>
<gyro site="pinky_IMU"/>
<touch name="S_palm_thumb" site="palm_thumb"/>
<touch name="S_palm_pinky" site="palm_pinky"/>
<touch name="S_palm_side" site="palm_side"/>
<touch name="S_palm_back" site="palm_back"/>
<touch name="S_thumb_proximal" site="thumb_proximal"/>
<touch name="S_thumb_medial" site="thumb_medial"/>
<touch name="S_thumb_distal" site="thumb_distal"/>
<touch name="S_index_proximal" site="index_proximal"/>
<touch name="S_index_medial" site="index_medial"/>
<touch name="S_index_distal" site="index_distal"/>
<touch name="S_middle_proximal" site="middle_proximal"/>
<touch name="S_middle_medial" site="middle_medial"/>
<touch name="S_middle_distal" site="middle_distal"/>
<touch name="S_ring_proximal" site="ring_proximal"/>
<touch name="S_ring_medial" site="ring_medial"/>
<touch name="S_ring_distal" site="ring_distal"/>
<touch name="S_pinky_proximal" site="pinky_proximal"/>
<touch name="S_pinky_medial" site="pinky_medial"/>
<touch name="S_pinky_distal" site="pinky_distal"/>
</sensor>
<tendon>
<!--Index coupler tendons-->
<fixed name="T_index32_cpl" range="0 1">
<joint joint="index_DIP" coef="0.00705"/>
<joint joint="index_PIP" coef="-0.00805"/>
</fixed>
<fixed name="T_index21_cpl" range="0 1">
<joint joint="index_PIP" coef="0.010"/>
<joint joint="index_MCP" coef="-0.010"/>
</fixed>
<!--Middle coupler tendons-->
<fixed name="T_middle32_cpl">
<joint joint="middle_DIP" coef="0.00705"/>
<joint joint="middle_PIP" coef="-0.00805"/>
</fixed>
<fixed name="T_middle21_cpl">
<joint joint="middle_PIP" coef="0.010"/>
<joint joint="middle_MCP" coef="-0.010"/>
</fixed>
<!--Ring coupler tendons-->
<fixed name="T_ring32_cpl">
<joint joint="ring_DIP" coef="0.00705"/>
<joint joint="ring_PIP" coef="-0.00805"/>
</fixed>
<fixed name="T_ring21_cpl">
<joint joint="ring_PIP" coef="0.010"/>
<joint joint="ring_MCP" coef="-0.010"/>
</fixed>
<!--Little coupler tendons-->
<fixed name="T_pinky32_cpl">
<joint joint="pinky_DIP" coef="0.00705"/>
<joint joint="pinky_PIP" coef="-0.00805"/>
</fixed>
<fixed name="T_pinky21_cpl">
<joint joint="pinky_PIP" coef="0.010"/>
<joint joint="pinky_MCP" coef="-0.010"/>
</fixed>
</tendon>
<equality>
<weld body1="mocap" body2="forearm" solref="0.01 1" solimp=".9 .9 0.01"/>
<!-- DIP-PIP-MCP Couplings -->
<tendon name="E_index32_cpl" tendon1="T_index32_cpl"/>
<tendon name="E_index21_cpl" tendon1="T_index21_cpl"/>
<tendon name="E_middle32_cpl" tendon1="T_middle32_cpl"/>
<tendon name="E_middle21_cpl" tendon1="T_middle21_cpl"/>
<tendon name="E_ring32_cpl" tendon1="T_ring32_cpl"/>
<tendon name="E_ring21_cpl" tendon1="T_ring21_cpl"/>
<tendon name="E_pinky32_cpl" tendon1="T_pinky32_cpl"/>
<tendon name="E_pinky21_cpl" tendon1="T_pinky21_cpl"/>
<!-- AD-AB Coupling -->
<joint name="ring_pinky_cpl" joint1="ring_ABD" joint2="pinky_ABD" polycoef="0 0.5 0 0 0"/>
</equality>
<actuator>
<!-- Wrist -->
<position name="A_wrist_PRO" class="MPL" joint="wrist_PRO" ctrlrange="-1.57 1.57"/>
<position name="A_wrist_UDEV" class="MPL" joint="wrist_UDEV" ctrlrange="-0.26 0.79"/>
<position name="A_wrist_FLEX" class="MPL" joint="wrist_FLEX" ctrlrange="-1 1"/>
<!-- Thumb -->
<position name="A_thumb_ABD" class="MPL" joint="thumb_ABD" ctrlrange="0 2.1"/>
<position name="A_thumb_MCP" class="MPL" joint="thumb_MCP" ctrlrange="0 1.0"/>
<position name="A_thumb_PIP" class="MPL" joint="thumb_PIP" ctrlrange="0 1.0"/>
<position name="A_thumb_DIP" class="MPL" joint="thumb_DIP" ctrlrange="-0.82 1.3"/>
<!-- Fingers -->
<position name="A_index_ABD" class="MPL" joint="index_ABD" ctrlrange="0 0.34"/>
<position name="A_index_MCP" class="MPL" joint="index_MCP" ctrlrange="0 1.6"/>
<position name="A_middle_MCP" class="MPL" joint="middle_MCP" ctrlrange="0 1.6"/>
<position name="A_ring_MCP" class="MPL" joint="ring_MCP" ctrlrange="0 1.6"/>
<position name="A_pinky_ABD" class="MPL" joint="pinky_ABD" ctrlrange="0 0.34"/>
<position name="A_pinky_MCP" class="MPL" joint="pinky_MCP" ctrlrange="0 1.6"/>
</actuator>
</mujoco>

BIN
data/MPL/mesh/index0.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/index1.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/index2.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/index3.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/middle0.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/middle1.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/middle2.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/middle3.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/palm.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/pinky0.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/pinky1.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/pinky2.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/pinky3.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/ring0.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/ring1.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/ring2.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/ring3.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/thumb0.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/thumb1.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/thumb2.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/thumb3.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/wristx.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/wristy.STL Normal file
View File

Binary file not shown.

BIN
data/MPL/mesh/wristz.STL Normal file
View File

Binary file not shown.

BIN
docs/pybullet_quickstartguide.pdf
View File

Binary file not shown.

4
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
View File

@@ -336,9 +336,7 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat) void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat)
{ {
//const char* nodeName = node->Attribute("id");
//printf("processing node %s\n", nodeName);
btMatrix4x4 nodeTrans; btMatrix4x4 nodeTrans;
nodeTrans.setIdentity(); nodeTrans.setIdentity();

279
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

@@ -8,6 +8,10 @@
#include "../ImportURDFDemo/UrdfParser.h" #include "../ImportURDFDemo/UrdfParser.h"
#include "../ImportURDFDemo/urdfStringSplit.h" #include "../ImportURDFDemo/urdfStringSplit.h"
#include "../ImportURDFDemo/urdfLexicalCast.h" #include "../ImportURDFDemo/urdfLexicalCast.h"
#include "../ImportObjDemo/LoadMeshFromObj.h"
#include "../ImportSTLDemo/LoadMeshFromSTL.h"
#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include "../ImportMeshUtility/b3ImportMeshUtility.h"
#include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
@@ -16,9 +20,22 @@
#include "BulletCollision/CollisionShapes/btCapsuleShape.h" #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
#include "BulletCollision/CollisionShapes/btCylinderShape.h" #include "BulletCollision/CollisionShapes/btCylinderShape.h"
#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btTriangleMesh.h"
#include <vector>
enum eMJCF_FILE_TYPE_ENUMS
{
MJCF_FILE_STL = 1,
MJCF_FILE_OBJ = 2
};
enum ePARENT_LINK_ENUMS enum ePARENT_LINK_ENUMS
{ {
BASE_LINK_INDEX=-1, BASE_LINK_INDEX=-1,
@@ -112,6 +129,10 @@ static bool parseVector6(btVector3& v0, btVector3& v1, const std::string& vector
} }
struct MyMJCFAsset
{
std::string m_fileName;
};
struct BulletMJCFImporterInternalData struct BulletMJCFImporterInternalData
{ {
@@ -119,12 +140,20 @@ struct BulletMJCFImporterInternalData
char m_pathPrefix[1024]; char m_pathPrefix[1024];
std::string m_fileModelName; std::string m_fileModelName;
btHashMap<btHashString,MyMJCFAsset> m_assets;
btAlignedObjectArray<UrdfModel*> m_models; btAlignedObjectArray<UrdfModel*> m_models;
//<compiler angle="radian" meshdir="mesh/" texturedir="texture/"/>
std::string m_meshDir;
std::string m_textureDir;
int m_activeModel; int m_activeModel;
//todo: for full MJCF compatibility, we would need a stack of default values //todo: for full MJCF compatibility, we would need a stack of default values
int m_defaultCollisionGroup; int m_defaultCollisionGroup;
int m_defaultCollisionMask; int m_defaultCollisionMask;
btScalar m_defaultCollisionMargin;
//those collision shapes are deleted by caller (todo: make sure this happens!) //those collision shapes are deleted by caller (todo: make sure this happens!)
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes; btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
@@ -132,7 +161,8 @@ struct BulletMJCFImporterInternalData
BulletMJCFImporterInternalData() BulletMJCFImporterInternalData()
:m_activeModel(-1), :m_activeModel(-1),
m_defaultCollisionGroup(1), m_defaultCollisionGroup(1),
m_defaultCollisionMask(1) m_defaultCollisionMask(1),
m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm
{ {
m_pathPrefix[0] = 0; m_pathPrefix[0] = 0;
} }
@@ -151,6 +181,48 @@ struct BulletMJCFImporterInternalData
return 0; return 0;
} }
void parseCompiler(TiXmlElement* root_xml, MJCFErrorLogger* logger)
{
const char* meshDirStr = root_xml->Attribute("meshdir");
if (meshDirStr)
{
m_meshDir = meshDirStr;
}
const char* textureDirStr = root_xml->Attribute("texturedir");
if (textureDirStr)
{
m_textureDir = textureDirStr;
}
#if 0
for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
{
std::string n = child_xml->Value();
}
#endif
}
void parseAssets(TiXmlElement* root_xml, MJCFErrorLogger* logger)
{
// <mesh name="index0" file="index0.stl"/>
for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
{
std::string n = child_xml->Value();
if (n=="mesh")
{
const char* assetNameStr = child_xml->Attribute("name");
const char* fileNameStr = child_xml->Attribute("file");
if (assetNameStr && fileNameStr)
{
btHashString assetName = assetNameStr;
MyMJCFAsset asset;
asset.m_fileName = m_meshDir + fileNameStr;
m_assets.insert(assetName,asset);
}
}
}
}
bool parseDefaults(TiXmlElement* root_xml, MJCFErrorLogger* logger) bool parseDefaults(TiXmlElement* root_xml, MJCFErrorLogger* logger)
{ {
bool handled= false; bool handled= false;
@@ -158,9 +230,14 @@ struct BulletMJCFImporterInternalData
for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement()) for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
{ {
std::string n = child_xml->Value(); std::string n = child_xml->Value();
if (n=="inertial") if (n=="inertial")
{ {
} }
if (n=="asset")
{
parseAssets(child_xml,logger);
}
if (n=="geom") if (n=="geom")
{ {
//contype, conaffinity //contype, conaffinity
@@ -534,6 +611,25 @@ struct BulletMJCFImporterInternalData
} }
} }
} }
if (geomType=="mesh")
{
const char* meshStr = link_xml->Attribute("mesh");
if (meshStr)
{
MyMJCFAsset* assetPtr = m_assets[meshStr];
if (assetPtr)
{
handledGeomType = true;
geom.m_type = URDF_GEOM_MESH;
geom.m_meshFileName = assetPtr->m_fileName;
geom.m_meshScale.setValue(1,1,1);
//todo: parse mesh scale
if (sz)
{
}
}
}
}
#if 0 #if 0
if (geomType == "cylinder") if (geomType == "cylinder")
{ {
@@ -1081,6 +1177,17 @@ bool BulletMJCFImporter::parseMJCFString(const char* xmlText, MJCFErrorLogger* l
{ {
m_data->parseDefaults(link_xml,logger); m_data->parseDefaults(link_xml,logger);
} }
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("compiler"); link_xml; link_xml = link_xml->NextSiblingElement("compiler"))
{
m_data->parseCompiler(link_xml,logger);
}
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("asset"); link_xml; link_xml = link_xml->NextSiblingElement("asset"))
{
m_data->parseAssets(link_xml,logger);
}
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("body"); link_xml; link_xml = link_xml->NextSiblingElement("body")) for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("body"); link_xml; link_xml = link_xml->NextSiblingElement("body"))
{ {
@@ -1281,6 +1388,52 @@ int BulletMJCFImporter::getBodyUniqueId() const
{ {
return 0; return 0;
} }
static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
{
btCompoundShape* compound = new btCompoundShape();
compound->setMargin(collisionMargin);
btTransform identity;
identity.setIdentity();
for (int s = 0; s<(int)shapes.size(); s++)
{
btConvexHullShape* convexHull = new btConvexHullShape();
convexHull->setMargin(collisionMargin);
tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size();
for (int f = 0; f<faceCount; f += 3)
{
btVector3 pt;
pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
convexHull->addPoint(pt*geomScale,false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
convexHull->addPoint(pt*geomScale, false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
convexHull->addPoint(pt*geomScale, false);
}
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();
compound->addChildShape(identity,convexHull);
}
return compound;
}
class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{ {
@@ -1319,6 +1472,129 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
} }
case URDF_GEOM_MESH: case URDF_GEOM_MESH:
{ {
//////////////////////
if (1)
{
if (col->m_geometry.m_meshFileName.length())
{
const char* filename = col->m_geometry.m_meshFileName.c_str();
//b3Printf("mesh->filename=%s\n",filename);
char fullPath[1024];
int fileType = 0;
sprintf(fullPath,"%s%s",pathPrefix,filename);
b3FileUtils::toLower(fullPath);
char tmpPathPrefix[1024];
int maxPathLen = 1024;
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
char collisionPathPrefix[1024];
sprintf(collisionPathPrefix,"%s%s",pathPrefix,tmpPathPrefix);
if (strstr(fullPath,".stl"))
{
fileType = MJCF_FILE_STL;
}
if (strstr(fullPath,".obj"))
{
fileType = MJCF_FILE_OBJ;
}
sprintf(fullPath,"%s%s",pathPrefix,filename);
FILE* f = fopen(fullPath,"rb");
if (f)
{
fclose(f);
GLInstanceGraphicsShape* glmesh = 0;
switch (fileType)
{
case MJCF_FILE_OBJ:
{
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
{
glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
}
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
//create a convex hull for each shape, and store it in a btCompoundShape
childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin);
}
break;
}
case MJCF_FILE_STL:
{
glmesh = LoadMeshFromSTL(fullPath);
break;
}
default:
{
b3Warning("Unsupported file type in Collision: %s\n",fullPath);
}
}
if (!childShape && glmesh && (glmesh->m_numvertices>0))
{
//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
//convex->setUserIndex(shapeId);
btAlignedObjectArray<btVector3> convertedVerts;
convertedVerts.reserve(glmesh->m_numvertices);
for (int i=0;i<glmesh->m_numvertices;i++)
{
convertedVerts.push_back(btVector3(
glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0],
glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1],
glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2]));
}
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
{
btTriangleMesh* meshInterface = new btTriangleMesh();
for (int i=0;i<glmesh->m_numIndices/3;i++)
{
float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
btVector3(v1[0],v1[1],v1[2]),
btVector3(v2[0],v2[1],v2[2]));
}
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
childShape = trimesh;
} else
{
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
convexHull->optimizeConvexHull();
//convexHull->initializePolyhedralFeatures();
convexHull->setMargin(m_data->m_defaultCollisionMargin);
childShape = convexHull;
}
} else
{
b3Warning("issue extracting mesh from STL file %s\n", fullPath);
}
delete glmesh;
} else
{
b3Warning("mesh geometry not found %s\n",fullPath);
}
}
}
//////////////////////
break; break;
} }
@@ -1349,6 +1625,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
} }
default: default:
{ {
} }
} }
if (childShape) if (childShape)

1
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
View File

@@ -119,6 +119,7 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option,
if (gMCFJFileNameArray.size()==0) if (gMCFJFileNameArray.size()==0)
{ {
gMCFJFileNameArray.push_back("MPL/MPL.xml");
gMCFJFileNameArray.push_back("mjcf/humanoid.xml"); gMCFJFileNameArray.push_back("mjcf/humanoid.xml");
gMCFJFileNameArray.push_back("mjcf/inverted_pendulum.xml"); gMCFJFileNameArray.push_back("mjcf/inverted_pendulum.xml");
gMCFJFileNameArray.push_back("mjcf/ant.xml"); gMCFJFileNameArray.push_back("mjcf/ant.xml");

97
examples/OpenGLWindow/MacOpenGLWindow.mm
View File

@@ -10,6 +10,61 @@
#include <stddef.h> #include <stddef.h>
#include <string.h> #include <string.h>
#include <Availability.h>
//aargh, Mac OSX 10.12 broke backwards compatibility, fix it here for now
#ifdef __MAC_10_12
#define MyNSTitledWindowMask NSWindowStyleMaskTitled
#define MyNSResizableWindowMask NSWindowStyleMaskResizable
#define MyNSClosableWindowMask NSWindowStyleMaskClosable
#define MyNSMiniaturizableWindowMask NSWindowStyleMaskMiniaturizable
#define MyNSAnyEventMask NSEventMaskAny
#define MyNSEventTypeFlagsChanged NSEventTypeFlagsChanged
#define MyNSEventModifierFlagShift NSEventModifierFlagShift
#define MyNSEventModifierFlagControl NSEventModifierFlagControl
#define MyNSEventModifierFlagOption NSEventModifierFlagOption
#define MyNSKeyUp NSEventTypeKeyUp
#define MyNSKeyDown NSEventTypeKeyDown
#define MyNSRightMouseDown NSEventTypeRightMouseDown
#define MyNSLeftMouseDown NSEventTypeLeftMouseDown
#define MyNSOtherMouseDown NSEventTypeOtherMouseDown
#define MyNSRightMouseUp NSEventTypeRightMouseUp
#define MyNSLeftMouseUp NSEventTypeLeftMouseUp
#define MyNSOtherMouseUp NSEventTypeOtherMouseUp
#define MyNSMouseMoved NSEventTypeMouseMoved
#define MyNSLeftMouseDragged NSEventTypeLeftMouseDragged
#define MyNSRightMouseDragged NSEventTypeRightMouseDragged
#define MyNSOtherMouseDragged NSEventTypeOtherMouseDragged
#define MyNSScrollWheel NSEventTypeScrollWheel
#else
#define MyNSTitledWindowMask NSTitledWindowMask
#define MyNSResizableWindowMask NSResizableWindowMask
#define MyNSClosableWindowMask NSClosableWindowMask
#define MyNSMiniaturizableWindowMask NSMiniaturizableWindowMask
#define MyNSAnyEventMask NSAnyEventMask
#define MyNSEventTypeFlagsChanged NSFlagsChanged
#define MyNSEventModifierFlagShift NSShiftKeyMask
#define MyNSEventModifierFlagControl NSControlKeyMask
#define MyNSEventModifierFlagOption NSAlternateKeyMask
#define MyNSKeyUp NSKeyUp
#define MyNSKeyDown NSKeyDown
#define MyNSRightMouseDown NSRightMouseDown
#define MyNSLeftMouseDown NSLeftMouseDown
#define MyNSOtherMouseDown NSOtherMouseDown
#define MyNSLeftMouseUp NSLeftMouseUp
#define MyNSRightMouseUp NSRightMouseUp
#define MyNSOtherMouseUp NSOtherMouseUp
#define MyNSMouseMoved NSMouseMoved
#define MyNSLeftMouseDragged NSLeftMouseDragged
#define MyNSRightMouseDragged NSRightMouseDragged
#define MyNSOtherMouseDragged NSOtherMouseDragged
#define MyNSScrollWheel NSScrollWheel
#endif
enum enum
{ {
@@ -368,7 +423,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
m_internalData->m_window = [NSWindow alloc]; m_internalData->m_window = [NSWindow alloc];
[m_internalData->m_window initWithContentRect:frame [m_internalData->m_window initWithContentRect:frame
styleMask:NSTitledWindowMask |NSResizableWindowMask| NSClosableWindowMask | NSMiniaturizableWindowMask styleMask:MyNSTitledWindowMask |MyNSResizableWindowMask| MyNSClosableWindowMask | MyNSMiniaturizableWindowMask
backing:NSBackingStoreBuffered backing:NSBackingStoreBuffered
defer:false]; defer:false];
@@ -771,7 +826,7 @@ void MacOpenGLWindow::startRendering()
[pool release]; [pool release];
pool = [[NSAutoreleasePool alloc] init]; pool = [[NSAutoreleasePool alloc] init];
event = [m_internalData->m_myApp event = [m_internalData->m_myApp
nextEventMatchingMask:NSAnyEventMask nextEventMatchingMask:MyNSAnyEventMask
untilDate:[NSDate distantPast] untilDate:[NSDate distantPast]
inMode:NSDefaultRunLoopMode inMode:NSDefaultRunLoopMode
// inMode:NSEventTrackingRunLoopMode // inMode:NSEventTrackingRunLoopMode
@@ -780,12 +835,12 @@ void MacOpenGLWindow::startRendering()
//NSShiftKeyMask = 1 << 17, //NSShiftKeyMask = 1 << 17,
//NSControlKeyMask //NSControlKeyMask
if ([event type] == NSFlagsChanged) if ([event type] == MyNSEventTypeFlagsChanged)
{ {
int modifiers = [event modifierFlags]; int modifiers = [event modifierFlags];
if (m_keyboardCallback) if (m_keyboardCallback)
{ {
if ((modifiers & NSShiftKeyMask)) if ((modifiers & MyNSEventModifierFlagShift))
{ {
m_keyboardCallback(B3G_SHIFT,1); m_keyboardCallback(B3G_SHIFT,1);
m_modifierFlags |= MY_MAC_SHIFTKEY; m_modifierFlags |= MY_MAC_SHIFTKEY;
@@ -797,7 +852,7 @@ void MacOpenGLWindow::startRendering()
m_modifierFlags &= ~MY_MAC_SHIFTKEY; m_modifierFlags &= ~MY_MAC_SHIFTKEY;
} }
} }
if (modifiers & NSControlKeyMask) if (modifiers & MyNSEventModifierFlagControl)
{ {
m_keyboardCallback(B3G_CONTROL,1); m_keyboardCallback(B3G_CONTROL,1);
m_modifierFlags |= MY_MAC_CONTROL_KEY; m_modifierFlags |= MY_MAC_CONTROL_KEY;
@@ -809,7 +864,7 @@ void MacOpenGLWindow::startRendering()
m_modifierFlags &= ~MY_MAC_CONTROL_KEY; m_modifierFlags &= ~MY_MAC_CONTROL_KEY;
} }
} }
if (modifiers & NSAlternateKeyMask) if (modifiers & MyNSEventModifierFlagOption)
{ {
m_keyboardCallback(B3G_ALT,1); m_keyboardCallback(B3G_ALT,1);
m_modifierFlags |= MY_MAC_ALTKEY; m_modifierFlags |= MY_MAC_ALTKEY;
@@ -826,7 +881,7 @@ void MacOpenGLWindow::startRendering()
} }
} }
if ([event type] == NSKeyUp) if ([event type] == MyNSKeyUp)
{ {
handledEvent = true; handledEvent = true;
@@ -841,7 +896,9 @@ void MacOpenGLWindow::startRendering()
m_keyboardCallback(keycode,state); m_keyboardCallback(keycode,state);
} }
} }
if ([event type] == NSKeyDown)
if ([event type] == MyNSKeyDown)
{ {
handledEvent = true; handledEvent = true;
@@ -861,7 +918,8 @@ void MacOpenGLWindow::startRendering()
} }
if (([event type]== NSRightMouseDown) || ([ event type]==NSLeftMouseDown)||([event type]==NSOtherMouseDown))
if (([event type]== MyNSRightMouseDown) || ([ event type]==MyNSLeftMouseDown)||([event type]==MyNSOtherMouseDown))
{ {
// printf("right mouse!"); // printf("right mouse!");
// float mouseX,mouseY; // float mouseX,mouseY;
@@ -873,17 +931,17 @@ void MacOpenGLWindow::startRendering()
int button=0; int button=0;
switch ([event type]) switch ([event type])
{ {
case NSLeftMouseDown: case MyNSLeftMouseDown:
{ {
button=0; button=0;
break; break;
} }
case NSOtherMouseDown: case MyNSOtherMouseDown:
{ {
button=1; button=1;
break; break;
} }
case NSRightMouseDown: case MyNSRightMouseDown:
{ {
button=2; button=2;
break; break;
@@ -902,7 +960,7 @@ void MacOpenGLWindow::startRendering()
} }
if (([event type]== NSRightMouseUp) || ([ event type]==NSLeftMouseUp)||([event type]==NSOtherMouseUp)) if (([event type]== MyNSRightMouseUp) || ([ event type]==MyNSLeftMouseUp)||([event type]==MyNSOtherMouseUp))
{ {
// printf("right mouse!"); // printf("right mouse!");
// float mouseX,mouseY; // float mouseX,mouseY;
@@ -915,17 +973,17 @@ void MacOpenGLWindow::startRendering()
int button=0; int button=0;
switch ([event type]) switch ([event type])
{ {
case NSLeftMouseUp: case MyNSLeftMouseUp:
{ {
button=0; button=0;
break; break;
} }
case NSOtherMouseUp: case MyNSOtherMouseUp:
{ {
button=1; button=1;
break; break;
} }
case NSRightMouseUp: case MyNSRightMouseUp:
{ {
button=2; button=2;
break; break;
@@ -943,7 +1001,7 @@ void MacOpenGLWindow::startRendering()
} }
if ([event type] == NSMouseMoved) if ([event type] == MyNSMouseMoved)
{ {
NSPoint eventLocation = [event locationInWindow]; NSPoint eventLocation = [event locationInWindow];
@@ -960,7 +1018,8 @@ void MacOpenGLWindow::startRendering()
} }
} }
if (([event type] == NSLeftMouseDragged) || ([event type] == NSRightMouseDragged) || ([event type] == NSOtherMouseDragged))
if (([event type] == MyNSLeftMouseDragged) || ([event type] == MyNSRightMouseDragged) || ([event type] == MyNSOtherMouseDragged))
{ {
int dx1, dy1; int dx1, dy1;
CGGetLastMouseDelta (&dx1, &dy1); CGGetLastMouseDelta (&dx1, &dy1);
@@ -979,7 +1038,7 @@ void MacOpenGLWindow::startRendering()
// printf("mouse coord = %f, %f\n",m_mouseX,m_mouseY); // printf("mouse coord = %f, %f\n",m_mouseX,m_mouseY);
} }
if ([event type] == NSScrollWheel) if ([event type] == MyNSScrollWheel)
{ {
float dy, dx; float dy, dx;
dy = [ event deltaY ]; dy = [ event deltaY ];

4
examples/SharedMemory/PhysicsClient.h
View File

@@ -34,6 +34,10 @@ public:
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0; virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0;
virtual int getNumUserConstraints() const = 0;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const = 0;
virtual void setSharedMemoryKey(int key) = 0; virtual void setSharedMemoryKey(int key) = 0;
virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0; virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0;

121
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -455,10 +455,13 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value; b3Assert(dofIndex>=0);
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP; if (dofIndex>=0)
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP; {
command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP;
}
return 0; return 0;
} }
@@ -466,10 +469,13 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value; b3Assert(dofIndex>=0);
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD; if (dofIndex>=0)
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD; {
command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD;
}
return 0; return 0;
} }
@@ -477,10 +483,13 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value; b3Assert(dofIndex>=0);
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT; if (dofIndex>=0)
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT; {
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT;
}
return 0; return 0;
} }
@@ -489,10 +498,13 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; b3Assert(dofIndex>=0);
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; if (dofIndex>=0)
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE; {
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
}
return 0; return 0;
} }
@@ -500,10 +512,13 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; b3Assert(dofIndex>=0);
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; if (dofIndex>=0)
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE; {
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
}
return 0; return 0;
} }
@@ -520,7 +535,7 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl
return (b3SharedMemoryCommandHandle) command; return (b3SharedMemoryCommandHandle) command;
} }
void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state) int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state)
{ {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status); b3Assert(status);
@@ -529,23 +544,32 @@ void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandl
if (bodyIndex>=0) if (bodyIndex>=0)
{ {
b3JointInfo info; b3JointInfo info;
b3GetJointInfo(physClient, bodyIndex,jointIndex, &info); bool result = b3GetJointInfo(physClient, bodyIndex,jointIndex, &info);
state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex]; if (result)
state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex]; {
for (int ii(0); ii < 6; ++ii) { state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii]; state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
for (int ii(0); ii < 6; ++ii) {
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
}
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
return 1;
} }
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
} }
return 0;
} }
void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState *state) int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState *state)
{ {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status); b3Assert(status);
int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId; int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
b3Assert(bodyIndex>=0); b3Assert(bodyIndex>=0);
if (bodyIndex>=0) b3Assert(linkIndex >= 0);
int numJoints = b3GetNumJoints(physClient,bodyIndex);
b3Assert(linkIndex < numJoints);
if ((bodyIndex>=0) && (linkIndex >= 0) && linkIndex < numJoints)
{ {
b3Transform wlf,com,inertial; b3Transform wlf,com,inertial;
@@ -575,7 +599,9 @@ void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
{ {
state->m_worldLinkFrameOrientation[i] = wlfOrn[i]; state->m_worldLinkFrameOrientation[i] = wlfOrn[i];
} }
return 1;
} }
return 0;
} }
b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient) b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient)
@@ -1019,6 +1045,31 @@ int b3GetNumBodies(b3PhysicsClientHandle physClient)
return cl->getNumBodies(); return cl->getNumBodies();
} }
int b3GetNumUserConstraints(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getNumUserConstraints();
}
int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* infoPtr)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3UserConstraint constraintInfo1;
b3Assert(physClient);
b3Assert(infoPtr);
b3Assert(constraintUniqueId>=0);
if (infoPtr==0)
return 0;
if (cl->getUserConstraintInfo(constraintUniqueId, constraintInfo1))
{
*infoPtr = constraintInfo1;
return 1;
}
return 0;
}
/// return the body unique id, given the index in range [0 , b3GetNumBodies() ) /// return the body unique id, given the index in range [0 , b3GetNumBodies() )
int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex) int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex)
{ {
@@ -1244,6 +1295,18 @@ void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastI
} }
///If you re-connected to an existing server, or server changed otherwise, sync the body info
b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_SYNC_BODY_INFO;
return (b3SharedMemoryCommandHandle) command;
}
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode) b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode)
{ {

17
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -57,6 +57,9 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* actualStateQdot[], const double* actualStateQdot[],
const double* jointReactionForces[]); const double* jointReactionForces[]);
///If you re-connected to an existing server, or server changed otherwise, sync the body info and user constraints etc.
b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
///return the total number of bodies in the simulation ///return the total number of bodies in the simulation
int b3GetNumBodies(b3PhysicsClientHandle physClient); int b3GetNumBodies(b3PhysicsClientHandle physClient);
@@ -73,15 +76,21 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info); int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id
int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle); int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
///change parameters of an existing user constraint
b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]); int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]);
int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]); int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]);
int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce); int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce);
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet ///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode); b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);
@@ -293,8 +302,8 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle c
int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable); int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId); b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state); int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state); int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ, double rayFromWorldY, double rayFromWorldZ,
@@ -317,7 +326,7 @@ void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastI
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient); b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags); void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags); void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
///experiments of robots interacting with non-rigid objects (such as btSoftBody) ///experiments of robots interacting with non-rigid objects (such as btSoftBody)
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient); b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale); int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);

192
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -28,6 +28,7 @@ struct PhysicsClientSharedMemoryInternalData {
SharedMemoryBlock* m_testBlock1; SharedMemoryBlock* m_testBlock1;
btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap; btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap;
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
btAlignedObjectArray<TmpFloat3> m_debugLinesFrom; btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
btAlignedObjectArray<TmpFloat3> m_debugLinesTo; btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
@@ -46,6 +47,8 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<b3RayHitInfo> m_raycastHits; btAlignedObjectArray<b3RayHitInfo> m_raycastHits;
btAlignedObjectArray<int> m_bodyIdsRequestInfo; btAlignedObjectArray<int> m_bodyIdsRequestInfo;
btAlignedObjectArray<int> m_constraintIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus; SharedMemoryStatus m_tempBackupServerStatus;
SharedMemoryStatus m_lastServerStatus; SharedMemoryStatus m_lastServerStatus;
@@ -138,6 +141,22 @@ bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b
return false; return false;
} }
int PhysicsClientSharedMemory::getNumUserConstraints() const
{
return m_data->m_userConstraintInfoMap.size();
}
int PhysicsClientSharedMemory::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const
{
b3UserConstraint* constraintPtr =m_data->m_userConstraintInfoMap[constraintUniqueId];
if (constraintPtr)
{
info = *constraintPtr;
return 1;
}
return 0;
}
PhysicsClientSharedMemory::PhysicsClientSharedMemory() PhysicsClientSharedMemory::PhysicsClientSharedMemory()
{ {
@@ -156,6 +175,8 @@ PhysicsClientSharedMemory::~PhysicsClientSharedMemory() {
if (m_data->m_isConnected) { if (m_data->m_isConnected) {
disconnectSharedMemory(); disconnectSharedMemory();
} }
resetData();
if (m_data->m_ownsSharedMemory) if (m_data->m_ownsSharedMemory)
{ {
delete m_data->m_sharedMemory; delete m_data->m_sharedMemory;
@@ -163,6 +184,34 @@ PhysicsClientSharedMemory::~PhysicsClientSharedMemory() {
delete m_data; delete m_data;
} }
void PhysicsClientSharedMemory::resetData()
{
m_data->m_debugLinesFrom.clear();
m_data->m_debugLinesTo.clear();
m_data->m_debugLinesColor.clear();
for (int i=0;i<m_data->m_bodyJointMap.size();i++)
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
for (int j=0;j<bodyJoints->m_jointInfo.size();j++) {
if (bodyJoints->m_jointInfo[j].m_jointName)
{
free(bodyJoints->m_jointInfo[j].m_jointName);
}
if (bodyJoints->m_jointInfo[j].m_linkName)
{
free(bodyJoints->m_jointInfo[j].m_linkName);
}
}
delete (*bodyJointsPtr);
}
}
m_data->m_bodyJointMap.clear();
m_data->m_userConstraintInfoMap.clear();
}
void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; } void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; }
@@ -398,7 +447,72 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break; break;
} }
case CMD_USER_CONSTRAINT_INFO_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
break;
}
case CMD_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
break;
}
case CMD_REMOVE_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.remove(cid);
break;
}
case CMD_CHANGE_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid];
if (userConstraintPtr)
{
const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs;
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
{
userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0];
userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1];
userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2];
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
{
userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3];
userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4];
userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5];
userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6];
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
{
userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce;
}
}
break;
}
case CMD_USER_CONSTRAINT_FAILED:
{
b3Warning("createConstraint failed");
break;
}
case CMD_REMOVE_USER_CONSTRAINT_FAILED:
{
b3Warning("removeConstraint failed");
break;
}
case CMD_CHANGE_USER_CONSTRAINT_FAILED:
{
b3Warning("changeConstraint failed");
break;
}
case CMD_ACTUAL_STATE_UPDATE_FAILED:
{
b3Warning("request actual state failed");
break;
}
case CMD_BODY_INFO_COMPLETED: case CMD_BODY_INFO_COMPLETED:
{ {
if (m_data->m_verboseOutput) { if (m_data->m_verboseOutput) {
@@ -497,30 +611,8 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_verboseOutput) { if (m_data->m_verboseOutput) {
b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n"); b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
} }
m_data->m_debugLinesFrom.clear(); resetData();
m_data->m_debugLinesTo.clear();
m_data->m_debugLinesColor.clear();
for (int i=0;i<m_data->m_bodyJointMap.size();i++)
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
for (int j=0;j<bodyJoints->m_jointInfo.size();j++) {
if (bodyJoints->m_jointInfo[j].m_jointName)
{
free(bodyJoints->m_jointInfo[j].m_jointName);
}
if (bodyJoints->m_jointInfo[j].m_linkName)
{
free(bodyJoints->m_jointInfo[j].m_linkName);
}
}
delete (*bodyJointsPtr);
}
}
m_data->m_bodyJointMap.clear();
break; break;
} }
case CMD_DEBUG_LINES_COMPLETED: { case CMD_DEBUG_LINES_COMPLETED: {
@@ -802,15 +894,11 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("User debug draw failed"); b3Warning("User debug draw failed");
break; break;
} }
case CMD_USER_CONSTRAINT_COMPLETED:
case CMD_SYNC_BODY_INFO_COMPLETED:
{ {
break; break;
} }
case CMD_USER_CONSTRAINT_FAILED:
{
b3Warning("createConstraint failed");
break;
}
default: { default: {
b3Error("Unknown server status %d\n", serverCmd.m_type); b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0); btAssert(0);
@@ -830,8 +918,14 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
} }
if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED)) if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED))
{ {
int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
for (int i=0;i<numConstraints;i++)
{
int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
m_data->m_constraintIdsRequestInfo.push_back(constraintUid);
}
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
if (numBodies>0) if (numBodies>0)
{ {
@@ -854,6 +948,25 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
} }
} }
if (serverCmd.m_type == CMD_USER_CONSTRAINT_INFO_COMPLETED)
{
if (m_data->m_constraintIdsRequestInfo.size())
{
int cid = m_data->m_constraintIdsRequestInfo[m_data->m_constraintIdsRequestInfo.size()-1];
m_data->m_constraintIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_USER_CONSTRAINT;
command.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
command.m_userConstraintArguments.m_userConstraintUniqueId = cid;
submitClientCommand(command);
return 0;
}
else
{
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
}
}
if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED) if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED)
{ {
//are there any bodies left to be processed? //are there any bodies left to be processed?
@@ -870,7 +983,20 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
return 0; return 0;
} else } else
{ {
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus; if (m_data->m_constraintIdsRequestInfo.size())
{
int cid = m_data->m_constraintIdsRequestInfo[m_data->m_constraintIdsRequestInfo.size()-1];
m_data->m_constraintIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_USER_CONSTRAINT;
command.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
command.m_userConstraintArguments.m_userConstraintUniqueId = cid;
submitClientCommand(command);
return 0;
} else
{
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
}
} }
} }

8
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -12,8 +12,8 @@ class PhysicsClientSharedMemory : public PhysicsClient {
protected: protected:
virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem); virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void resetData();
public: public:
PhysicsClientSharedMemory(); PhysicsClientSharedMemory();
virtual ~PhysicsClientSharedMemory(); virtual ~PhysicsClientSharedMemory();
@@ -44,6 +44,10 @@ public:
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const; virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual void setSharedMemoryKey(int key); virtual void setSharedMemoryKey(int key);
virtual void uploadBulletFileToSharedMemory(const char* data, int len); virtual void uploadBulletFileToSharedMemory(const char* data, int len);

101
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -19,6 +19,8 @@ struct BodyJointInfoCache2
btAlignedObjectArray<b3JointInfo> m_jointInfo; btAlignedObjectArray<b3JointInfo> m_jointInfo;
}; };
struct PhysicsDirectInternalData struct PhysicsDirectInternalData
{ {
DummyGUIHelper m_noGfx; DummyGUIHelper m_noGfx;
@@ -34,6 +36,7 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<TmpFloat3> m_debugLinesColor; btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
btHashMap<btHashInt,BodyJointInfoCache2*> m_bodyJointMap; btHashMap<btHashInt,BodyJointInfoCache2*> m_bodyJointMap;
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
@@ -112,6 +115,7 @@ void PhysicsDirect::resetData()
} }
} }
m_data->m_bodyJointMap.clear(); m_data->m_bodyJointMap.clear();
m_data->m_userConstraintInfoMap.clear();
} }
// return true if connection succesfull, can also check 'isConnected' // return true if connection succesfull, can also check 'isConnected'
@@ -670,12 +674,78 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
break; break;
} }
case CMD_USER_CONSTRAINT_INFO_COMPLETED:
case CMD_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
break;
}
case CMD_REMOVE_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.remove(cid);
break;
}
case CMD_CHANGE_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid];
if (userConstraintPtr)
{
const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs;
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
{
userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0];
userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1];
userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2];
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
{
userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3];
userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4];
userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5];
userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6];
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
{
userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce;
}
}
break;
}
case CMD_SYNC_BODY_INFO_COMPLETED:
case CMD_MJCF_LOADING_COMPLETED: case CMD_MJCF_LOADING_COMPLETED:
case CMD_SDF_LOADING_COMPLETED: case CMD_SDF_LOADING_COMPLETED:
{ {
//we'll stream further info from the physics server //we'll stream further info from the physics server
//so serverCmd will be invalid, make a copy //so serverCmd will be invalid, make a copy
int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
for (int i=0;i<numConstraints;i++)
{
int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
SharedMemoryCommand infoRequestCommand;
infoRequestCommand.m_type = CMD_USER_CONSTRAINT;
infoRequestCommand.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
infoRequestCommand.m_userConstraintArguments.m_userConstraintUniqueId = constraintUid;
SharedMemoryStatus infoStatus;
bool hasStatus = m_data->m_commandProcessor->processCommand(infoRequestCommand, infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int timeout = 1024 * 1024 * 1024;
while ((!hasStatus) && (timeout-- > 0))
{
hasStatus = m_data->m_commandProcessor->receiveStatus(infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
if (hasStatus)
{
int cid = infoStatus.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,infoStatus.m_userConstraintResultArgs);
}
}
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
for (int i = 0; i<numBodies; i++) for (int i = 0; i<numBodies; i++)
@@ -720,10 +790,18 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{ {
break; break;
} }
case CMD_USER_CONSTRAINT_COMPLETED:
case CMD_REMOVE_USER_CONSTRAINT_FAILED:
{ {
b3Warning("removeConstraint failed");
break; break;
} }
case CMD_CHANGE_USER_CONSTRAINT_FAILED:
{
b3Warning("changeConstraint failed");
break;
}
case CMD_USER_CONSTRAINT_FAILED: case CMD_USER_CONSTRAINT_FAILED:
{ {
b3Warning("createConstraint failed"); b3Warning("createConstraint failed");
@@ -764,10 +842,12 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus; m_data->m_hasStatus = hasStatus;
if (hasStatus) /*if (hasStatus)
{ {
postProcessStatus(m_data->m_serverStatus); postProcessStatus(m_data->m_serverStatus);
m_data->m_hasStatus = false;
} }
*/
return hasStatus; return hasStatus;
} }
@@ -776,6 +856,23 @@ int PhysicsDirect::getNumBodies() const
return m_data->m_bodyJointMap.size(); return m_data->m_bodyJointMap.size();
} }
int PhysicsDirect::getNumUserConstraints() const
{
return m_data->m_userConstraintInfoMap.size();
}
int PhysicsDirect::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const
{
b3UserConstraint* constraintPtr =m_data->m_userConstraintInfoMap[constraintUniqueId];
if (constraintPtr)
{
info = *constraintPtr;
return 1;
}
return 0;
}
int PhysicsDirect::getBodyUniqueId(int serialIndex) const int PhysicsDirect::getBodyUniqueId(int serialIndex) const
{ {

6
examples/SharedMemory/PhysicsDirect.h
View File

@@ -63,6 +63,10 @@ public:
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
///todo: move this out of the ///todo: move this out of the
virtual void setSharedMemoryKey(int key); virtual void setSharedMemoryKey(int key);
@@ -86,7 +90,7 @@ public:
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
//those 2 APIs are for internal use for visualization //the following APIs are for internal use for visualization:
virtual bool connect(struct GUIHelperInterface* guiHelper); virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene(); virtual void renderScene();
virtual void debugDraw(int debugDrawMode); virtual void debugDraw(int debugDrawMode);

11
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -99,7 +99,16 @@ bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3Joint
return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info); return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
} }
///todo: move this out of the int PhysicsLoopBack::getNumUserConstraints() const
{
return m_data->m_physicsClient->getNumUserConstraints();
}
int PhysicsLoopBack::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const
{
return m_data->m_physicsClient->getUserConstraintInfo( constraintUniqueId, info);
}
///todo: move this out of the interface
void PhysicsLoopBack::setSharedMemoryKey(int key) void PhysicsLoopBack::setSharedMemoryKey(int key)
{ {
m_data->m_physicsServer->setSharedMemoryKey(key); m_data->m_physicsServer->setSharedMemoryKey(key);

4
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -48,6 +48,10 @@ public:
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const;
///todo: move this out of the ///todo: move this out of the
virtual void setSharedMemoryKey(int key); virtual void setSharedMemoryKey(int key);

391
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -135,6 +135,9 @@ struct InteralUserConstraintData
{ {
btTypedConstraint* m_rbConstraint; btTypedConstraint* m_rbConstraint;
btMultiBodyConstraint* m_mbConstraint; btMultiBodyConstraint* m_mbConstraint;
b3UserConstraint m_userConstraintData;
InteralUserConstraintData() InteralUserConstraintData()
:m_rbConstraint(0), :m_rbConstraint(0),
m_mbConstraint(0) m_mbConstraint(0)
@@ -423,6 +426,12 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<InternalBodyHandle> m_bodyHandles; btAlignedObjectArray<InternalBodyHandle> m_bodyHandles;
int m_numUsedHandles; // number of active handles int m_numUsedHandles; // number of active handles
int m_firstFreeHandle; // free handles list int m_firstFreeHandle; // free handles list
int getNumHandles() const
{
return m_bodyHandles.size();
}
InternalBodyHandle* getHandle(int handle) InternalBodyHandle* getHandle(int handle)
{ {
btAssert(handle>=0); btAssert(handle>=0);
@@ -828,6 +837,9 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
deleteCachedInverseDynamicsBodies(); deleteCachedInverseDynamicsBodies();
deleteCachedInverseKinematicsBodies(); deleteCachedInverseKinematicsBodies();
m_data->m_userConstraints.clear();
m_data->m_saveWorldBodyData.clear();
for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++) for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++)
{ {
delete m_data->m_multiBodyJointFeedbacks[i]; delete m_data->m_multiBodyJointFeedbacks[i];
@@ -1741,6 +1753,34 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
break; break;
} }
case CMD_SYNC_BODY_INFO:
{
int numHandles = m_data->getNumHandles();
int actualNumBodies = 0;
for (int i=0;i<numHandles;i++)
{
InteralBodyData* body = m_data->getHandle(i);
if (body && (body->m_multiBody || body->m_rigidBody))
{
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = i;
}
}
serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies;
int usz = m_data->m_userConstraints.size();
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz;
for (int i=0;i<usz;i++)
{
int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1();
// int uid = m_data->m_userConstraints.getAtIndex(i)->m_userConstraintData.m_userConstraintUniqueId;
serverStatusOut.m_sdfLoadedArgs.m_userConstraintUniqueIds[i] = key;
}
serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED;
hasStatus = true;
break;
}
case CMD_REQUEST_BODY_INFO: case CMD_REQUEST_BODY_INFO:
{ {
const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs; const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
@@ -1762,7 +1802,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
//saveWorld(clientCmd.m_sdfArguments.m_sdfFileName); //saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
int constraintCount = 0;
FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w"); FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w");
if (f) if (f)
{ {
@@ -1877,6 +1917,93 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}; };
} }
//user constraints
{
for (int i=0;i<m_data->m_userConstraints.size();i++)
{
InteralUserConstraintData* ucptr = m_data->m_userConstraints.getAtIndex(i);
b3UserConstraint& uc = ucptr->m_userConstraintData;
int parentBodyIndex=uc.m_parentBodyIndex;
int parentJointIndex=uc.m_parentJointIndex;
int childBodyIndex=uc.m_childBodyIndex;
int childJointIndex=uc.m_childJointIndex;
btVector3 jointAxis(uc.m_jointAxis[0],uc.m_jointAxis[1],uc.m_jointAxis[2]);
btVector3 pivotParent(uc.m_parentFrame[0],uc.m_parentFrame[1],uc.m_parentFrame[2]);
btVector3 pivotChild(uc.m_childFrame[0],uc.m_childFrame[1],uc.m_childFrame[2]);
btQuaternion ornFrameParent(uc.m_parentFrame[3],uc.m_parentFrame[4],uc.m_parentFrame[5],uc.m_parentFrame[6]);
btQuaternion ornFrameChild(uc.m_childFrame[3],uc.m_childFrame[4],uc.m_childFrame[5],uc.m_childFrame[6]);
{
char jointTypeStr[1024]="FIXED";
bool hasKnownJointType = true;
switch (uc.m_jointType)
{
case eRevoluteType:
{
sprintf(jointTypeStr,"p.JOINT_REVOLUTE");
break;
}
case ePrismaticType:
{
sprintf(jointTypeStr,"p.JOINT_PRISMATIC");
break;
}
case eSphericalType:
{
sprintf(jointTypeStr,"p.JOINT_SPHERICAL");
break;
}
case ePlanarType:
{
sprintf(jointTypeStr,"p.JOINT_PLANAR");
break;
}
case eFixedType :
{
sprintf(jointTypeStr,"p.JOINT_FIXED");
break;
}
case ePoint2PointType:
{
sprintf(jointTypeStr,"p.JOINT_POINT2POINT");
break; }
default:
{
hasKnownJointType = false;
b3Warning("unknown constraint type in SAVE_WORLD");
}
};
if (hasKnownJointType)
{
{
sprintf(line,"cid%d = p.createConstraint(%d,%d,%d,%d,%s,[%f,%f,%f],[%f,%f,%f],[%f,%f,%f],[%f,%f,%f,%f],[%f,%f,%f,%f])\n",
constraintCount,
parentBodyIndex,
parentJointIndex,
childBodyIndex,
childJointIndex,
jointTypeStr,
jointAxis[0],jointAxis[1],jointAxis[2],
pivotParent[0],pivotParent[1],pivotParent[2],
pivotChild[0],pivotChild[1],pivotChild[2],
ornFrameParent[0],ornFrameParent[1],ornFrameParent[2],ornFrameParent[3],
ornFrameChild[0],ornFrameChild[1],ornFrameChild[2],ornFrameChild[3]
);
int len = strlen(line);
fwrite(line,len,1,f);
}
{
sprintf(line,"p.changeConstraint(cid%d,maxForce=%f)\n",constraintCount,uc.m_maxAppliedForce);
int len = strlen(line);
fwrite(line,len,1,f);
constraintCount++;
}
}
}
}
}
{ {
btVector3 grav=this->m_data->m_dynamicsWorld->getGravity(); btVector3 grav=this->m_data->m_dynamicsWorld->getGravity();
sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]); sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]);
@@ -1919,6 +2046,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size();
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
for (int i=0;i<maxBodies;i++) for (int i=0;i<maxBodies;i++)
{ {
@@ -3522,126 +3650,159 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd =serverStatusOut; SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED; serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
hasStatus = true; hasStatus = true;
if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_INFO)
{
int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange);
if (userConstraintPtr)
{
serverCmd.m_userConstraintResultArgs = userConstraintPtr->m_userConstraintData;
serverCmd.m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED;
}
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT) if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT)
{ {
btScalar defaultMaxForce = 500.0;
InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
if (parentBody && parentBody->m_multiBody) if (parentBody && parentBody->m_multiBody)
{ {
InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=-1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks())
//also create a constraint with just a single multibody/rigid body without child
//if (childBody)
{ {
btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); //also create a constraint with just a single multibody/rigid body without child
btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6])); //if (childBody)
btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6]));
btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]);
if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
{ {
if (childBody && childBody->m_multiBody) btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]);
btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]);
btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6]));
btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6]));
btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]);
if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
{ {
btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); if (childBody && childBody->m_multiBody)
multibodyFixed->setMaxAppliedImpulse(500.0); {
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed); if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex <childBody->m_multiBody->getNumLinks()))
InteralUserConstraintData userConstraintData; {
userConstraintData.m_mbConstraint = multibodyFixed; btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
int uid = m_data->m_userConstraintUIDGenerator++; multibodyFixed->setMaxAppliedImpulse(defaultMaxForce);
m_data->m_userConstraints.insert(uid,userConstraintData); m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; InteralUserConstraintData userConstraintData;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; userConstraintData.m_mbConstraint = multibodyFixed;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
} }
else else
{ {
btRigidBody* rb = childBody? childBody->m_rigidBody : 0; btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild); btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild);
rigidbodyFixed->setMaxAppliedImpulse(500.0); rigidbodyFixed->setMaxAppliedImpulse(defaultMaxForce);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodyFixed); world->addMultiBodyConstraint(rigidbodyFixed);
InteralUserConstraintData userConstraintData; InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = rigidbodyFixed; userConstraintData.m_mbConstraint = rigidbodyFixed;
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData); serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
} userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
}
else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType)
{
if (childBody && childBody->m_multiBody)
{
btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
multibodySlider->setMaxAppliedImpulse(500.0);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = multibodySlider;
int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
} }
else else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType)
{ {
btRigidBody* rb = childBody? childBody->m_rigidBody : 0; if (childBody && childBody->m_multiBody)
{
btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
multibodySlider->setMaxAppliedImpulse(defaultMaxForce);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = multibodySlider;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
else
{
btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
rigidbodySlider->setMaxAppliedImpulse(500.0); rigidbodySlider->setMaxAppliedImpulse(defaultMaxForce);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodySlider); world->addMultiBodyConstraint(rigidbodySlider);
InteralUserConstraintData userConstraintData; InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = rigidbodySlider; userConstraintData.m_mbConstraint = rigidbodySlider;
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData); serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
} userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; }
} else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType) } else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType)
{
if (childBody && childBody->m_multiBody)
{ {
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild); if (childBody && childBody->m_multiBody)
p2p->setMaxAppliedImpulse(500); {
m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p); btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild);
InteralUserConstraintData userConstraintData; p2p->setMaxAppliedImpulse(defaultMaxForce);
userConstraintData.m_mbConstraint = p2p; m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
int uid = m_data->m_userConstraintUIDGenerator++; InteralUserConstraintData userConstraintData;
m_data->m_userConstraints.insert(uid,userConstraintData); userConstraintData.m_mbConstraint = p2p;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
} serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
else serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
{ userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
btRigidBody* rb = childBody? childBody->m_rigidBody : 0; m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
else
{
btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild); btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild);
p2p->setMaxAppliedImpulse(500); p2p->setMaxAppliedImpulse(defaultMaxForce);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(p2p); world->addMultiBodyConstraint(p2p);
InteralUserConstraintData userConstraintData; InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = p2p; userConstraintData.m_mbConstraint = p2p;
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid,userConstraintData); serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
} userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
} else } else
{ {
b3Warning("unknown constraint type"); b3Warning("unknown constraint type");
} }
}
}
} }
}
} }
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT)
{ {
int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange);
if (userConstraintPtr) if (userConstraintPtr)
{ {
if (userConstraintPtr->m_mbConstraint) if (userConstraintPtr->m_mbConstraint)
@@ -3651,7 +3812,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0], btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0],
clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[1],
clientCmd.m_userConstraintArguments.m_childFrame[2]); clientCmd.m_userConstraintArguments.m_childFrame[2]);
userConstraintPtr->m_userConstraintData.m_childFrame[0] = clientCmd.m_userConstraintArguments.m_childFrame[0];
userConstraintPtr->m_userConstraintData.m_childFrame[1] = clientCmd.m_userConstraintArguments.m_childFrame[1];
userConstraintPtr->m_userConstraintData.m_childFrame[2] = clientCmd.m_userConstraintArguments.m_childFrame[2];
userConstraintPtr->m_mbConstraint->setPivotInB(pivotInB); userConstraintPtr->m_mbConstraint->setPivotInB(pivotInB);
} }
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
@@ -3660,26 +3823,33 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[4],
clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[5],
clientCmd.m_userConstraintArguments.m_childFrame[6]); clientCmd.m_userConstraintArguments.m_childFrame[6]);
userConstraintPtr->m_userConstraintData.m_childFrame[3] = clientCmd.m_userConstraintArguments.m_childFrame[3];
userConstraintPtr->m_userConstraintData.m_childFrame[4] = clientCmd.m_userConstraintArguments.m_childFrame[4];
userConstraintPtr->m_userConstraintData.m_childFrame[5] = clientCmd.m_userConstraintArguments.m_childFrame[5];
userConstraintPtr->m_userConstraintData.m_childFrame[6] = clientCmd.m_userConstraintArguments.m_childFrame[6];
btMatrix3x3 childFrameBasis(childFrameOrn); btMatrix3x3 childFrameBasis(childFrameOrn);
userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis); userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis);
} }
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
{ {
btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime;
userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce;
userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp); userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp);
} }
} }
if (userConstraintPtr->m_rbConstraint) if (userConstraintPtr->m_rbConstraint)
{ {
//todo
} }
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1; serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidChange;
serverCmd.m_updateFlags = clientCmd.m_updateFlags;
serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_COMPLETED;
} }
} }
if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT) if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT)
{ {
serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_FAILED;
int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove);
if (userConstraintPtr) if (userConstraintPtr)
@@ -3694,8 +3864,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
} }
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED;
} }
@@ -3951,7 +4123,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int numRb = importer->getNumRigidBodies(); int numRb = importer->getNumRigidBodies();
serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0;
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
for( int i=0;i<numRb;i++) for( int i=0;i<numRb;i++)
{ {
btCollisionObject* colObj = importer->getRigidBodyByIndex(i); btCollisionObject* colObj = importer->getRigidBodyByIndex(i);
@@ -4021,6 +4194,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size();
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
for (int i=0;i<maxBodies;i++) for (int i=0;i<maxBodies;i++)
{ {
@@ -4367,7 +4541,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
} }
btVector3 gVRGripperPos(0.6, 0.4, 0.7); btVector3 gVRGripperPos(0.7, 0.3, 0.7);
btQuaternion gVRGripperOrn(0,0,0,1); btQuaternion gVRGripperOrn(0,0,0,1);
btVector3 gVRController2Pos(0,0,0.2); btVector3 gVRController2Pos(0,0,0.2);
btQuaternion gVRController2Orn(0,0,0,1); btQuaternion gVRController2Orn(0,0,0,1);
@@ -4516,7 +4690,8 @@ void PhysicsServerCommandProcessor::resetSimulation()
m_data->m_gripperRigidbodyFixed = 0; m_data->m_gripperRigidbodyFixed = 0;
} }
//todo: move this to Python/scripting
//todo: move this to Python/scripting (it is almost ready to be removed!)
void PhysicsServerCommandProcessor::createDefaultRobotAssets() void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{ {
static btAlignedObjectArray<char> gBufferServerToClient; static btAlignedObjectArray<char> gBufferServerToClient;
@@ -4764,7 +4939,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_huskyId = bodyId; m_data->m_huskyId = bodyId;
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
} }

6
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -1119,7 +1119,7 @@ public:
if (args.CheckCmdLineFlag("norobotassets")) if (args.CheckCmdLineFlag("norobotassets"))
{ {
// gCreateDefaultRobotAssets = false; gCreateDefaultRobotAssets = false;
} }
@@ -1671,8 +1671,8 @@ void PhysicsServerExample::drawUserDebugLines()
void PhysicsServerExample::renderScene() void PhysicsServerExample::renderScene()
{ {
btTransform vrTrans; btTransform vrTrans;
gVRTeleportPos1 = gVRTeleportPosLocal; //gVRTeleportPos1 = gVRTeleportPosLocal;
gVRTeleportOrn = gVRTeleportOrnLocal; //gVRTeleportOrn = gVRTeleportOrnLocal;
///little VR test to follow/drive Husky vehicle ///little VR test to follow/drive Husky vehicle
if (gVRTrackingObjectUniqueId >= 0) if (gVRTrackingObjectUniqueId >= 0)

32
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -33,7 +33,7 @@
#define MAX_SDF_FILENAME_LENGTH 1024 #define MAX_SDF_FILENAME_LENGTH 1024
#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
#define MAX_SDF_BODIES 500 #define MAX_SDF_BODIES 512
struct TmpFloat3 struct TmpFloat3
{ {
@@ -432,6 +432,8 @@ struct SdfLoadedArgs
{ {
int m_numBodies; int m_numBodies;
int m_bodyUniqueIds[MAX_SDF_BODIES]; int m_bodyUniqueIds[MAX_SDF_BODIES];
int m_numUserConstraints;
int m_userConstraintUniqueIds[MAX_SDF_BODIES];
///@todo(erwincoumans) load cameras, lights etc ///@todo(erwincoumans) load cameras, lights etc
//int m_numCameras; //int m_numCameras;
@@ -541,27 +543,13 @@ enum EnumUserConstraintFlags
USER_CONSTRAINT_CHANGE_PIVOT_IN_B=8, USER_CONSTRAINT_CHANGE_PIVOT_IN_B=8,
USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16, USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16,
USER_CONSTRAINT_CHANGE_MAX_FORCE=32, USER_CONSTRAINT_CHANGE_MAX_FORCE=32,
USER_CONSTRAINT_REQUEST_INFO=64,
}; };
struct UserConstraintArgs
{
int m_parentBodyIndex;
int m_parentJointIndex;
int m_childBodyIndex;
int m_childJointIndex;
double m_parentFrame[7];
double m_childFrame[7];
double m_jointAxis[3];
int m_jointType;
double m_maxAppliedForce;
int m_userConstraintUniqueId;
};
struct UserConstraintResultArgs
{
int m_userConstraintUniqueId;
};
enum EnumUserDebugDrawFlags enum EnumUserDebugDrawFlags
{ {
@@ -659,7 +647,7 @@ struct SharedMemoryCommand
struct ExternalForceArgs m_externalForceArguments; struct ExternalForceArgs m_externalForceArguments;
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments; struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
struct CalculateJacobianArgs m_calculateJacobianArguments; struct CalculateJacobianArgs m_calculateJacobianArguments;
struct UserConstraintArgs m_userConstraintArguments; struct b3UserConstraint m_userConstraintArguments;
struct RequestContactDataArgs m_requestContactPointArguments; struct RequestContactDataArgs m_requestContactPointArguments;
struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs; struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments; struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments;
@@ -708,6 +696,10 @@ struct SharedMemoryStatus
int m_numDataStreamBytes; int m_numDataStreamBytes;
char* m_dataStream; char* m_dataStream;
//m_updateFlags is a bit fields to tell which parameters were updated,
//m_updateFlags is ignored for most status messages
int m_updateFlags;
union union
{ {
struct BulletDataStreamArgs m_dataStreamArguments; struct BulletDataStreamArgs m_dataStreamArguments;
@@ -723,7 +715,7 @@ struct SharedMemoryStatus
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs; struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
struct SendVisualShapeDataArgs m_sendVisualShapeArgs; struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
struct UserDebugDrawResultArgs m_userDebugDrawArgs; struct UserDebugDrawResultArgs m_userDebugDrawArgs;
struct UserConstraintResultArgs m_userConstraintResultArgs; struct b3UserConstraint m_userConstraintResultArgs;
struct SendVREvents m_sendVREvents; struct SendVREvents m_sendVREvents;
struct SendRaycastHits m_raycastHits; struct SendRaycastHits m_raycastHits;
}; };

23
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -47,7 +47,7 @@ enum EnumSharedMemoryClientCommand
CMD_USER_DEBUG_DRAW, CMD_USER_DEBUG_DRAW,
CMD_REQUEST_VR_EVENTS_DATA, CMD_REQUEST_VR_EVENTS_DATA,
CMD_SET_VR_CAMERA_STATE, CMD_SET_VR_CAMERA_STATE,
CMD_SYNC_BODY_INFO,
//don't go beyond this command! //don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS, CMD_MAX_CLIENT_COMMANDS,
@@ -112,9 +112,16 @@ enum EnumSharedMemoryServerStatus
CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED, CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED,
CMD_USER_DEBUG_DRAW_FAILED, CMD_USER_DEBUG_DRAW_FAILED,
CMD_USER_CONSTRAINT_COMPLETED, CMD_USER_CONSTRAINT_COMPLETED,
CMD_USER_CONSTRAINT_INFO_COMPLETED,
CMD_REMOVE_USER_CONSTRAINT_COMPLETED,
CMD_CHANGE_USER_CONSTRAINT_COMPLETED,
CMD_REMOVE_USER_CONSTRAINT_FAILED,
CMD_CHANGE_USER_CONSTRAINT_FAILED,
CMD_USER_CONSTRAINT_FAILED, CMD_USER_CONSTRAINT_FAILED,
CMD_REQUEST_VR_EVENTS_DATA_COMPLETED, CMD_REQUEST_VR_EVENTS_DATA_COMPLETED,
CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED, CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED,
CMD_SYNC_BODY_INFO_COMPLETED,
CMD_SYNC_BODY_INFO_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS! //don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS CMD_MAX_SERVER_COMMANDS
}; };
@@ -162,6 +169,20 @@ struct b3JointInfo
double m_jointAxis[3]; // joint axis in parent local frame double m_jointAxis[3]; // joint axis in parent local frame
}; };
struct b3UserConstraint
{
int m_parentBodyIndex;
int m_parentJointIndex;
int m_childBodyIndex;
int m_childJointIndex;
double m_parentFrame[7];
double m_childFrame[7];
double m_jointAxis[3];
int m_jointType;
double m_maxAppliedForce;
int m_userConstraintUniqueId;
};
struct b3BodyInfo struct b3BodyInfo
{ {
const char* m_baseName; const char* m_baseName;

3
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@@ -2246,12 +2246,13 @@ int main(int argc, char *argv[])
if (sExample) if (sExample)
{ {
//until we have a proper VR gui, always assume we want the hard-coded default robot assets //until we have a proper VR gui, always assume we want the hard-coded default robot assets
#if 0
char* newargv[2]; char* newargv[2];
char* t0 = (char*)"--robotassets"; char* t0 = (char*)"--robotassets";
newargv[0] = t0; newargv[0] = t0;
newargv[1] = t0; newargv[1] = t0;
sExample->processCommandLineArgs(2,newargv); sExample->processCommandLineArgs(2,newargv);
#endif
sExample->processCommandLineArgs(argc,argv); sExample->processCommandLineArgs(argc,argv);
} }

32
examples/pybullet/hand.ino Normal file
View File

@@ -0,0 +1,32 @@
//arduino script for vr glove, sending analogue 'finger' readings
//to be used with pybullet and hand.py
int sensorPin0 = A0;
int sensorPin1 = A1;
int sensorPin2 = A2;
int sensorPin3 = A3;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
}
void loop() {
// put your main code here, to run repeatedly:
int sensorValue0 = analogRead(sensorPin0);
int sensorValue1 = analogRead(sensorPin1);
int sensorValue2 = analogRead(sensorPin2);
int sensorValue3 = analogRead(sensorPin3);
Serial.print(",");
Serial.print(sensorValue0);
Serial.print(",");
Serial.print(sensorValue1);
Serial.print(",");
Serial.print(sensorValue2);
Serial.print(",");
Serial.print(sensorValue3);
Serial.println(",");
delay(10);
}

78
examples/pybullet/hand.py Normal file
View File

@@ -0,0 +1,78 @@
#script to control a simulated robot hand using a VR glove
#see https://twitter.com/erwincoumans/status/821953216271106048
#and https://www.youtube.com/watch?v=I6s37aBXbV8
#vr glove was custom build using Spectra Symbolflex sensors (4.5")
#inside a Under Armour Batting Glove, using DFRobot Bluno BLE/Beetle
#with BLE Link to receive serial (for wireless bluetooth serial)
import serial
import time
import pybullet as p
#first try to connect to shared memory (VR), if it fails use local GUI
c = p.connect(p.SHARED_MEMORY)
print(c)
if (c<0):
p.connect(p.GUI)
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/mpl.xml")
hand=objects[0]
#clamp in range 400-600
minV = 400
maxV = 600
p.setRealTimeSimulation(1)
def convertSensor(x):
v = minV
try:
v = float(x)
except ValueError:
v = minV
if (v<minV):
v=minV
if (v>maxV):
v=maxV
b = (v-minV)/float(maxV-minV)
return (1.0-b)
ser = serial.Serial(port='COM13',baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
if (ser.isOpen()):
while True:
while ser.inWaiting() > 0:
line = str(ser.readline())
words = line.split(",")
if (len(words)==6):
middle = convertSensor(words[1])
pink = convertSensor(words[2])
index = convertSensor(words[3])
thumb = convertSensor(words[4])
p.setJointMotorControl2(hand,7,p.POSITION_CONTROL,thumb)
p.setJointMotorControl2(hand,9,p.POSITION_CONTROL,thumb)
p.setJointMotorControl2(hand,11,p.POSITION_CONTROL,thumb)
p.setJointMotorControl2(hand,13,p.POSITION_CONTROL,thumb)
p.setJointMotorControl2(hand,17,p.POSITION_CONTROL,index)
p.setJointMotorControl2(hand,19,p.POSITION_CONTROL,index)
p.setJointMotorControl2(hand,21,p.POSITION_CONTROL,index)
p.setJointMotorControl2(hand,24,p.POSITION_CONTROL,middle)
p.setJointMotorControl2(hand,26,p.POSITION_CONTROL,middle)
p.setJointMotorControl2(hand,28,p.POSITION_CONTROL,middle)
p.setJointMotorControl2(hand,40,p.POSITION_CONTROL,pink)
p.setJointMotorControl2(hand,42,p.POSITION_CONTROL,pink)
p.setJointMotorControl2(hand,44,p.POSITION_CONTROL,pink)
ringpos = 0.5*(pink+middle)
p.setJointMotorControl2(hand,32,p.POSITION_CONTROL,ringpos)
p.setJointMotorControl2(hand,34,p.POSITION_CONTROL,ringpos)
p.setJointMotorControl2(hand,36,p.POSITION_CONTROL,ringpos)
#print(middle)
#print(pink)
#print(index)
#print(thumb)

288
examples/pybullet/pybullet.c
View File

@@ -31,6 +31,7 @@ static PyObject* SpamError;
#define MAX_PHYSICS_CLIENTS 1024 #define MAX_PHYSICS_CLIENTS 1024
static b3PhysicsClientHandle sPhysicsClients1[MAX_PHYSICS_CLIENTS] = {0}; static b3PhysicsClientHandle sPhysicsClients1[MAX_PHYSICS_CLIENTS] = {0};
static int sPhysicsClientsGUI[MAX_PHYSICS_CLIENTS] = {0};
static int sNumPhysicsClients=0; static int sNumPhysicsClients=0;
b3PhysicsClientHandle getPhysicsClient(int physicsClientId) b3PhysicsClientHandle getPhysicsClient(int physicsClientId)
@@ -50,6 +51,8 @@ b3PhysicsClientHandle getPhysicsClient(int physicsClientId)
//broken connection? //broken connection?
b3DisconnectSharedMemory(sm); b3DisconnectSharedMemory(sm);
sPhysicsClients1[physicsClientId] = 0; sPhysicsClients1[physicsClientId] = 0;
sPhysicsClientsGUI[physicsClientId] = 0;
sNumPhysicsClients--; sNumPhysicsClients--;
} }
return 0; return 0;
@@ -209,6 +212,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
int freeIndex = -1; int freeIndex = -1;
int method = eCONNECT_GUI;
int i; int i;
b3PhysicsClientHandle sm=0; b3PhysicsClientHandle sm=0;
@@ -221,7 +225,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
{ {
int method = eCONNECT_GUI;
int key = SHARED_MEMORY_KEY; int key = SHARED_MEMORY_KEY;
int port = 1234; int port = 1234;
const char* hostName = "localhost"; const char* hostName = "localhost";
@@ -237,6 +241,21 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
} }
} }
//Only one local in-process GUI connection allowed.
if (method == eCONNECT_GUI)
{
int i;
for (i=0;i<MAX_PHYSICS_CLIENTS;i++)
{
if (sPhysicsClientsGUI[i] ==eCONNECT_GUI)
{
PyErr_SetString(SpamError,
"Only one local in-process GUI connection allowed. Use DIRECT connection mode or start a separate GUI physics server (ExampleBrowser, App_SharedMemoryPhysics_GUI, App_SharedMemoryPhysics_VR) and connect over SHARED_MEMORY or UDP instead.");
return NULL;
}
}
}
if (size == 2) if (size == 2)
{ {
if (!PyArg_ParseTuple(args, "ii", &method, &key)) if (!PyArg_ParseTuple(args, "ii", &method, &key))
@@ -315,9 +334,26 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
if (freeIndex>=0) if (freeIndex>=0)
{ {
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
sPhysicsClients1[freeIndex] = sm; sPhysicsClients1[freeIndex] = sm;
sPhysicsClientsGUI[freeIndex] = method;
sNumPhysicsClients++; sNumPhysicsClients++;
command = b3InitSyncBodyInfoCommand(sm);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
#if 0
if (statusType != CMD_BODY_INFO_COMPLETED) {
PyErr_SetString(SpamError, "b3InitSyncBodyInfoCommand failed.");
return NULL;
}
#endif
} }
} }
return PyInt_FromLong(freeIndex); return PyInt_FromLong(freeIndex);
} }
@@ -347,6 +383,7 @@ static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
} }
sPhysicsClients1[physicsClientId] = 0; sPhysicsClients1[physicsClientId] = 0;
sPhysicsClientsGUI[physicsClientId] = 0;
sNumPhysicsClients--; sNumPhysicsClients--;
Py_INCREF(Py_None); Py_INCREF(Py_None);
@@ -1541,6 +1578,7 @@ static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args, PyObject*
} }
} }
static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObject* keywds)
{ {
int physicsClientId = 0; int physicsClientId = 0;
@@ -1613,6 +1651,122 @@ static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject*
} }
static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyObject* keywds)
{
{
int constraintUniqueId= -1;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char *kwlist[] = { "constraintUniqueId", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&constraintUniqueId, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
struct b3UserConstraint constraintInfo;
if (b3GetUserConstraintInfo(sm,constraintUniqueId, &constraintInfo))
{
PyObject* pyListConstraintInfo = PyTuple_New(11);
PyTuple_SetItem(pyListConstraintInfo,0,PyLong_FromLong(constraintInfo.m_parentBodyIndex));
PyTuple_SetItem(pyListConstraintInfo,1,PyLong_FromLong(constraintInfo.m_parentJointIndex));
PyTuple_SetItem(pyListConstraintInfo,2,PyLong_FromLong(constraintInfo.m_childBodyIndex));
PyTuple_SetItem(pyListConstraintInfo,3,PyLong_FromLong(constraintInfo.m_childJointIndex));
PyTuple_SetItem(pyListConstraintInfo,4,PyLong_FromLong(constraintInfo.m_jointType));
{
PyObject* axisObj = PyTuple_New(3);
PyTuple_SetItem(axisObj,0,PyFloat_FromDouble(constraintInfo.m_jointAxis[0]));
PyTuple_SetItem(axisObj,1,PyFloat_FromDouble(constraintInfo.m_jointAxis[1]));
PyTuple_SetItem(axisObj,2,PyFloat_FromDouble(constraintInfo.m_jointAxis[2]));
PyTuple_SetItem(pyListConstraintInfo,5,axisObj);
}
{
PyObject* parentFramePositionObj = PyTuple_New(3);
PyTuple_SetItem(parentFramePositionObj,0,PyFloat_FromDouble(constraintInfo.m_parentFrame[0]));
PyTuple_SetItem(parentFramePositionObj,1,PyFloat_FromDouble(constraintInfo.m_parentFrame[1]));
PyTuple_SetItem(parentFramePositionObj,2,PyFloat_FromDouble(constraintInfo.m_parentFrame[2]));
PyTuple_SetItem(pyListConstraintInfo,6,parentFramePositionObj);
}
{
PyObject* childFramePositionObj = PyTuple_New(3);
PyTuple_SetItem(childFramePositionObj,0,PyFloat_FromDouble(constraintInfo.m_childFrame[0]));
PyTuple_SetItem(childFramePositionObj,1,PyFloat_FromDouble(constraintInfo.m_childFrame[1]));
PyTuple_SetItem(childFramePositionObj,2,PyFloat_FromDouble(constraintInfo.m_childFrame[2]));
PyTuple_SetItem(pyListConstraintInfo,7,childFramePositionObj);
}
{
PyObject* parentFrameOrientationObj = PyTuple_New(4);
PyTuple_SetItem(parentFrameOrientationObj,0,PyFloat_FromDouble(constraintInfo.m_parentFrame[3]));
PyTuple_SetItem(parentFrameOrientationObj,1,PyFloat_FromDouble(constraintInfo.m_parentFrame[4]));
PyTuple_SetItem(parentFrameOrientationObj,2,PyFloat_FromDouble(constraintInfo.m_parentFrame[5]));
PyTuple_SetItem(parentFrameOrientationObj,3,PyFloat_FromDouble(constraintInfo.m_parentFrame[6]));
PyTuple_SetItem(pyListConstraintInfo,8,parentFrameOrientationObj);
}
{
PyObject* childFrameOrientation = PyTuple_New(4);
PyTuple_SetItem(childFrameOrientation,0,PyFloat_FromDouble(constraintInfo.m_childFrame[3]));
PyTuple_SetItem(childFrameOrientation,1,PyFloat_FromDouble(constraintInfo.m_childFrame[4]));
PyTuple_SetItem(childFrameOrientation,2,PyFloat_FromDouble(constraintInfo.m_childFrame[5]));
PyTuple_SetItem(childFrameOrientation,3,PyFloat_FromDouble(constraintInfo.m_childFrame[6]));
PyTuple_SetItem(pyListConstraintInfo,9,childFrameOrientation);
}
PyTuple_SetItem(pyListConstraintInfo,10,PyFloat_FromDouble(constraintInfo.m_maxAppliedForce));
return pyListConstraintInfo;
} else
{
PyErr_SetString(SpamError, "Couldn't get user constraint info");
return NULL;
}
}
}
PyErr_SetString(SpamError, "error in getConstraintInfo.");
return NULL;
}
static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyObject* keywds)
{
int numConstraints = 0;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char *kwlist[] = { "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
numConstraints = b3GetNumUserConstraints(sm);
#if PY_MAJOR_VERSION >= 3
return PyLong_FromLong(numConstraints);
#else
return PyInt_FromLong(numConstraints);
#endif
}
// Return the number of joints in an object based on // Return the number of joints in an object based on
// body index; body index is based on order of sequence // body index; body index is based on order of sequence
// the object is loaded into simulation // the object is loaded into simulation
@@ -1989,24 +2143,29 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args,PyObject*
pyListJointState = PyTuple_New(sensorStateSize); pyListJointState = PyTuple_New(sensorStateSize);
pyListJointForceTorque = PyTuple_New(forceTorqueSize); pyListJointForceTorque = PyTuple_New(forceTorqueSize);
b3GetJointState(sm, status_handle, jointIndex, &sensorState); if (b3GetJointState(sm, status_handle, jointIndex, &sensorState))
{
PyTuple_SetItem(pyListJointState, 0,
PyFloat_FromDouble(sensorState.m_jointPosition));
PyTuple_SetItem(pyListJointState, 1,
PyFloat_FromDouble(sensorState.m_jointVelocity));
PyTuple_SetItem(pyListJointState, 0, for (j = 0; j < forceTorqueSize; j++) {
PyFloat_FromDouble(sensorState.m_jointPosition)); PyTuple_SetItem(pyListJointForceTorque, j,
PyTuple_SetItem(pyListJointState, 1, PyFloat_FromDouble(sensorState.m_jointForceTorque[j]));
PyFloat_FromDouble(sensorState.m_jointVelocity)); }
for (j = 0; j < forceTorqueSize; j++) { PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
PyTuple_SetItem(pyListJointForceTorque, j,
PyFloat_FromDouble(sensorState.m_jointForceTorque[j]));
}
PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque); PyTuple_SetItem(pyListJointState, 3,
PyFloat_FromDouble(sensorState.m_jointMotorTorque));
PyTuple_SetItem(pyListJointState, 3, return pyListJointState;
PyFloat_FromDouble(sensorState.m_jointMotorTorque)); } else
{
return pyListJointState; PyErr_SetString(SpamError, "getJointState failed (2).");
return NULL;
}
} }
} }
@@ -2070,54 +2229,55 @@ b3PhysicsClientHandle sm = 0;
return NULL; return NULL;
} }
b3GetLinkState(sm, status_handle, linkIndex, &linkState); if (b3GetLinkState(sm, status_handle, linkIndex, &linkState))
{
pyLinkStateWorldPosition = PyTuple_New(3);
for (i = 0; i < 3; ++i) {
PyTuple_SetItem(pyLinkStateWorldPosition, i,
PyFloat_FromDouble(linkState.m_worldPosition[i]));
}
pyLinkStateWorldPosition = PyTuple_New(3); pyLinkStateWorldOrientation = PyTuple_New(4);
for (i = 0; i < 3; ++i) { for (i = 0; i < 4; ++i) {
PyTuple_SetItem(pyLinkStateWorldPosition, i, PyTuple_SetItem(pyLinkStateWorldOrientation, i,
PyFloat_FromDouble(linkState.m_worldPosition[i])); PyFloat_FromDouble(linkState.m_worldOrientation[i]));
} }
pyLinkStateWorldOrientation = PyTuple_New(4); pyLinkStateLocalInertialPosition = PyTuple_New(3);
for (i = 0; i < 4; ++i) { for (i = 0; i < 3; ++i) {
PyTuple_SetItem(pyLinkStateWorldOrientation, i, PyTuple_SetItem(pyLinkStateLocalInertialPosition, i,
PyFloat_FromDouble(linkState.m_worldOrientation[i])); PyFloat_FromDouble(linkState.m_localInertialPosition[i]));
} }
pyLinkStateLocalInertialPosition = PyTuple_New(3); pyLinkStateLocalInertialOrientation = PyTuple_New(4);
for (i = 0; i < 3; ++i) { for (i = 0; i < 4; ++i) {
PyTuple_SetItem(pyLinkStateLocalInertialPosition, i, PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i,
PyFloat_FromDouble(linkState.m_localInertialPosition[i])); PyFloat_FromDouble(linkState.m_localInertialOrientation[i]));
} }
pyLinkStateLocalInertialOrientation = PyTuple_New(4); pyLinkStateWorldLinkFramePosition = PyTuple_New(3);
for (i = 0; i < 4; ++i) { for (i = 0; i < 3; ++i) {
PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i, PyTuple_SetItem(pyLinkStateWorldLinkFramePosition , i,
PyFloat_FromDouble(linkState.m_localInertialOrientation[i])); PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i]));
} }
pyLinkStateWorldLinkFramePosition = PyTuple_New(3); pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4);
for (i = 0; i < 3; ++i) { for (i = 0; i < 4; ++i) {
PyTuple_SetItem(pyLinkStateWorldLinkFramePosition , i, PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i,
PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i])); PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i]));
} }
pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4);
for (i = 0; i < 4; ++i) {
PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i,
PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i]));
}
pyLinkState = PyTuple_New(6); pyLinkState = PyTuple_New(6);
PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition); PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition);
PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation); PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation);
PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition); PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition);
PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation); PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation);
PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition ); PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition );
PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation); PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation);
return pyLinkState; return pyLinkState;
}
} }
} }
@@ -2178,14 +2338,10 @@ static PyObject* pybullet_addUserDebugParameter(PyObject* self, PyObject* args,
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
int res = 0;
char* text; char* text;
double posXYZ[3];
double colorRGB[3]={1,1,1};
PyObject* textPositionObj=0;
double rangeMin = 0.f; double rangeMin = 0.f;
double rangeMax = 1.f; double rangeMax = 1.f;
double startValue = 0.f; double startValue = 0.f;
@@ -4499,7 +4655,7 @@ static PyMethodDef SpamMethods[] = {
"Load multibodies from an MJCF file." }, "Load multibodies from an MJCF file." },
{"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS, {"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS,
"Create a constraint between two bodies. Returns a (int) unique id, if successfull." "Create a constraint between two bodies. Returns a (int) unique id, if successfull."
}, },
{"changeConstraint", (PyCFunction)pybullet_changeUserConstraint, METH_VARARGS | METH_KEYWORDS, {"changeConstraint", (PyCFunction)pybullet_changeUserConstraint, METH_VARARGS | METH_KEYWORDS,
@@ -4519,11 +4675,21 @@ static PyMethodDef SpamMethods[] = {
"Get the number of bodies in the simulation."}, "Get the number of bodies in the simulation."},
{"getBodyUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS, {"getBodyUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS,
"Get the unique id of the body, given a integer serial index in range [0.. number of bodies)."}, "getBodyUniqueId is used after connecting to server with existing bodies."
"Get the unique id of the body, given a integer range [0.. number of bodies)."},
{"getBodyInfo",(PyCFunction) pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS, {"getBodyInfo",(PyCFunction) pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS,
"Get the body info, given a body unique id."}, "Get the body info, given a body unique id."},
{"getNumConstraints", (PyCFunction)pybullet_getNumConstraints, METH_VARARGS| METH_KEYWORDS,
"Get the number of user-created constraints in the simulation."},
{"getConstraintInfo",(PyCFunction) pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS,
"Get the user-created constraint info, given a constraint unique id."},
{"getConstraintUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS,
"Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."},
{"getBasePositionAndOrientation",(PyCFunction) pybullet_getBasePositionAndOrientation, {"getBasePositionAndOrientation",(PyCFunction) pybullet_getBasePositionAndOrientation,
METH_VARARGS | METH_KEYWORDS, METH_VARARGS | METH_KEYWORDS,
"Get the world position and orientation of the base of the object. " "Get the world position and orientation of the base of the object. "

7
examples/pybullet/vrEvent.py
View File

@@ -7,10 +7,15 @@ import pybullet as p
CONTROLLER_ID = 0 CONTROLLER_ID = 0
POSITION=1 POSITION=1
ORIENTATION=2
BUTTONS=6 BUTTONS=6
#assume that the VR physics server is already started before #assume that the VR physics server is already started before
p.connect(p.SHARED_MEMORY) c = p.connect(p.SHARED_MEMORY)
print(c)
if (c<0):
p.connect(p.GUI)
p.setInternalSimFlags(0)#don't load default robot assets etc p.setInternalSimFlags(0)#don't load default robot assets etc
p.resetSimulation() p.resetSimulation()
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")

22
examples/pybullet/vr_kuka_pr2_move.py Normal file
View File

@@ -0,0 +1,22 @@
#python script with hardcoded values, assumes that you run the vr_kuka_setup.py first
import pybullet as p
p.connect(p.SHARED_MEMORY)
pr2_gripper = 2
pr2_cid = 1
CONTROLLER_ID = 0
POSITION=1
ORIENTATION=2
BUTTONS=6
while True:
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33]&p.VR_BUTTON_IS_DOWN):
p.changeConstraint(pr2_cid,e[POSITION],e[ORIENTATION], maxForce=50)
#todo
#p.setJointMotorControl2(pr2_gripper,0)

76
examples/pybullet/vr_kuka_setup.py Normal file
View File

@@ -0,0 +1,76 @@
import pybullet as p
p.connect(p.SHARED_MEMORY)
p.resetSimulation()
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)]
pr2_gripper = objects[0]
print ("pr2_gripper=")
print (pr2_gripper)
jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
for jointIndex in range (p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
print ("pr2_cid")
print (pr2_cid)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
kuka = objects[0]
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
for jointIndex in range (p.getNumJoints(kuka)):
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)]
objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
kuka_gripper = objects[0]
print ("kuka gripper=")
print(kuka_gripper)
p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970])
jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ]
for jointIndex in range (p.getNumJoints(kuka_gripper)):
p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex])
kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0])
objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("teddy_vhacd.urdf", 1.050000,-0.500000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("duck_vhacd.urdf", 0.850000,-0.400000,0.900000,0.000000,0.000000,0.707107,0.707107)]
objects = p.loadSDF("kiva_shelf/model.sdf")
ob = objects[0]
p.resetBasePositionAndOrientation(ob,[0.000000,1.000000,1.204500],[0.000000,0.000000,0.000000,1.000000])
objects = [p.loadURDF("teddy_vhacd.urdf", -0.100000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("sphere_small.urdf", -0.100000,0.955006,1.169706,0.633232,-0.000000,-0.000000,0.773962)]
objects = [p.loadURDF("cube_small.urdf", 0.300000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("table_square/table_square.urdf", -1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
ob = objects[0]
jointPositions=[ 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
objects = [p.loadURDF("husky/husky.urdf", 2.000000,-5.000000,1.000000,0.000000,0.000000,0.000000,1.000000)]
ob = objects[0]
jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
p.setGravity(0.000000,0.000000,0.000000)
p.setGravity(0,0,-10)
p.stepSimulation()
p.disconnect()

29
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -430,6 +430,13 @@ const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
{ {
btAssert(i>=-1);
btAssert(i<m_links.size());
if ((i<-1) || (i>=m_links.size()))
{
return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
}
btVector3 result = local_pos; btVector3 result = local_pos;
while (i != -1) { while (i != -1) {
// 'result' is in frame i. transform it to frame parent(i) // 'result' is in frame i. transform it to frame parent(i)
@@ -447,6 +454,13 @@ btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
{ {
btAssert(i>=-1);
btAssert(i<m_links.size());
if ((i<-1) || (i>=m_links.size()))
{
return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
}
if (i == -1) { if (i == -1) {
// world to base // world to base
return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos())); return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
@@ -458,6 +472,14 @@ btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
{ {
btAssert(i>=-1);
btAssert(i<m_links.size());
if ((i<-1) || (i>=m_links.size()))
{
return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
}
btVector3 result = local_dir; btVector3 result = local_dir;
while (i != -1) { while (i != -1) {
result = quatRotate(getParentToLocalRot(i).inverse() , result); result = quatRotate(getParentToLocalRot(i).inverse() , result);
@@ -469,6 +491,13 @@ btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
{ {
btAssert(i>=-1);
btAssert(i<m_links.size());
if ((i<-1) || (i>=m_links.size()))
{
return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
}
if (i == -1) { if (i == -1) {
return quatRotate(getWorldToBaseRot(), world_dir); return quatRotate(getWorldToBaseRot(), world_dir);
} else { } else {

2
src/LinearMath/btScalar.h
View File

@@ -29,7 +29,7 @@ subject to the following restrictions:
#include <float.h> #include <float.h>
/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
#define BT_BULLET_VERSION 285 #define BT_BULLET_VERSION 286
inline int btGetVersion() inline int btGetVersion()
{ {

2
src/LinearMath/btSerializer.h
View File

@@ -505,7 +505,7 @@ public:
buffer[9] = '2'; buffer[9] = '2';
buffer[10] = '8'; buffer[10] = '8';
buffer[11] = '5'; buffer[11] = '6';
} }