flagpole.py

import time
# import roboverse.bullet as bullet
import pybullet as p
import numpy as np

def connect():
    clid = p.connect(p.SHARED_MEMORY)
    if (clid < 0):
        p.connect(p.GUI)

    p.createCollisionShape(p.GEOM_PLANE)
    p.createMultiBody(0,0)
    p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
    p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
    p.resetDebugVisualizerCamera(0.8, 90, -20, [0.75, -.2, 0])

def setup(real_time=True, gravity=-10):
    '''
        sets parameters for running pybullet 
        interactively
    '''
    p.setRealTimeSimulation(real_time)
    p.setGravity(0, 0, gravity)
    p.stepSimulation()

connect()
setup()

base_mass = 0
base_shape = p.createCollisionShape(
		p.GEOM_BOX,
		halfExtents=[.02,.2,.02])
base_position = [0,0,0]
base_orientation = [0,0,0,1]

num_children = 2
link_masses = [1 for _ in range(num_children)]
collision_shapes = [
	p.createCollisionShape(
		p.GEOM_BOX,
		halfExtents=[.02,.2,.4],
        collisionFramePosition=[0,0,.44])
	for _ in range(num_children)
]

print("collision_shapes: ", collision_shapes)

### First link starts at zero, next 0.8, 1.6, ...
link_adjust = np.arange(num_children)
link_positions = [[0.0,0,0.8*i] for i in range(num_children)]
link_orientations = [[0,0,0,1]]*num_children
intertial_positions = [[0,0,0]]*num_children
inertial_orientations = [[0,0,0,1]]*num_children
link_parents = list(range(num_children))
joint_types = [p.JOINT_REVOLUTE]*num_children
joint_axes = [[0,1,0]]*num_children

# pdb.set_trace()

body = p.createMultiBody(base_mass,
	base_shape,
	-1,
	basePosition=base_position,
	baseOrientation=base_orientation,
	linkMasses=link_masses,
	linkCollisionShapeIndices=collision_shapes, 
	linkVisualShapeIndices=[-1]*num_children,
	linkPositions=link_positions,
	linkOrientations=link_orientations,
	linkInertialFramePositions=intertial_positions,
	linkInertialFrameOrientations=inertial_orientations,
	linkParentIndices=link_parents,
	linkJointTypes=joint_types,
	linkJointAxis=joint_axes)

p.setJointMotorControl2(body,0,p.POSITION_CONTROL,targetPosition=0.01,force=1000,maxVelocity=3)

print(
	'base_mass: {}\n'.format(base_mass),
	'base_shape: {}\n'.format(base_shape),
	'base_position: {}\n'.format(base_position),
	'base_orientation: {}\n'.format(base_orientation),
	'link_masses: {}\n'.format(link_masses),
	'collision_shapes: {}\n'.format(collision_shapes),
	'visual_shapes: {}\n'.format(visual_shapes),
	'link_positions: {}\n'.format(link_positions),
	'link_orientations: {}\n'.format(link_orientations),
	'inertial_orientations: {}\n'.format(inertial_orientations),
	'link_parents: {}\n'.format(link_parents),
	'joint_types: {}\n'.format(joint_types),
	'joint_axes: {}\n'.format(joint_axes)
	)

while True:
    time.sleep(0.01)
    # bullet.step()
    p.stepSimulation()