From 680475a03881c268328e725edba2448e6eacd6f1 Mon Sep 17 00:00:00 2001 From: Jim Fleming Date: Thu, 26 Nov 2015 13:11:38 -0800 Subject: [PATCH 1/3] Adding muscle setup example --- experiments/actuate/app.py | 44 ++++-- experiments/actuate/main.py | 2 + experiments/babble/app.py | 18 +-- experiments/config.py | 200 +++++++++++++++++++++++++- experiments/muscle/__init__.py | 0 experiments/muscle/app.py | 219 +++++++++++++++++++++++++++++ experiments/muscle/main.py | 49 +++++++ experiments/muscle/muscle.py | 0 experiments/rigid_body_rig.py | 25 +++- experiments/utils/physics_utils.py | 4 +- models/plane.egg | 28 ++++ test.py | 36 +++++ 12 files changed, 594 insertions(+), 31 deletions(-) create mode 100644 experiments/muscle/__init__.py create mode 100644 experiments/muscle/app.py create mode 100644 experiments/muscle/main.py create mode 100644 experiments/muscle/muscle.py create mode 100644 models/plane.egg create mode 100644 test.py diff --git a/experiments/actuate/app.py b/experiments/actuate/app.py index 786c729..2af78c8 100644 --- a/experiments/actuate/app.py +++ b/experiments/actuate/app.py @@ -7,8 +7,9 @@ import sys from direct.showbase.ShowBase import ShowBase +from direct.filter.CommonFilters import CommonFilters -from panda3d.core import VBase4, Vec3, PerspectiveLens, TransformState, ClockObject, DirectionalLight, AmbientLight, BitMask32 +from panda3d.core import VBase4, Vec3, PerspectiveLens, TransformState, ClockObject, DirectionalLight, AmbientLight, BitMask32, AntialiasAttrib, Spotlight, Point3 from panda3d.bullet import BulletWorld, BulletPlaneShape, BulletRigidBodyNode, BulletDebugNode from ..exposed_joint_rig import ExposedJointRig @@ -36,10 +37,18 @@ def __init__(self, args): if not headless: self.setupCamera(width, height, Vec3(200, -200, 0), Vec3(0, 0, 0)) + # self.render.setAntialias(AntialiasAttrib.MAuto) + + # filters = CommonFilters(self.win, self.cam) + # filters.setAmbientOcclusion(numsamples=16, radius=0.01, amount=1.0, strength=2.0, falloff=0.002) + # filters.setAmbientOcclusion(radius=0.01, amount=0.5, strength=0.5) + + self.render.setShaderAuto() + self.world = BulletWorld() - self.world.setGravity(Vec3(0, 0, -9.81 * 10.0)) + self.world.setGravity(Vec3(0, 0, -9.81 * 100.0)) # self.world.setGravity(Vec3(0, 0, 0)) - self.setupDebug() + # self.setupDebug() self.createPlane(Vec3(0, 0, -100)) self.animated_rig = ExposedJointRig('walking', { 'walk': 'walking-animation.egg' }) @@ -55,6 +64,7 @@ def __init__(self, args): self.physical_rig.setCollideMask(BitMask32.bit(1)) self.physical_rig.attachRigidBodies(self.world) self.physical_rig.attachConstraints(self.world) + self.physical_rig.attachCubes(self.loader) self.target_physical_rig = RigidBodyRig() self.target_physical_rig.reparentTo(self.render) @@ -114,32 +124,42 @@ def setupCamera(self, width, height, pos, look): def createLighting(self): light = DirectionalLight('light') - light.setColor(VBase4(0.2, 0.2, 0.2, 1)) + light.setColor(VBase4(0.4, 0.4, 0.4, 1)) + light.setShadowCaster(True) + light.getLens().setNearFar(100.0, 400.0) + light.getLens().setFilmSize(400, 400) + # light.showFrustum() np = self.render.attachNewNode(light) - np.setPos(0, -200, 0) - np.lookAt(0, 0, 0) + np.setPos(100, -100, 200) + np.lookAt(0, 0, -100) self.render.setLight(np) light = AmbientLight('ambient') - light.setColor(VBase4(0.4, 0.4, 0.4, 1)) + light.setColor(VBase4(0.2, 0.2, 0.2, 1)) np = self.render.attachNewNode(light) self.render.setLight(np) def createPlane(self, pos): - rb = BulletRigidBodyNode('Ground') + rb = BulletRigidBodyNode('GroundPlane') rb.addShape(BulletPlaneShape(Vec3(0, 0, 1), 1)) rb.setFriction(1.0) rb.setAnisotropicFriction(1.0) - rb.setRestitution(0.0) + rb.setRestitution(1.0) np = self.render.attachNewNode(rb) np.setPos(pos) np.setCollideMask(BitMask32.bit(0)) + plane = self.loader.loadModel('cube') + plane.setScale(Vec3(100, 100, 1)) + plane.setPos(Vec3(0, 0, -0.5)) + plane.setColor(VBase4(0.8, 0.8, 0.8, 1.0)) + plane.reparentTo(np) + self.world.attachRigidBody(rb) return np @@ -167,9 +187,9 @@ def update(self, task): target_directions = next_joint_positions - joint_positions target_rotations = get_angle_vec(next_joint_rotations - joint_rotations) - X_max = [210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 210.5078125, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 552.7041625976562, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 52.49853515625, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 10.845664978027344, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312, 179.99978637695312] + X_max = [123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0] - Y_max = 10000.0 + Y_max = 200000.0 X = np.concatenate([ joint_positions, @@ -180,7 +200,7 @@ def update(self, task): target_rotations ]) / X_max - Y = self.model.predict(np.array([X])) * Y_max + Y = np.clip(self.model.predict(np.array([X])), -1.0, 1.0) * Y_max self.physical_rig.apply_forces(Y[0]) diff --git a/experiments/actuate/main.py b/experiments/actuate/main.py index 4c35c4d..f7efa50 100644 --- a/experiments/actuate/main.py +++ b/experiments/actuate/main.py @@ -41,6 +41,8 @@ def main(): loadPrcFileData('', 'audio-library-name null') # suppress warning loadPrcFileData('', 'model-path %s' % model_path) loadPrcFileData('', 'bullet-filter-algorithm groups-mask') + # loadPrcFileData('', 'framebuffer-multisample 1') + # loadPrcFileData('', 'multisamples 1') if args['--headless']: loadPrcFileData('', 'window-type none') diff --git a/experiments/babble/app.py b/experiments/babble/app.py index 230f93a..95ee181 100644 --- a/experiments/babble/app.py +++ b/experiments/babble/app.py @@ -36,22 +36,22 @@ def __init__(self, args): self.createLighting() if not headless: - self.setupCamera(width, height, Vec3(200, -200, 0), Vec3(0, 0, 0)) + self.setupCamera(width, height, Vec3(0, -200, 0), Vec3(0, 0, 0)) self.world = BulletWorld() - self.world.setGravity(Vec3(0, 0, -9.81 * 10.0)) + self.world.setGravity(Vec3(0, 0, -9.81 * 100.0)) # self.world.setGravity(Vec3(0, 0, 0)) self.setupDebug() self.createPlane(Vec3(0, 0, -100)) self.animated_rig = ExposedJointRig('walking', { 'walk': 'walking-animation.egg' }) self.animated_rig.reparentTo(self.render) - self.animated_rig.setPos(0, 0, -100) + self.animated_rig.setPos(0, 0, 0) # self.animated_rig.createLines(VBase4(0.5, 0.75, 1.0, 1.0)) self.physical_rig = RigidBodyRig() self.physical_rig.reparentTo(self.render) - self.physical_rig.setPos(0, 0, -100) + self.physical_rig.setPos(0, 0, 0) self.physical_rig.createColliders(self.animated_rig) self.physical_rig.createConstraints() self.physical_rig.setCollideMask(BitMask32.bit(1)) @@ -59,7 +59,8 @@ def __init__(self, args): self.physical_rig.attachConstraints(self.world) self.disableCollisions() - self.setAnimationFrame(0) + # self.setAnimationFrame(0) + self.physical_rig.matchPose(self.animated_rig) self.frame_count = self.animated_rig.getNumFrames('walk') self.babble_count = 10 @@ -160,7 +161,8 @@ def setAnimationFrame(self, frame): def generate(self, count): if count % self.babble_count == 0: - self.setAnimationFrame(int(count / self.babble_count)) + # self.setAnimationFrame(int(count / self.babble_count)) + self.physical_rig.matchPose(self.animated_rig) joint_positions = self.physical_rig.getJointPositions() joint_rotations = self.physical_rig.getJointRotations() @@ -192,8 +194,8 @@ def update(self, task): # if globalClock.getFrameCount() % 100 == 0: # self.setAnimationFrame(globalClock.getFrameCount() / 100) # self.physical_rig.babble() - self.world.doPhysics(globalClock.getDt(), 10, 1.0 / 180.0) - return task.cont + # self.world.doPhysics(globalClock.getDt(), 10, 1.0 / 180.0) + # return task.cont curr_train_count = len(self.X_train) curr_test_count = len(self.X_test) diff --git a/experiments/config.py b/experiments/config.py index d4df92d..33aea36 100644 --- a/experiments/config.py +++ b/experiments/config.py @@ -10,7 +10,16 @@ "Hips": { "mass": 100, + "F_max": 0, + "axis": (1, 0, 0), "joints": { + "Spine": { + "type": "hinge", + "axis_parent": (1, 0, 0), + "axis_child": (1, 0, 0), + "offset_parent": (0, 8.75134, -3.32052), + "offset_child": (0, -8.1274, 0), + }, "LeftUpLeg": { "type": "hinge", "limit": (-200, -90), @@ -30,10 +39,107 @@ }, }, + "Spine": { + "mass": 10, + "F_max": 4000.0, + "axis": (1, 0, 0), + "joints": { + "Spine1": { + "type": "hinge", + "axis_parent": (1, 0, 0), + "axis_child": (1, 0, 0), + }, + } + }, + + # Spine1 Spine2 LPoint3f(0, 13.7007, 0) + # Spine1 Spine2 LVecBase3f(1, 6.85034, 1) LVecBase3f(1, 6.2423, 1) + + "Spine1": { + "mass": 10, + "F_max": 400.0, + "axis": (1, 0, 0), + "joints": { + "Spine2": { + "type": "hinge", + "axis_parent": (1, 0, 0), + "axis_child": (1, 0, 0), + "offset_parent": (0, 6.8503, 0), + "offset_child": (0, 0, 0), + } + } + }, + + # print node.getName(), child_node.getName(), child_node.getPos(child_parent) + # Spine2 RightShoulder LPoint3f(-6.14139, 10.8672, -0.228481) + # Spine2 Neck LPoint3f(-0.0016115, 14.5373, 0.45673) + # Spine2 LeftShoulder LPoint3f(6.14301, 10.8686, -0.228251) + + + # print parent.getName(), child.getName(), extents_parent, extents_child + # Spine2 RightShoulder LVecBase3f(1, 6.2423, 1) LVecBase3f(1, 6.25128, 1) + # Spine2 Neck LVecBase3f(1, 6.2423, 1) LVecBase3f(1, 2.21894, 1) + # Spine2 LeftShoulder LVecBase3f(1, 6.2423, 1) LVecBase3f(1, 6.24931, 1) + + "Spine2": { + "mass": 10, + "F_max": 200.0, + "axis": (1, 0, 0), + "joints": { + "Neck": { + "type": "hinge", + "axis_parent": (1, 0, 0), + "axis_child": (1, 0, 0), + "offset_parent": (-0.0016115, 14.5373, 0.45673), + "offset_child": (0, -2.2189, 0), + }, + "LeftShoulder": { + "type": "hinge", + "axis_parent": (0, 0, 1), + "axis_child": (0, 0, 1), + "offset_parent": (6.14301, 10.8686, -0.228251), + "offset_child": (0, -6.2493, 0), + }, + "RightShoulder": { + "type": "hinge", + "axis_parent": (0, 0, 1), + "axis_child": (0, 0, 1), + "offset_parent": (-6.14139, 10.8672, -0.228481), + "offset_child": (0, -6.2512, 0), + }, + } + }, + + "Neck": { + "mass": 5, + "F_max": 100.0, + "axis": (1, 0, 0), + "joints": { + "Neck1": { + "type": "hinge", + "axis_parent": (1, 0, 0), + "axis_child": (1, 0, 0), + } + } + }, + + "Neck1": { + "mass": 1, + "F_max": 0.0, + "axis": (1, 0, 0), + "joints": { + "Head": { + "type": None + } + } + }, + # Left Leg "LeftUpLeg": { - "mass": 10, + "mass": 30, + "F_max": 200000.0, + "axis": (1, 0, 0), "joints": { "LeftLeg": { "type": "hinge", @@ -44,7 +150,9 @@ } }, "LeftLeg": { - "mass": 10, + "mass": 20, + "F_max": 40000.0, + "axis": (1, 0, 0), "joints": { "LeftFoot": { "type": "hinge", @@ -55,6 +163,9 @@ } }, "LeftFoot": { + "mass": 10, + "F_max": 4000.0, + "axis": (1, 0, 0), "joints": { "LeftToeBase": { "type": None @@ -65,7 +176,9 @@ # Right Leg "RightUpLeg": { - "mass": 10, + "mass": 30, + "F_max": 200000.0, + "axis": (1, 0, 0), "joints": { "RightLeg": { "type": "hinge", @@ -76,7 +189,9 @@ } }, "RightLeg": { - "mass": 10, + "mass": 20, + "F_max": 40000.0, + "axis": (1, 0, 0), "joints": { "RightFoot": { "type": "hinge", @@ -87,10 +202,87 @@ } }, "RightFoot": { + "mass": 10, + "F_max": 4000.0, + "axis": (1, 0, 0), "joints": { "RightToeBase": { "type": None } } + }, + + # Left Arm + + "LeftShoulder": { + "mass": 20, + "F_max": 40000.0, + "axis": (0, 0, 1), + "joints": { + "LeftArm": { + "type": "hinge", + "axis_parent": (0, 0, 1), + "axis_child": (0, 0, 1), + } + } + }, + "LeftArm": { + "mass": 10, + "F_max": 4000.0, + "axis": (0, 0, 1), + "joints": { + "LeftForeArm": { + "type": "hinge", + "axis_parent": (0, 0, 1), + "axis_child": (0, 0, 1), + } + } + }, + "LeftForeArm": { + "mass": 5, + "F_max": 2000.0, + "axis": (0, 0, 1), + "joints": { + "LeftHand": { + "type": None + } + } + }, + + # Right Arm + + "RightShoulder": { + "mass": 20, + "F_max": 40000.0, + "axis": (0, 0, 1), + "joints": { + "RightArm": { + "type": "hinge", + "axis_parent": (0, 0, 1), + "axis_child": (0, 0, 1), + } + } + }, + "RightArm": { + "mass": 10, + "F_max": 4000.0, + "axis": (0, 0, 1), + "joints": { + "RightForeArm": { + "type": "hinge", + "axis_parent": (0, 0, 1), + "axis_child": (0, 0, 1), + } + } + }, + "RightForeArm": { + "mass": 5, + "F_max": 2000.0, + "axis": (0, 0, 1), + "joints": { + "RightHand": { + "type": None + } + } } } diff --git a/experiments/muscle/__init__.py b/experiments/muscle/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/experiments/muscle/app.py b/experiments/muscle/app.py new file mode 100644 index 0000000..514ecbd --- /dev/null +++ b/experiments/muscle/app.py @@ -0,0 +1,219 @@ +from __future__ import division + +import math +import numpy as np +import cPickle as pickle +import random +import sys + +from direct.showbase.ShowBase import ShowBase +from direct.filter.CommonFilters import CommonFilters + +from panda3d.core import VBase4, Vec3, PerspectiveLens, TransformState, ClockObject, DirectionalLight, AmbientLight, BitMask32, AntialiasAttrib, Spotlight, Point3, NodePath, LineSegs +from panda3d.bullet import BulletWorld, BulletPlaneShape, BulletRigidBodyNode, BulletDebugNode, BulletBoxShape, BulletHingeConstraint + +from ..utils.math_utils import get_angle_vec +from ..utils.keras_utils import load_model + +class Muscle: + + def __init__(self, F_max): + self.F_max = F_max + + def drawLineSeg(self, loader, parent, start, end): + lines = LineSegs() + lines.setThickness(5.0) + lines.setColor(VBase4(1, 0.5, 0.5, 1.0)) + lines.moveTo(start) + lines.drawTo(end) + + np = parent.attachNewNode(lines.create()) + np.setDepthWrite(True) + np.setDepthTest(True) + + def drawTestCube(self, loader, parent, pos): + cube = loader.loadModel('cube') + cube.reparentTo(parent) + cube.setPos(pos) + cube.setScale(Vec3(0.1, 0.1, 0.1)) + cube.setColor(VBase4(1, 0.5, 0.5, 1.0)) + + def setupDebug(self, loader): + self.drawTestCube(loader, self.a, self.a_pos) + self.drawTestCube(loader, self.b, self.b_pos) + self.drawTestCube(loader, self.a, self.joint_pos) + + def setA(self, a, a_pos): + self.a = a + self.a_pos = a_pos + + def setB(self, b, b_pos): + self.b = b + self.b_pos = b_pos + + def setJointCenter(self, joint_pos): + self.joint_pos = joint_pos + + def getAttachmentA(self): + return self.a.getTransform().compose(TransformState.makePos(self.a_pos)).getPos() + + def getAttachmentB(self): + return self.b.getTransform().compose(TransformState.makePos(self.b_pos)).getPos() + + def getJointCenter(self): + return self.a.getTransform().compose(TransformState.makePos(self.joint_pos)).getPos() + + def getMomentArm(self): + d = self.getAttachmentB() - self.getAttachmentA() + v = self.getAttachmentA() - self.getJointCenter() + r = v.cross(d) + return r + + def apply(self, u): + F = u * self.F_max + r = self.getMomentArm() + + self.applyTorque(self.a, +F, r) + self.applyTorque(self.b, -F, r) + + def applyTorque(self, np, F, r): + T = r * F + np.node().applyTorque(T) + +class App(ShowBase): + + def __init__(self, args): + ShowBase.__init__(self) + + width = args['--width'] + height = args['--height'] + + globalClock.setMode(ClockObject.MNonRealTime) + globalClock.setDt(0.02) # 20ms per frame + + self.setBackgroundColor(0.9, 0.9, 0.9) + self.createLighting() + self.setupCamera(width, height, Vec3(0, -10, 10), Vec3(0, 0, 0)) + self.render.setShaderAuto() + + self.world = BulletWorld() + self.world.setGravity(Vec3(0, 0, -9.81)) + + # self.setupDebug() + + self.createPlane(Vec3(0, 0, -1), Vec3(10, 10, 1)) + + box_a = self.createBox(1.5, Vec3(2, 0, 1), Vec3(2, 1, 1), VBase4(0.5, 1, 0.5, 1.0)) + box_b = self.createBox(1.0, Vec3(-2, 0, 1), Vec3(2, 1, 1), VBase4(0.5, 0.5, 1, 1.0)) + + constraint = BulletHingeConstraint(box_a.node(), box_b.node(), \ + Vec3(-2, 0, 0), Vec3(2, 0, 0), \ + Vec3(0, 1, 0), Vec3(0, 1, 0)) + self.world.attachConstraint(constraint, linked_collision=True) + + self.muscle = Muscle(30) + self.muscle.setA(box_a, Vec3(-1, 0, 1)) + self.muscle.setB(box_b, Vec3(1, 0, 1)) + self.muscle.setJointCenter(Vec3(-2, 0, 0)) + # self.muscle.setupDebug(self.loader) + + self.disableCollisions() + + self.accept('escape', sys.exit) + self.taskMgr.add(self.update, 'update') + + def disableCollisions(self): + for i in range(32): + self.world.setGroupCollisionFlag(i, i, False) + self.world.setGroupCollisionFlag(0, 1, True) + + def setupDebug(self): + node = BulletDebugNode('Debug') + node.showWireframe(True) + + self.world.setDebugNode(node) + + np = self.render.attachNewNode(node) + np.show() + + def createLens(self, aspect_ratio, fov=60.0, near=1.0, far=1000.0): + lens = PerspectiveLens() + lens.setFov(fov) + lens.setAspectRatio(aspect_ratio) + lens.setNearFar(near, far) + return lens + + def setupCamera(self, width, height, pos, look): + self.cam.setPos(pos) + self.cam.lookAt(look) + self.cam.node().setLens(self.createLens(width / height)) + + def createLighting(self): + light = DirectionalLight('light') + light.setColor(VBase4(0.6, 0.6, 0.6, 1)) + light.setShadowCaster(True, 1024, 1024) + light.getLens().setNearFar(1.0, 40.0) + light.getLens().setFilmSize(40, 40) + # light.showFrustum() + + np = self.render.attachNewNode(light) + np.setPos(10, -10, 20) + np.lookAt(0, 0, 0) + + self.render.setLight(np) + + light = AmbientLight('ambient') + light.setColor(VBase4(0.4, 0.4, 0.4, 1)) + + np = self.render.attachNewNode(light) + + self.render.setLight(np) + + def createBox(self, mass, pos, scale, color): + rb = BulletRigidBodyNode('box') + rb.addShape(BulletBoxShape(scale)) + rb.setMass(mass) + rb.setLinearDamping(0.2) + rb.setAngularDamping(0.9) + rb.setFriction(1.0) + rb.setAnisotropicFriction(1.0) + rb.setRestitution(0.0) + + self.world.attachRigidBody(rb) + + np = self.render.attachNewNode(rb) + np.setPos(pos) + np.setCollideMask(BitMask32.bit(1)) + + cube = self.loader.loadModel('cube') + cube.setScale(scale) + cube.setColor(color) + cube.reparentTo(np) + + return np + + def createPlane(self, pos, scale): + rb = BulletRigidBodyNode('plane') + rb.addShape(BulletPlaneShape(Vec3(0, 0, 1), 1)) + rb.setFriction(1.0) + rb.setAnisotropicFriction(1.0) + rb.setRestitution(1.0) + + self.world.attachRigidBody(rb) + + np = self.render.attachNewNode(rb) + np.setPos(pos) + np.setCollideMask(BitMask32.bit(0)) + + plane = self.loader.loadModel('cube') + plane.setScale(scale) + plane.setPos(Vec3(0, 0, 0)) + plane.setColor(VBase4(0.8, 0.8, 0.8, 1.0)) + plane.reparentTo(np) + + return np + + def update(self, task): + self.muscle.apply(1.0) + self.world.doPhysics(globalClock.getDt(), 10, 1.0 / 180.0) + return task.cont diff --git a/experiments/muscle/main.py b/experiments/muscle/main.py new file mode 100644 index 0000000..57aa5e1 --- /dev/null +++ b/experiments/muscle/main.py @@ -0,0 +1,49 @@ +"""Generate training data for target matching. + +Usage: + main.py [--width=] [--height=] + main.py (-h | --help) + +Options: + -h --help Show this text. + --width= Save path for weights [default: 640]. + --height= Load path for weights [default: 640]. +""" + +import os +import sys +from docopt import docopt +from schema import Schema, Or, Use, SchemaError +from panda3d.core import loadPrcFileData + +from app import App + +def main(): + args = docopt(__doc__) + + schema = Schema({ + '--help': bool, + '--width': Use(int), + '--height': Use(int), + }) + + try: + args = schema.validate(args) + except SchemaError as e: + exit(e) + + model_path = 'models' + + loadPrcFileData('', 'window-title Babble') + loadPrcFileData('', 'win-size %d %d' % (args['--width'], args['--height'])) + loadPrcFileData('', 'audio-library-name null') # suppress warning + loadPrcFileData('', 'model-path %s' % model_path) + loadPrcFileData('', 'bullet-filter-algorithm groups-mask') + # loadPrcFileData('', 'framebuffer-multisample 1') + # loadPrcFileData('', 'multisamples 1') + + app = App(args) + app.run() + +if __name__ == '__main__': + main() diff --git a/experiments/muscle/muscle.py b/experiments/muscle/muscle.py new file mode 100644 index 0000000..e69de29 diff --git a/experiments/rigid_body_rig.py b/experiments/rigid_body_rig.py index 25538d1..7784707 100644 --- a/experiments/rigid_body_rig.py +++ b/experiments/rigid_body_rig.py @@ -2,7 +2,7 @@ import random import numpy as np -from panda3d.core import NodePath, Vec3, RigidBodyCombiner, lookAt, Quat +from panda3d.core import NodePath, Vec3, RigidBodyCombiner, lookAt, Quat, TransformState from pandac.PandaModules import CharacterJoint from panda3d.bullet import BulletBoxShape, BulletRigidBodyNode, BulletSphericalConstraint, BulletHingeConstraint, BulletConeTwistConstraint @@ -13,7 +13,7 @@ from .utils.math_utils import get_angle_vec, random_spherical from .config import joints_config, excluded_joints -F_MAX = 1000.0 +F_MAX = 100.0 class RigidBodyRig: @@ -29,6 +29,20 @@ def createConstraints(self): def clearMasses(self): [collider.node().setMass(0) for collider in self.colliders] + def attachCubes(self, loader): + for collider in self.colliders: + for i in range(collider.node().getNumShapes()): + shape = collider.node().getShape(i) + mat = collider.node().getShapeMat(i) + scale = shape.getHalfExtentsWithMargin() + + transform = TransformState.makeMat(mat) + + cube = loader.loadModel("cube.egg") + cube.setTransform(transform) + cube.setScale(scale) + cube.reparentTo(collider) + def setPos(self, *pos): self.root.setPos(*pos) @@ -36,13 +50,14 @@ def babble(self): F_all = [] for collider in self.colliders: - if collider.getName() == "Hips": + collider_name = collider.getName() + if collider_name == "Hips": continue - F = random.uniform(-1.0, 1.0) * collider.node().getMass() * F_MAX + F = random.uniform(-1.0, 1.0) * joints_config[collider_name].get("F_max", 4000.0) F_all.append(F) - r = Vec3(1, 0, 0) + r = Vec3(*joints_config[collider_name].get("axis", (1, 0, 0))) r_world = collider.getQuat(self.root).xform(r) # TODO: is root necessary? T = r_world * F diff --git a/experiments/utils/physics_utils.py b/experiments/utils/physics_utils.py index e7e525d..3abfa33 100644 --- a/experiments/utils/physics_utils.py +++ b/experiments/utils/physics_utils.py @@ -109,8 +109,8 @@ def create_constraints(root, joint_pairs): if 'limit' in joint_config: low, high = joint_config['limit'] softness = 0.0 - bias = 0.0 - relaxation = 1.0 + bias = 4.0 + relaxation = 0.1 constraint.setLimit(low, high, softness, bias, relaxation) elif joint_config['type'] == 'cone': frame_parent = TransformState.makePosHpr(offset_parent, Vec3(90, 0, 0)) diff --git a/models/plane.egg b/models/plane.egg new file mode 100644 index 0000000..a11124b --- /dev/null +++ b/models/plane.egg @@ -0,0 +1,28 @@ + { Z-up } + Plane { + { + { + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 0.0 0.0 0.0 1.0 + } + } + + Plane { + + 0 {-1.000000 -1.000000 0.000000 + } + 1 {1.000000 -1.000000 0.000000 + } + 2 {1.000000 1.000000 0.000000 + } + 3 {-1.000000 1.000000 0.000000 + }} + + + { + {0.000000 0.000000 1.000000} + { 0 1 2 3 { Plane }} + } + } diff --git a/test.py b/test.py new file mode 100644 index 0000000..aa4b1fe --- /dev/null +++ b/test.py @@ -0,0 +1,36 @@ +from panda3d.core import * +loadPrcFileData("", "prefer-parasite-buffer #f") +import direct.directbase.DirectStart + +# setup scene +scene = loader.loadModel('environment') +scene.reparentTo(render) +panda = loader.loadModel('panda') +panda.reparentTo(render) +panda.setY(30) +#panda.setShaderAuto(BitMask32.allOn() & ~BitMask32.bit(Shader.BitAutoShaderShadow)) + +# setup lights, shadow +light = render.attachNewNode(Spotlight("Spot")) +light.node().setScene(render) +render.setLight(light) +light.setP(-45) +light.setZ(20) +light.node().setShadowCaster(True) +light.node().setAttenuation(Point3(0,0,0)) +light.node().showFrustum() +light.node().getLens().setFov(70) +light.node().getLens().setNearFar(10, 500) +light.node().getLens().setFilmSize(100, 100) + +# the light which is actually used +dlight = render.attachNewNode(DirectionalLight("Directional")) +dlight.node().setScene(render) +render.setLight(dlight) + +alight = render.attachNewNode(AmbientLight("Ambient")) +alight.node().setColor(Vec4(0.2, 0.2, 0.2, 1)) +render.setLight(alight) + +render.setShaderAuto() +run() From 1b1c2b05f76e54e904bd6a95cc102f2ca227d430 Mon Sep 17 00:00:00 2001 From: Jim Fleming Date: Thu, 26 Nov 2015 13:33:24 -0800 Subject: [PATCH 2/3] Exporting separate muscle class --- experiments/muscle/app.py | 66 +----------------------------------- experiments/muscle/muscle.py | 66 ++++++++++++++++++++++++++++++++++++ 2 files changed, 67 insertions(+), 65 deletions(-) diff --git a/experiments/muscle/app.py b/experiments/muscle/app.py index 514ecbd..8eb8cf8 100644 --- a/experiments/muscle/app.py +++ b/experiments/muscle/app.py @@ -14,71 +14,7 @@ from ..utils.math_utils import get_angle_vec from ..utils.keras_utils import load_model - -class Muscle: - - def __init__(self, F_max): - self.F_max = F_max - - def drawLineSeg(self, loader, parent, start, end): - lines = LineSegs() - lines.setThickness(5.0) - lines.setColor(VBase4(1, 0.5, 0.5, 1.0)) - lines.moveTo(start) - lines.drawTo(end) - - np = parent.attachNewNode(lines.create()) - np.setDepthWrite(True) - np.setDepthTest(True) - - def drawTestCube(self, loader, parent, pos): - cube = loader.loadModel('cube') - cube.reparentTo(parent) - cube.setPos(pos) - cube.setScale(Vec3(0.1, 0.1, 0.1)) - cube.setColor(VBase4(1, 0.5, 0.5, 1.0)) - - def setupDebug(self, loader): - self.drawTestCube(loader, self.a, self.a_pos) - self.drawTestCube(loader, self.b, self.b_pos) - self.drawTestCube(loader, self.a, self.joint_pos) - - def setA(self, a, a_pos): - self.a = a - self.a_pos = a_pos - - def setB(self, b, b_pos): - self.b = b - self.b_pos = b_pos - - def setJointCenter(self, joint_pos): - self.joint_pos = joint_pos - - def getAttachmentA(self): - return self.a.getTransform().compose(TransformState.makePos(self.a_pos)).getPos() - - def getAttachmentB(self): - return self.b.getTransform().compose(TransformState.makePos(self.b_pos)).getPos() - - def getJointCenter(self): - return self.a.getTransform().compose(TransformState.makePos(self.joint_pos)).getPos() - - def getMomentArm(self): - d = self.getAttachmentB() - self.getAttachmentA() - v = self.getAttachmentA() - self.getJointCenter() - r = v.cross(d) - return r - - def apply(self, u): - F = u * self.F_max - r = self.getMomentArm() - - self.applyTorque(self.a, +F, r) - self.applyTorque(self.b, -F, r) - - def applyTorque(self, np, F, r): - T = r * F - np.node().applyTorque(T) +from .muscle import Muscle class App(ShowBase): diff --git a/experiments/muscle/muscle.py b/experiments/muscle/muscle.py index e69de29..e790e57 100644 --- a/experiments/muscle/muscle.py +++ b/experiments/muscle/muscle.py @@ -0,0 +1,66 @@ +from panda3d.core import VBase4, Vec3, TransformState, LineSegs + +class Muscle: + + def __init__(self, F_max): + self.F_max = F_max + + def drawLineSeg(self, loader, parent, start, end): + lines = LineSegs() + lines.setThickness(5.0) + lines.setColor(VBase4(1, 0.5, 0.5, 1.0)) + lines.moveTo(start) + lines.drawTo(end) + + np = parent.attachNewNode(lines.create()) + np.setDepthWrite(True) + np.setDepthTest(True) + + def drawTestCube(self, loader, parent, pos): + cube = loader.loadModel('cube') + cube.reparentTo(parent) + cube.setPos(pos) + cube.setScale(Vec3(0.1, 0.1, 0.1)) + cube.setColor(VBase4(1, 0.5, 0.5, 1.0)) + + def setupDebug(self, loader): + self.drawTestCube(loader, self.a, self.a_pos) + self.drawTestCube(loader, self.b, self.b_pos) + self.drawTestCube(loader, self.a, self.joint_pos) + + def setA(self, a, a_pos): + self.a = a + self.a_pos = a_pos + + def setB(self, b, b_pos): + self.b = b + self.b_pos = b_pos + + def setJointCenter(self, joint_pos): + self.joint_pos = joint_pos + + def getAttachmentA(self): + return self.a.getTransform().compose(TransformState.makePos(self.a_pos)).getPos() + + def getAttachmentB(self): + return self.b.getTransform().compose(TransformState.makePos(self.b_pos)).getPos() + + def getJointCenter(self): + return self.a.getTransform().compose(TransformState.makePos(self.joint_pos)).getPos() + + def getMomentArm(self): + d = self.getAttachmentB() - self.getAttachmentA() + v = self.getAttachmentA() - self.getJointCenter() + r = v.cross(d) + return r + + def apply(self, u): + F = u * self.F_max + r = self.getMomentArm() + + self.applyTorque(self.a, +F, r) + self.applyTorque(self.b, -F, r) + + def applyTorque(self, np, F, r): + T = r * F + np.node().applyTorque(T) From 96b885827902c43b762ab1239653999497104ad4 Mon Sep 17 00:00:00 2001 From: Jim Fleming Date: Fri, 18 Dec 2015 12:37:18 -0800 Subject: [PATCH 3/3] Adding simple muscle --- experiments/babble/app.py | 27 +++++------ experiments/config.py | 74 +++++++++++------------------- experiments/exposed_joint_rig.py | 3 ++ experiments/muscle/muscle.py | 8 ++-- experiments/rigid_body_rig.py | 13 +++--- experiments/utils/physics_utils.py | 30 ++++++------ 6 files changed, 71 insertions(+), 84 deletions(-) diff --git a/experiments/babble/app.py b/experiments/babble/app.py index 95ee181..354e445 100644 --- a/experiments/babble/app.py +++ b/experiments/babble/app.py @@ -36,24 +36,25 @@ def __init__(self, args): self.createLighting() if not headless: - self.setupCamera(width, height, Vec3(0, -200, 0), Vec3(0, 0, 0)) + self.setupCamera(width, height, Vec3(0, -20, 0), Vec3(0, 0, 0)) self.world = BulletWorld() - self.world.setGravity(Vec3(0, 0, -9.81 * 100.0)) - # self.world.setGravity(Vec3(0, 0, 0)) + self.world.setGravity(Vec3(0, 0, -9.81)) self.setupDebug() - self.createPlane(Vec3(0, 0, -100)) + self.createPlane(Vec3(0, 0, -11)) self.animated_rig = ExposedJointRig('walking', { 'walk': 'walking-animation.egg' }) self.animated_rig.reparentTo(self.render) self.animated_rig.setPos(0, 0, 0) - # self.animated_rig.createLines(VBase4(0.5, 0.75, 1.0, 1.0)) + self.animated_rig.setScale(0.1, 0.1, 0.1) + self.animated_rig.createLines(VBase4(0.5, 0.75, 1.0, 1.0)) self.physical_rig = RigidBodyRig() self.physical_rig.reparentTo(self.render) self.physical_rig.setPos(0, 0, 0) + self.physical_rig.setScale(0.1, 0.1, 0.1) self.physical_rig.createColliders(self.animated_rig) - self.physical_rig.createConstraints() + self.physical_rig.createConstraints(offset_scale=0.1) self.physical_rig.setCollideMask(BitMask32.bit(1)) self.physical_rig.attachRigidBodies(self.world) self.physical_rig.attachConstraints(self.world) @@ -119,7 +120,7 @@ def createLighting(self): light.setColor(VBase4(0.2, 0.2, 0.2, 1)) np = self.render.attachNewNode(light) - np.setPos(0, -200, 0) + np.setPos(10, -10, 20) np.lookAt(0, 0, 0) self.render.setLight(np) @@ -132,11 +133,11 @@ def createLighting(self): self.render.setLight(np) def createPlane(self, pos): - rb = BulletRigidBodyNode('Ground') + rb = BulletRigidBodyNode('plane') rb.addShape(BulletPlaneShape(Vec3(0, 0, 1), 1)) rb.setFriction(1.0) rb.setAnisotropicFriction(1.0) - rb.setRestitution(0.0) + rb.setRestitution(1.0) np = self.render.attachNewNode(rb) np.setPos(pos) @@ -191,11 +192,11 @@ def generate(self, count): return X, Y def update(self, task): - # if globalClock.getFrameCount() % 100 == 0: - # self.setAnimationFrame(globalClock.getFrameCount() / 100) + # if globalClock.getFrameCount() % self.babble_count == 0: + # self.setAnimationFrame(globalClock.getFrameCount() / self.babble_count) # self.physical_rig.babble() - # self.world.doPhysics(globalClock.getDt(), 10, 1.0 / 180.0) - # return task.cont + self.world.doPhysics(globalClock.getDt(), 10, 1.0 / 180.0) + return task.cont curr_train_count = len(self.X_train) curr_test_count = len(self.X_test) diff --git a/experiments/config.py b/experiments/config.py index 33aea36..10dc4f8 100644 --- a/experiments/config.py +++ b/experiments/config.py @@ -9,9 +9,7 @@ # Hips RightUpLeg LVecBase3f(1, 4.68006, 1) LVecBase3f(1, 20.8829, 1) "Hips": { - "mass": 100, - "F_max": 0, - "axis": (1, 0, 0), + "mass": 6, "joints": { "Spine": { "type": "hinge", @@ -37,12 +35,20 @@ "offset_child": (0, -20.8829, 0), }, }, + "muscles": { + "Spine": [ + { + "F_max": 100, + "attachment_a": (), + "attachment_b": (), + "joint_center": (), + } + ] + }, }, "Spine": { - "mass": 10, - "F_max": 4000.0, - "axis": (1, 0, 0), + "mass": 1, "joints": { "Spine1": { "type": "hinge", @@ -56,9 +62,7 @@ # Spine1 Spine2 LVecBase3f(1, 6.85034, 1) LVecBase3f(1, 6.2423, 1) "Spine1": { - "mass": 10, - "F_max": 400.0, - "axis": (1, 0, 0), + "mass": 1, "joints": { "Spine2": { "type": "hinge", @@ -82,9 +86,7 @@ # Spine2 LeftShoulder LVecBase3f(1, 6.2423, 1) LVecBase3f(1, 6.24931, 1) "Spine2": { - "mass": 10, - "F_max": 200.0, - "axis": (1, 0, 0), + "mass": 1, "joints": { "Neck": { "type": "hinge", @@ -111,9 +113,7 @@ }, "Neck": { - "mass": 5, - "F_max": 100.0, - "axis": (1, 0, 0), + "mass": 1, "joints": { "Neck1": { "type": "hinge", @@ -125,8 +125,6 @@ "Neck1": { "mass": 1, - "F_max": 0.0, - "axis": (1, 0, 0), "joints": { "Head": { "type": None @@ -137,9 +135,7 @@ # Left Leg "LeftUpLeg": { - "mass": 30, - "F_max": 200000.0, - "axis": (1, 0, 0), + "mass": 4, "joints": { "LeftLeg": { "type": "hinge", @@ -150,9 +146,7 @@ } }, "LeftLeg": { - "mass": 20, - "F_max": 40000.0, - "axis": (1, 0, 0), + "mass": 4, "joints": { "LeftFoot": { "type": "hinge", @@ -163,9 +157,7 @@ } }, "LeftFoot": { - "mass": 10, - "F_max": 4000.0, - "axis": (1, 0, 0), + "mass": 1, "joints": { "LeftToeBase": { "type": None @@ -176,9 +168,7 @@ # Right Leg "RightUpLeg": { - "mass": 30, - "F_max": 200000.0, - "axis": (1, 0, 0), + "mass": 4, "joints": { "RightLeg": { "type": "hinge", @@ -189,9 +179,7 @@ } }, "RightLeg": { - "mass": 20, - "F_max": 40000.0, - "axis": (1, 0, 0), + "mass": 4, "joints": { "RightFoot": { "type": "hinge", @@ -202,9 +190,7 @@ } }, "RightFoot": { - "mass": 10, - "F_max": 4000.0, - "axis": (1, 0, 0), + "mass": 1, "joints": { "RightToeBase": { "type": None @@ -215,8 +201,7 @@ # Left Arm "LeftShoulder": { - "mass": 20, - "F_max": 40000.0, + "mass": 1, "axis": (0, 0, 1), "joints": { "LeftArm": { @@ -227,8 +212,7 @@ } }, "LeftArm": { - "mass": 10, - "F_max": 4000.0, + "mass": 2, "axis": (0, 0, 1), "joints": { "LeftForeArm": { @@ -239,8 +223,7 @@ } }, "LeftForeArm": { - "mass": 5, - "F_max": 2000.0, + "mass": 2, "axis": (0, 0, 1), "joints": { "LeftHand": { @@ -252,8 +235,7 @@ # Right Arm "RightShoulder": { - "mass": 20, - "F_max": 40000.0, + "mass": 1, "axis": (0, 0, 1), "joints": { "RightArm": { @@ -264,8 +246,7 @@ } }, "RightArm": { - "mass": 10, - "F_max": 4000.0, + "mass": 2, "axis": (0, 0, 1), "joints": { "RightForeArm": { @@ -276,8 +257,7 @@ } }, "RightForeArm": { - "mass": 5, - "F_max": 2000.0, + "mass": 2, "axis": (0, 0, 1), "joints": { "RightHand": { diff --git a/experiments/exposed_joint_rig.py b/experiments/exposed_joint_rig.py index f2a002d..c29a80a 100644 --- a/experiments/exposed_joint_rig.py +++ b/experiments/exposed_joint_rig.py @@ -43,6 +43,9 @@ def getNumFrames(self, animation_name): def setPlayRate(self, play_rate, animation_name): self.actor.setPlayRate(play_rate, animation_name) + def setScale(self, x, y, z): + self.actor.setScale(x, y, z) + def setPos(self, x, y, z): self.actor.setPos(x, y, z) diff --git a/experiments/muscle/muscle.py b/experiments/muscle/muscle.py index e790e57..b7c01a4 100644 --- a/experiments/muscle/muscle.py +++ b/experiments/muscle/muscle.py @@ -2,8 +2,8 @@ class Muscle: - def __init__(self, F_max): - self.F_max = F_max + def __init__(self, max_force): + self.max_force = max_force def drawLineSeg(self, loader, parent, start, end): lines = LineSegs() @@ -54,8 +54,8 @@ def getMomentArm(self): r = v.cross(d) return r - def apply(self, u): - F = u * self.F_max + def apply(self, activation): + F = activation * self.max_force r = self.getMomentArm() self.applyTorque(self.a, +F, r) diff --git a/experiments/rigid_body_rig.py b/experiments/rigid_body_rig.py index 7784707..42bc563 100644 --- a/experiments/rigid_body_rig.py +++ b/experiments/rigid_body_rig.py @@ -13,8 +13,6 @@ from .utils.math_utils import get_angle_vec, random_spherical from .config import joints_config, excluded_joints -F_MAX = 100.0 - class RigidBodyRig: def __init__(self): @@ -23,8 +21,8 @@ def __init__(self): def createColliders(self, pose_rig): self.colliders = list(create_colliders(self.root, pose_rig, joints_config)) - def createConstraints(self): - self.constraints = list(create_constraints(self.root, get_joint_pairs(self.root, joints_config))) + def createConstraints(self, offset_scale): + self.constraints = list(create_constraints(self.root, get_joint_pairs(self.root, joints_config), offset_scale)) def clearMasses(self): [collider.node().setMass(0) for collider in self.colliders] @@ -43,6 +41,9 @@ def attachCubes(self, loader): cube.setScale(scale) cube.reparentTo(collider) + def setScale(self, *scale): + self.root.setScale(*scale) + def setPos(self, *pos): self.root.setPos(*pos) @@ -58,7 +59,7 @@ def babble(self): F_all.append(F) r = Vec3(*joints_config[collider_name].get("axis", (1, 0, 0))) - r_world = collider.getQuat(self.root).xform(r) # TODO: is root necessary? + r_world = collider.getQuat(self.root).xform(r) T = r_world * F collider.node().applyTorqueImpulse(T) @@ -74,7 +75,7 @@ def apply_forces(self, F_all): F = F_all[i] r = Vec3(1, 0, 0) - r_world = collider.getQuat(self.root).xform(r) # TODO: is root necessary? + r_world = collider.getQuat(self.root).xform(r) T = r_world * F collider.node().applyTorqueImpulse(T) diff --git a/experiments/utils/physics_utils.py b/experiments/utils/physics_utils.py index 3abfa33..6c97c48 100644 --- a/experiments/utils/physics_utils.py +++ b/experiments/utils/physics_utils.py @@ -35,12 +35,12 @@ def create_colliders(root, pose_rig, joints_config): mass = joint_config['mass'] if 'mass' in joint_config else 1 box_rb = BulletRigidBodyNode(node.getName()) - box_rb.setMass(mass) - box_rb.setLinearDamping(0.2) - box_rb.setAngularDamping(0.9) - box_rb.setFriction(1.0) - box_rb.setAnisotropicFriction(1.0) - box_rb.setRestitution(0.0) + box_rb.setMass(1.0) + # box_rb.setLinearDamping(0.2) + # box_rb.setAngularDamping(0.9) + # box_rb.setFriction(1.0) + # box_rb.setAnisotropicFriction(1.0) + # box_rb.setRestitution(0.0) for joint in joints: child_node, child_parent = next((child_node, child_parent) for child_node, child_parent in pose_rig.exposed_joints if child_node.getName() == joint) @@ -57,7 +57,6 @@ def create_colliders(root, pose_rig, joints_config): transform = TransformState.makePosHpr(child_node.getPos(child_parent) / 2.0, quat.getHpr()) else: transform = TransformState.makeHpr(quat.getHpr()) - # transform = TransformState.makeIdentity() box_rb.addShape(shape, transform) @@ -73,7 +72,7 @@ def create_colliders(root, pose_rig, joints_config): yield box_np -def create_constraints(root, joint_pairs): +def create_constraints(root, joint_pairs, offset_scale): for joint_config, parent, child in joint_pairs: rb_parent = parent.node() rb_child = child.node() @@ -82,7 +81,7 @@ def create_constraints(root, joint_pairs): extents_child = rb_child.get_shape(0).getHalfExtentsWithMargin() if 'offset_parent' in joint_config: - offset_parent = joint_config['offset_parent'] + offset_parent = Point3(joint_config['offset_parent']) else: offset_parent = (0, 1, 0) offset_parent_x, offset_parent_y, offset_parent_z = offset_parent @@ -91,7 +90,7 @@ def create_constraints(root, joint_pairs): offset_parent_z * extents_parent.getZ()) if 'offset_child' in joint_config: - offset_child = joint_config['offset_child'] + offset_child = Point3(*joint_config['offset_child']) else: offset_child = (0, -1, 0) offset_child_x, offset_child_y, offset_child_z = offset_child @@ -99,6 +98,9 @@ def create_constraints(root, joint_pairs): offset_child_y * extents_child.getY(), \ offset_child_z * extents_child.getZ()) + offset_parent = offset_parent * offset_scale + offset_child = offset_child * offset_scale + if joint_config['type'] == 'hinge': axis_parent = Vec3(*joint_config['axis_parent']) axis_child = Vec3(*joint_config['axis_child']) @@ -108,9 +110,9 @@ def create_constraints(root, joint_pairs): if 'limit' in joint_config: low, high = joint_config['limit'] - softness = 0.0 - bias = 4.0 - relaxation = 0.1 + softness = 1.0 + bias = 0.3 + relaxation = 1.0 constraint.setLimit(low, high, softness, bias, relaxation) elif joint_config['type'] == 'cone': frame_parent = TransformState.makePosHpr(offset_parent, Vec3(90, 0, 0)) @@ -123,6 +125,6 @@ def create_constraints(root, joint_pairs): elif joint_config['type'] == 'spherical': constraint = BulletSphericalConstraint(rb_parent, rb_child, offset_parent, offset_child) - constraint.setDebugDrawSize(3.0) + # constraint.setDebugDrawSize(1.0) yield constraint