Building a stick figure


#1

Hi everyone,

I need some help please because I can’t seem to get this in my head.

So, I’m trying to build a stick creature with Bullet.

So basically I have a numpy array with coordinates like so:

[[ 0 0 0]
[ 10 0 0]
[ 20 0 0]
[ 10 0 0]
[ 15 -8 0]
[ 10 0 0]
[ 5 -8 0]
[ 0 -17 0]
[ -4 -8 0]
[ 0 -17 0]
[ 5 -8 0]
[ 15 -8 0]
[ 5 -8 0]
[ 10 -17 0]]

So each entry is a point in 3D space. Now I want to iterate through the list and add sticks between each pair of points with a cone-twist constraint.

I’ve tried this:

for sticks in range(len(coordinates)-1):
    length = np.linalg.norm(
        coordinates[(sticks + 1)] - coordinates[sticks])
    shape = BulletCylinderShape(0.1, length, ZUp)
    node = BulletRigidBodyNode('legs' + str(sticks))
    node.setMass(1)
    node.addShape(shape)
    cylinder = render.attachNewNode(node)
    pos = coordinates[sticks]
    cylinder.setPos(Vec3(pos[0], pos[1], pos[2]))
    world.attachRigidBody(node)

Which seems to add all the sticks (small cylinders) but changes the position of all the sticks to the last coordinate. Also, I’m not sure how to loop add the constraints since I can’t figure out how to call a previously generated stick

Please help

Thank you


#2

You obviously need to study the behavior of the lists, since the panda has nothing to do with it.
I would first achieve the desired behavior of the elements in the list, then I would take up physics.


#3

It’s not clear to me from your code why all the sticks would be at the last coordinate. Maybe you could share some more complete code that we can run so that we can see for ourselves?


#4

For this at least I think that I have an answer: just use a new list to store the NodePaths that you create in your loop. Something like this:

stickList = []

# You extant loop:
for sticks in range(len(coordinates)-1):
    # ... Code here ...
    cylinder = render.attachNewNode(node)
    stickList.append(cylinder)
    # ... More code here ...

# Now we can iterate over "stickList" and
# work with the sticks that we created!

#5

Here is my complete code:

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from math import sin, cos, radians

import direct.directbase.DirectStart

from panda3d.core import Vec3
from panda3d.bullet import BulletWorld, BulletPlaneShape, \
    BulletRigidBodyNode, BulletCylinderShape, BulletDebugNode, ZUp

from direct.showbase.DirectObject import DirectObject


def Rules(oldCharacter):
    newCharacter = ""
    if oldCharacter == 'F':
        newCharacter = "B-F"
    elif oldCharacter == 'B':
        newCharacter = "F+B"
    elif oldCharacter == '+':
        newCharacter = ']B+F--B['
    elif oldCharacter == '-':
        newCharacter = '[F-B++F]'
    elif oldCharacter == '[':
        newCharacter = '^[FB+-]^'
    elif oldCharacter == ']':
        newCharacter = '.]BF-+[.'
    else:
        newCharacter = oldCharacter

    return newCharacter


def processString(oldString):
    newString = ""
    for eachChar in oldString:
        newString = newString + Rules(eachChar)
    return ('FF' + newString)


def createLSystem(numofIters, startString):
    endString = ""
    for i in range(numofIters):
        endString = processString(startString)
        startString = endString

    return endString


def toCoordinates(commandSequence, distance, degrees_xy, degrees_z):
    angle = radians(0)
    pitch = radians(0)
    gps = np.array([0, 0, 0])
    for character in commandSequence:
        if character == "F":
            try:
                gps = np.vstack(
                    (gps, gps[(len(gps) - 1)] + [sin(radians(90) - angle)*distance, cos(radians(90) - angle)*distance, sin(pitch)*distance]))
            except:
                gps = np.vstack((gps, [distance, 0, 0]))
        if character == "B":
            try:
                gps = np.vstack(
                    (gps, gps[(len(gps) - 1)] - [sin(radians(90)-angle)*distance, cos(radians(90)-angle)*distance, sin(pitch)*distance]))
            except:
                gps = np.vstack((gps, [-distance, 0, 0]))
        if character == "+":
            angle = angle + radians(degrees_xy)
        if character == '-':
            angle = angle - radians(degrees_xy)
        if character == "^":
            pitch = pitch + radians(degrees_z)
        if character == '.':
            pitch = pitch - radians(degrees_z)
        if character == '[':
            try:
                savedCoor = np.vstack((savedCoor, (len(gps) - 1)))
            except:
                savedCoor = (len(gps) - 1)
        if character == ']':
            try:
                loc_T = int(savedCoor[len(savedCoor) - 1])
                loc_P = (len(gps) - 1)
                gps = np.insert(gps, (loc_P), gps[loc_T], axis=0)
            except:
                pass

    return gps.astype(int)


def drawLSystem(ax, vector1, vector2, time):
    if time == 0:
        ax.scatter(vector1[0], vector1[1], vector1[2], color='k', marker="*")
        ax.scatter(vector2[0], vector2[1], vector2[2], color='b', marker="*")
        ax.plot([vector1[0], vector2[0]], [vector1[1], vector2[1]],
                [vector1[2], vector2[2]], color='r')
    else:
        ax.scatter(vector1[0], vector1[1], vector1[2], color='b', marker="*")
        ax.scatter(vector2[0], vector2[1], vector2[2], color='b', marker="*")
        ax.plot([vector1[0], vector2[0]], [vector1[1], vector2[1]],
                [vector1[2], vector2[2]], color='r')


def toggleDebug():
    if debugNP.isHidden():
        debugNP.show()
    else:
        debugNP.hide()


def Simulate(coordinates):
    base.cam.setPos(0, -1000, 1000)
    base.cam.lookAt(0, 0, 0)

    # Debug node
    debugNode = BulletDebugNode('Debug')
    debugNode.showWireframe(True)
    debugNode.showConstraints(True)
    debugNode.showBoundingBoxes(False)
    debugNode.showNormals(False)
    debugNP = render.attachNewNode(debugNode)
    debugNP.show()

    # World
    world = BulletWorld()
    world.setGravity(Vec3(0, 0, -9.81))
    world.setDebugNode(debugNP.node())

    # Plane
    shape = BulletPlaneShape(Vec3(0, 0, 1), 1)
    node = BulletRigidBodyNode('Ground')
    node.addShape(shape)
    ground = render.attachNewNode(node)
    ground.setPos(0, 0, 0)
    world.attachRigidBody(node)

    for sticks in range(len(coordinates)-1):
        length = np.linalg.norm(
            coordinates[(sticks + 1)] - coordinates[sticks])
        shape = BulletCylinderShape(0.1, length, ZUp)
        node = BulletRigidBodyNode('legs' + str(sticks))
        node.setMass(1)
        node.addShape(shape)
        cylinder = render.attachNewNode(node)
        pos = coordinates[sticks]
        cylinder.setPos(Vec3(pos[0], pos[1], pos[2]))
        world.attachRigidBody(node)

    def update(task):
        dt = globalClock.getDt()
        world.doPhysics(dt)
        return task.cont

    o = DirectObject()
    o.accept('f1', toggleDebug)

    taskMgr.add(update, 'update')
    base.run()


def main():
    instance = createLSystem(2, "F")
    # print("\n" + instance + "\n")
    path = toCoordinates(instance, 10, 60, 90)
    print(path)
    Simulate(path)

main()


#6

Okay, I see. Thanks!


#7

A question: if you comment out the call to “world.doPhysics”, do you still end up with all of the sticks in the same place?


#8

All I get is a grey screen then


#9

@Thaumaturge Genuis man!!! stickList works like a charm. Added all the cone constraints no problem. Now I just need to figure out why they fly into the air. New code is now:

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from math import sin, cos, radians

import direct.directbase.DirectStart

from panda3d.core import Vec3, TransformState, Point3
from panda3d.bullet import BulletWorld, BulletPlaneShape, \
    BulletRigidBodyNode, BulletCylinderShape, BulletDebugNode, ZUp, BulletConeTwistConstraint

from direct.showbase.DirectObject import DirectObject


def Rules(oldCharacter):
    newCharacter = ""
    if oldCharacter == 'F':
        newCharacter = "B-F"
    elif oldCharacter == 'B':
        newCharacter = "F+B"
    elif oldCharacter == '+':
        newCharacter = ']B+F--B['
    elif oldCharacter == '-':
        newCharacter = '[F-B++F]'
    elif oldCharacter == '[':
        newCharacter = '^[FB+-]^'
    elif oldCharacter == ']':
        newCharacter = '.]BF-+[.'
    else:
        newCharacter = oldCharacter

    return newCharacter


def processString(oldString):
    newString = ""
    for eachChar in oldString:
        newString = newString + Rules(eachChar)
    return ('FF' + newString)


def createLSystem(numofIters, startString):
    endString = ""
    for i in range(numofIters):
        endString = processString(startString)
        startString = endString

    return endString


def toCoordinates(commandSequence, distance, degrees_xy, degrees_z):
    angle = radians(0)
    pitch = radians(0)
    gps = np.array([0, 0, 0])
    for character in commandSequence:
        if character == "F":
            try:
                gps = np.vstack(
                    (gps, gps[(len(gps) - 1)] + [sin(radians(90) - angle)*distance, cos(radians(90) - angle)*distance, sin(pitch)*distance]))
            except:
                gps = np.vstack((gps, [distance, 0, 0]))
        if character == "B":
            try:
                gps = np.vstack(
                    (gps, gps[(len(gps) - 1)] - [sin(radians(90)-angle)*distance, cos(radians(90)-angle)*distance, sin(pitch)*distance]))
            except:
                gps = np.vstack((gps, [-distance, 0, 0]))
        if character == "+":
            angle = angle + radians(degrees_xy)
        if character == '-':
            angle = angle - radians(degrees_xy)
        if character == "^":
            pitch = pitch + radians(degrees_z)
        if character == '.':
            pitch = pitch - radians(degrees_z)
        if character == '[':
            try:
                savedCoor = np.vstack((savedCoor, (len(gps) - 1)))
            except:
                savedCoor = (len(gps) - 1)
        if character == ']':
            try:
                loc_T = int(savedCoor[len(savedCoor) - 1])
                loc_P = (len(gps) - 1)
                gps = np.insert(gps, (loc_P), gps[loc_T], axis=0)
            except:
                pass

    return gps.astype(int)


def drawLSystem(ax, vector1, vector2, time):
    if time == 0:
        ax.scatter(vector1[0], vector1[1], vector1[2], color='k', marker="*")
        ax.scatter(vector2[0], vector2[1], vector2[2], color='b', marker="*")
        ax.plot([vector1[0], vector2[0]], [vector1[1], vector2[1]],
                [vector1[2], vector2[2]], color='r')
    else:
        ax.scatter(vector1[0], vector1[1], vector1[2], color='b', marker="*")
        ax.scatter(vector2[0], vector2[1], vector2[2], color='b', marker="*")
        ax.plot([vector1[0], vector2[0]], [vector1[1], vector2[1]],
                [vector1[2], vector2[2]], color='r')


def toggleDebug():
    if debugNP.isHidden():
        debugNP.show()
    else:
        debugNP.hide()


def Simulate(coordinates):
    base.cam.setPos(0, -1000, 1000)
    base.cam.lookAt(0, 0, 0)

    # Debug node
    debugNode = BulletDebugNode('Debug')
    debugNode.showWireframe(True)
    debugNode.showConstraints(True)
    debugNode.showBoundingBoxes(False)
    debugNode.showNormals(False)
    debugNP = render.attachNewNode(debugNode)
    debugNP.show()

    # World
    world = BulletWorld()
    world.setGravity(Vec3(0, 0, -9.81))
    world.setDebugNode(debugNP.node())

    # Plane
    shape = BulletPlaneShape(Vec3(0, 0, 1), 1)
    node = BulletRigidBodyNode('Ground')
    node.addShape(shape)
    ground = render.attachNewNode(node)
    ground.setPos(0, 0, 0)
    world.attachRigidBody(node)

    stickList = []

    for sticks in range(len(coordinates)-1):
        length = np.linalg.norm(
            coordinates[(sticks + 1)] - coordinates[sticks])
        shape = BulletCylinderShape(0.1, length, ZUp)
        node = BulletRigidBodyNode('legs' + str(sticks))
        node.setMass(1)
        node.addShape(shape)
        cylinder = render.attachNewNode(node)
        stickList.append(cylinder)
        pos = coordinates[sticks]
        cylinder.setPos(Vec3(pos[0], pos[1], pos[2]))
        world.attachRigidBody(node)

    print(stickList)
    for lines in range(len(stickList) - 1):
        frameA = TransformState.makePosHpr(Point3(0, 0, -5), Vec3(0, 0, -90))
        frameB = TransformState.makePosHpr(Point3(0, 0, 5), Vec3(0, 0, -90))

        swing1 = 60  # degrees
        swing2 = 36  # degrees
        twist = 120  # degrees

        Stick1 = stickList[lines]
        Stick2 = stickList[lines + 1]

        cs = BulletConeTwistConstraint(
            Stick1.node(), Stick2.node(), frameA, frameB)
        cs.setDebugDrawSize(2.0)
        cs.setLimit(swing1, swing2, twist)
        world.attachConstraint(cs)

    def update(task):
        dt = globalClock.getDt()
        world.doPhysics(dt)
        return task.cont

    o = DirectObject()
    o.accept('f1', toggleDebug)

    taskMgr.add(update, 'update')
    base.run()


def main():
    instance = createLSystem(2, "F")
    # print("\n" + instance + "\n")
    path = toCoordinates(instance, 10, 60, 90)
    print(path)
    Simulate(path)


main()


#10

Hmm… Here’s a thought: what are the first few values that you’re getting for “dt” in your “update” method? Could it perhaps be that your first few updates are a little long, and that the physics system is thus getting a very high dt, and thus acting up a bit?


#11

Hello again @Thaumaturge, it seems there is some irregularity there. Here are the first few dt values:

0.9937492519470374
2.8383099707498163
1.0323746823180926
0.5774020461829528
0.6832406039298418
0.5398191654143707
0.38274974119992233
0.3105863405354059
0.22940325846621068
0.21934172739165092
0.2084175293919195
0.21195568866718162
0.2200451596511268
0.21773783184679196
0.21501124441716257
0.20604125822683272
0.21828307933946078
0.19058359679962678
0.21111506961879734
0.19710556873373086
0.20828594206377815
0.1996347751711518
0.21003962318428648
0.20611930071134132
0.18560952580266665
0.22365051245565049
69.70166492966727
0.3904420004353568
0.21944636731481637
0.22330369585409926
0.8938734199896459
1.0805762405080515
0.2591927397391629
0.23136621943305613
0.2090327701442476
0.22324455154968348
0.20594501749481253
0.22646004189796543
0.2471608984054825

However, if I set dt=0.2 they still fly into the air…


#12

@Thaumaturge, would you be so kind as to explain the TransformState.makePosHpr function to me? I just copied those pieces from the manual and I don’t understand what they do.

Also, if I “pause” the scene after the first dt step this is what happens:

Which shows that the position of the sticks are not correct so it does have something to do with that. So I realize that the sticks are not orientated correctly due to the node being in the center of the stick. Is there some way to move them to the tip of each stick?


#13

Honestly, I’m not all that familiar with such use of TransformStates. (And in addition, I’m pretty tired today.) That said, looking at the manual, it seems to me that what might be happening is that the TransformStates specify the relative offsets and orientations for the constraints. That is, the “A” spatial frame specifies the position and orientation of the constraint relative to body “A”, and the “B” spatial frame specifies the position and orientation of the constraint relative to body “B”. In addition, given the terminology, I imagine that the constraint would operate with those locations and orientations as its “zeroes”–the location relative to the body in question that it considers to be (0, 0, 0), and the rotation relative to the body in question that it considers to be “unrotated”.

However, I very much stand to be corrected on this by someone more familiar with these things, or simply less tired than me! ^^;

As to offsetting the rigid bodies, I’m not sure of whether one can do so with Bullet bodies, offhand. If not, you could perhaps parent them each below an additional node, and offset them relative to that.

I’m not convinced that doing so is called for here, however. Perhaps it would be better to offset your coordinates to account for the origin of each “stick” being at its centre?