def _loadObjects(self):
"""
Spawns the Differential Drive robot based on the input pose.
"""
# Create a Box
box_pos = [
self.basepos[0], self.basepos[1] + self.cubesize, self.basepos[2]
]
rot = self.baserot
boxbody = ode.Body(self.world)
boxgeom = ode.GeomBox(space=self.space,
lengths=(self.cubesize, self.cubesize * 0.75,
self.cubesize))
boxgeom.setBody(boxbody)
boxgeom.setPosition(box_pos)
boxgeom.setRotation(rot)
boxM = ode.Mass()
boxM.setBox(self.cubemass, self.cubesize, self.cubesize * 0.75,
self.cubesize)
boxbody.setMass(boxM)
# Create two Cylindrical Wheels
rot = self.multmatrix(self.genmatrix(math.pi / 2, 2), rot)
leftwheel_pos = [
box_pos[0] + 0.5 * self.track, box_pos[1] - self.wheelradius * 0.5,
box_pos[2] + self.cubesize * 0.2
]
rightwheel_pos = [
box_pos[0] - 0.5 * self.track, box_pos[1] - self.wheelradius * 0.5,
box_pos[2] + self.cubesize * 0.2
]
leftwheelbody = ode.Body(self.world)
leftwheelgeom = ode.GeomCylinder(space=self.space,
radius=self.wheelradius,
length=self.wheelradius / 3.0)
leftwheelgeom.setBody(leftwheelbody)
leftwheelgeom.setPosition(leftwheel_pos)
leftwheelgeom.setRotation(rot)
leftwheelM = ode.Mass()
leftwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius,
self.wheelradius / 3.0)
leftwheelbody.setMass(leftwheelM)
lefthinge = ode.HingeJoint(self.world)
lefthinge.attach(leftwheelbody, boxbody)
lefthinge.setAnchor(leftwheel_pos)
lefthinge.setAxis(self.rotate((0, 0, 1), rot))
lefthinge.setParam(ode.ParamFMax, self.hingemaxforce)
rightwheelbody = ode.Body(self.world)
rightwheelgeom = ode.GeomCylinder(space=self.space,
radius=self.wheelradius,
length=self.wheelradius)
rightwheelgeom.setBody(rightwheelbody)
rightwheelgeom.setPosition(rightwheel_pos)
rightwheelgeom.setRotation(rot)
rightwheelM = ode.Mass()
rightwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius,
self.cubemass / 3.0)
rightwheelbody.setMass(rightwheelM)
righthinge = ode.HingeJoint(self.world)
righthinge.attach(rightwheelbody, boxbody)
righthinge.setAnchor(rightwheel_pos)
righthinge.setAxis(self.rotate((0, 0, 1), rot))
righthinge.setParam(ode.ParamFMax, self.hingemaxforce)
# Create a spherical wheel at the back for support
caster_pos = [
box_pos[0], box_pos[1] - self.cubesize * 0.5,
box_pos[2] - self.cubesize * 0.5
]
casterbody = ode.Body(self.world)
castergeom = ode.GeomSphere(space=self.space,
radius=self.cubesize / 5.0)
castergeom.setBody(casterbody)
castergeom.setPosition(caster_pos)
castergeom.setRotation(rot)
casterM = ode.Mass()
casterM.setSphere(self.cubemass * 100.0, self.cubesize / 5.0)
casterbody.setMass(casterM)
self.fixed = ode.FixedJoint(self.world)
self.fixed.attach(casterbody, boxbody)
self.fixed.setFixed()
# WHEW, THE END OF ALL THAT FINALLY!
# Build the Geoms and Joints arrays for rendering.
self.boxgeom = boxgeom
self._geoms = [
boxgeom, leftwheelgeom, rightwheelgeom, castergeom, self.ground
]
self.lefthinge = lefthinge
self.righthinge = righthinge
self._joints = [self.lefthinge, self.righthinge, self.fixed]
def loadObstacles(self, obstaclefile):
"""
Loads obstacles from the obstacle text file.
"""
# Initiate data structures.
data = open(obstaclefile, "r")
obs_sizes = []
obs_positions = []
obs_masses = []
reading = "None"
# Parse the obstacle text file.
for line in data:
linesplit = line.split()
if linesplit != []:
if linesplit[0] != "#":
obs_sizes.append([
float(linesplit[0]),
float(linesplit[1]),
float(linesplit[2])
])
obs_positions.append([
float(linesplit[3]),
float(linesplit[4]),
float(linesplit[5])
])
obs_masses.append(float(linesplit[6]))
# Go through all the obstacles in the list and spawn them.
for i in range(len(obs_sizes)):
obs_size = obs_sizes[i]
obs_pos = obs_positions[i]
obs_mass = obs_masses[i]
# Create the obstacle.
body = ode.Body(self.world)
geom = ode.GeomBox(space=self.space, lengths=obs_size)
geom.setBody(body)
geom.setPosition(obs_pos)
M = ode.Mass()
M.setBox(obs_mass, obs_size[0], obs_size[1], obs_size[2])
body.setMass(M)
# Append all these new pointers to the simulator class.
self._geoms.append(geom)
# Miles Jacobs <*****@*****.**> # Department of Electrical Engineering # Cape Peninsula University of Technology # Bellville, South Africa from visual import * import ode # Create a world object world = ode.World() world.setGravity((0, -9.81, 0)) # Create two bodies body1 = ode.Body(world) M = ode.Mass() M.setSphere(2500, 0.05) body1.setMass(M) body1.setPosition((1, 2, 0)) body2 = ode.Body(world) M = ode.Mass() M.setSphere(2500, 0.05) body2.setMass(M) body2.setPosition((2, 2, 0)) # Connect body1 with the static environment j1 = ode.HingeJoint(world) j1.attach(body1, ode.environment) j1.setAnchor((0, 2, 0)) j1.setAxis((0, 0, 1))
def setNode(self, node):
super().setNode(node)
aabb = node.getBoundingBox()
if not aabb.isValid():
return
if not self._body:
self._body = ode.Body(self._world)
self._body.setMaxAngularSpeed(0)
mass = ode.Mass()
mass.setBox(5.0, Helpers.toODE(aabb.width),
Helpers.toODE(aabb.height), Helpers.toODE(aabb.depth))
self._body.setMass(mass)
if not self._geom:
if node.getMeshData():
scale_matrix = Matrix()
scale_matrix.setByScaleFactor(1.01)
mesh = node.getMeshData().getTransformed(scale_matrix)
self._trimesh = ode.TriMeshData()
debug_builder = MeshBuilder()
vertices = mesh.getVertices()
indices = mesh.getConvexHull().simplices
_fixWindingOrder(vertices, indices, debug_builder)
self._trimesh.build(vertices / Helpers.ScaleFactor, indices)
self._geom = ode.GeomTriMesh(self._trimesh, self._space)
mb = MeshBuilder()
for i in range(self._geom.getTriangleCount()):
tri = self._geom.getTriangle(i)
v0 = Helpers.fromODE(tri[0])
v1 = Helpers.fromODE(tri[1])
v2 = Helpers.fromODE(tri[2])
mb.addFace(v0=v0,
v1=v1,
v2=v2,
color=Color(1.0, 0.0, 0.0, 0.5))
chn = SceneNode(node)
chn.setMeshData(mb.build())
def _renderConvexHull(renderer):
renderer.queueNode(chn, transparent=True)
return True
chn.render = _renderConvexHull
n = SceneNode(node)
n.setMeshData(debug_builder.build())
def _renderNormals(renderer):
renderer.queueNode(n, mode=1, overlay=True)
return True
n.render = _renderNormals
else:
self._geom = ode.GeomBox(self._space,
lengths=(Helpers.toODE(aabb.width),
Helpers.toODE(aabb.height),
Helpers.toODE(aabb.depth)))
self._geom.setBody(self._body)
self._body.setPosition(Helpers.toODE(node.getWorldPosition()))
node.transformationChanged.connect(self._onTransformationChanged)
print('quickstep default', world.GetQuickStepNumIterations())
world.SetQuickStepNumIterations(24)
world.SetGravity(0, 0, -.9)
space = ode.SimpleSpaceCreate()
print('space', space)
bodies = []
masses = []
for i in range(20):
body = ode.BodyCreate(world)
print(body)
bodies.append(body)
body.SetPosition(random(), random(), random())
mass = ode.Mass() # no function to create a mass?
print(mass)
mass.SetSphere(0.1 + random(), 0.5) # density, radius
print('setting mass')
body.SetMass(mass)
masses.append(mass)
print('starting sim')
start = time.time()
for i in range(500):
world.QuickStep(1.1)
print('frame', i)
for b in bodies:
b.AddForce(0.1, .0, .0)
x, y, z = b.GetPosition()
print(x, y, z)
def addSpringyCappedCylinder(self,
p1,
p2,
radius,
n_members=2,
k_bend=1e5,
k_torsion=1e5):
"""
Adds a capsule body between joint positions p1 and p2 and with given
radius to the ragdoll.
Along the axis of the capsule distribute n_joints ball joints
With spring constants given by k_bend and k_torsion
TODO: FUNCTION NOT FINISHED Wed 14 Dec 2011 06:06:24 PM EST
"""
p_start = p1 = add3(p1, self.offset)
p_end = add3(p2, self.offset)
iteration_jump = div3(sub3(p_end, p_start), n_members)
p2 = add3(p1, iteration_jump)
for i in range(n_members):
# cylinder length not including endcaps, make capsules overlap by half
# radius at joints
cyllen = dist3(p1, p2) - radius
body = ode.Body(self.sim.world)
m = ode.Mass()
m.setCappedCylinder(self.density, 3, radius, cyllen)
body.setMass(m)
# set parameters for drawing the body
body.shape = "capsule"
body.length = cyllen
body.radius = radius
# create a capsule geom for collision detection
geom = ode.GeomCCylinder(self.sim.space, radius, cyllen)
geom.setBody(body)
# define body rotation automatically from body axis
za = norm3(sub3(p2, p1))
if (abs(dot3(za, (1.0, 0.0, 0.0))) < 0.7):
xa = (1.0, 0.0, 0.0)
else:
xa = (0.0, 1.0, 0.0)
ya = cross(za, xa)
xa = norm3(cross(ya, za))
ya = cross(za, xa)
rot = (xa[0], ya[0], za[0], xa[1], ya[1], za[1], xa[2], ya[2],
za[2])
body.setPosition(mul3(add3(p1, p2), 0.5))
body.setRotation(rot)
self.bodies.append(body)
self.geoms.append(geom)
self.totalMass += body.getMass().mass
if i != n_members:
p1 = p2
p2 = add3(p1, iteration_jump)
return body
def __init__(self, bones: List[mmdpy_bone.mmdpyBone],
bodies: List[mmdpy_type.mmdpyTypePhysicsBody],
joints: List[mmdpy_type.mmdpyTypePhysicsJoint]):
self.bones: List[mmdpy_bone.mmdpyBone] = bones
self.bodies: List[mmdpy_type.mmdpyTypePhysicsBody] = bodies
self.joints: List[mmdpy_type.mmdpyTypePhysicsJoint] = joints
self.ode_bodies: List[Any] = []
self.ode_joints: List[Any] = []
self.ode_geoms: List[Any] = []
# 世界生成
self.world = ode.World()
self.world.setGravity([0, -9.81, 0])
self.world.setERP(0.80)
self.world.setCFM(1e-5)
# Create a space object
self.space = ode.Space()
# A joint group for the contact joints that are generated whenever
# two bodies collide
self.contactgroup = ode.JointGroup()
# 剛体
for i, body in enumerate(self.bodies):
body.cid = i
body.bone = self.bones[body.bone_id]
ode_body = ode.Body(self.world)
mass = ode.Mass()
density: float = 120.0
ode_geom = None
if body.type_id == 0: # 球
mass.setSphere(density, body.sizes[0])
ode_geom = ode.GeomSphere(self.space, radius=body.sizes[0])
if body.type_id == 1: # 箱
mass.setBox(density, body.sizes[0], body.sizes[1],
body.sizes[2])
ode_geom = ode.GeomBox(self.space, lengths=body.sizes)
if body.type_id == 2: # カプセル
mass.setCylinder(density, 2, body.sizes[0], body.sizes[1])
ode_geom = ode.GeomCylinder(self.space,
radius=body.sizes[0],
length=body.sizes[1])
mass.mass = (1 if body.calc == 0 else body.mass / density)
ode_body.setMass(mass)
if ode_geom is not None:
ode_geom.setBody(ode_body)
ode_body.setPosition(body.pos)
quat: np.ndarray = scipy.spatial.transform.Rotation.from_rotvec(
body.rot).as_matrix().reshape(9)
ode_body.setRotation(quat)
# debug
body.calc = 0
self.ode_bodies.append(ode_body)
self.ode_geoms.append(ode_geom)
# joint
# https://so-zou.jp/robot/tech/physics-engine/ode/joint/
for i, joint in enumerate(self.joints[:1]):
joint.cid = i
body_a = self.ode_bodies[joint.rigidbody_a]
body_b = self.ode_bodies[joint.rigidbody_b]
ode_joint = ode.BallJoint(self.world)
# ode_joint = ode.FixedJoint(self.world)
# ode_joint = ode.HingeJoint(self.world)
# ode_joint.attach(ode.environment, body_b)
ode_joint.attach(body_a, body_b)
ode_joint.setAnchor(joint.pos)
# ode_joint = ode.UniversalJoint(self.world)
# ode_joint.attach(body_a, body_b)
# ode_joint.setAnchor(joint.pos)
# ode_joint.setAxis1(joint.rot)
# ode_joint.setParam(ode.ParamLoStop, joint.)
self.ode_joints.append(ode_joint)
# # 接続確認
# print(
# self.bodies[joint.rigidbody_a].name,
# self.bodies[joint.rigidbody_b].name,
# ode.areConnected(body_a, body_b)
# )
# print(joint.pos, body_b.getPosition(), self.bodies[joint.rigidbody_b].pos)
# debug
self.bodies[joint.rigidbody_b].calc = 1
for i, body in enumerate(self.bodies):
ode_body = self.ode_bodies[i]
if body.calc == 0:
ode_joint = ode.BallJoint(self.world)
ode_joint.attach(ode.environment, ode_body)
ode_joint.setAnchor(body.pos)
self.ode_joints.append(ode_joint)
# 時刻保存
self.prev_time: float = time.time()
geoms = []
buoy_list = [v(54.5, -33.9, 0), v(53.5, -37.4, 0), v(38.0, -29.6, 0), v(35.7, -33.5, 0)]
for pos in buoy_list:
newbuoy_mesh = srh.mesh_from_obj(roslib.packages.resource_file('navigator_sim_model', 'models', 'green_buoy.obj'))
newbuoy_mesh = newbuoy_mesh.translate(pos)
g_world.objs.append(newbuoy_mesh)
geoms.append(ode.GeomTriMesh(newbuoy_mesh.ode_trimeshdata, space))
i.init(g_world)
if __name__ == "__main__":
world = ode.World()
body = ode.Body(world)
mass = ode.Mass()
space = ode.HashSpace()
g_world = srh.World()
body_geom = ode.GeomBox(space, (boat_length, boat_width, boat_height))
i = srh.Interface()
c1 = srh.Camera(g_world, "forward_camera", set_forward_view, body, fovy=90)
boat_model = Boat(body)
sim = Sim(boat_model)
setup(world, body, mass, space, g_world, body_geom, i, c1, boat_model)
def update():
try:
i.step()
c1.step()
except:
def create_objects(self, world):
print 'chassis'
# chassis
density = 10
lx, ly, lz = (8, 0.5, 8)
# Create body
body = ode.Body(world.world)
mass = ode.Mass()
mass.setBox(density, lx, ly, lz)
body.setMass(mass)
# Set parameters for drawing the body
body.shape = "box"
body.boxsize = (lx, ly, lz)
# Create a box geom for collision detection
geom = ode.GeomBox(world.space, lengths=body.boxsize)
geom.setBody(body)
#body.setPosition((0, 3, 0))
world.add_body(body)
world.add_geom(geom)
self._chassis_body = body
density = 1
print 'left wheel'
# left wheel
radius = 1
height = 0.2
px, py, pz = (lx / 2, 0, -(lz / 2))
left_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
#wheel_mass.setSphere(density, radius)
wheel_mass.setCylinder(density, 1, radius, height)
left_wheel_body.setMass(wheel_mass)
#left_wheel_geom = ode.GeomSphere(world.space, radius=radius)
left_wheel_geom = ode.GeomCylinder(world.space, radius=radius,
length=height)
left_wheel_geom.setBody(left_wheel_body)
#left_wheel_body.setPosition((px, py, pz))
left_wheel_body.setRotation((0, 0, 1,
0, 1, 0,
-1, 0, 0))
left_wheel_body.setPosition((px - height / 2, py, pz))
world.add_body(left_wheel_body)
world.add_geom(left_wheel_geom)
print 'right wheel'
# right wheel
#radius = 1
px = -lx / 2
right_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
#wheel_mass.setSphere(density, radius)
wheel_mass.setCylinder(density, 1, radius, height)
right_wheel_body.setMass(wheel_mass)
#right_wheel_geom = ode.GeomSphere(world.space, radius=radius)
right_wheel_geom = ode.GeomCylinder(world.space, radius=radius,
length=height)
right_wheel_geom.setBody(right_wheel_body)
#right_wheel_body.setPosition((px, py, pz))
right_wheel_body.setRotation((0, 0, 1,
0, 1, 0,
-1, 0, 0))
right_wheel_body.setPosition((px - height / 2, py, pz))
world.add_body(right_wheel_body)
world.add_geom(right_wheel_geom)
print 'front wheel'
# front wheel
#radius = 1
px, py, pz = (0, 0, lz / 2)
front_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
wheel_mass.setSphere(density, radius)
front_wheel_body.setMass(wheel_mass)
front_wheel_geom = ode.GeomSphere(world.space, radius=radius)
front_wheel_geom.setBody(front_wheel_body)
front_wheel_body.setPosition((px, py, pz))
world.add_body(front_wheel_body)
world.add_geom(front_wheel_geom)
#left_wheel_joint = ode.Hinge2Joint(world.world)
left_wheel_joint = ode.HingeJoint(world.world)
left_wheel_joint.attach(body, left_wheel_body)
left_wheel_joint.setAnchor(left_wheel_body.getPosition())
left_wheel_joint.setAxis((-1, 0, 0))
#left_wheel_joint.setAxis1((0, 1, 0))
#left_wheel_joint.setAxis2((1, 0, 0))
left_wheel_joint.setParam(ode.ParamFMax, 500000)
#left_wheel_joint.setParam(ode.ParamLoStop, 0)
#left_wheel_joint.setParam(ode.ParamHiStop, 0)
#left_wheel_joint.setParam(ode.ParamFMax2, 0.1)
#left_wheel_joint.setParam(ode.ParamSuspensionERP, 0.2)
#left_wheel_joint.setParam(ode.ParamSuspensionCFM, 0.1)
self._left_wheel_joints.append(left_wheel_joint)
#right_wheel_joint = ode.Hinge2Joint(world.world)
right_wheel_joint = ode.HingeJoint(world.world)
right_wheel_joint.attach(body, right_wheel_body)
right_wheel_joint.setAnchor(right_wheel_body.getPosition())
right_wheel_joint.setAxis((-1, 0, 0))
#right_wheel_joint.setAxis1((0, 1, 0))
#right_wheel_joint.setAxis2((1, 0, 0))
right_wheel_joint.setParam(ode.ParamFMax, 500000)
#right_wheel_joint.setParam(ode.ParamLoStop, 0)
#right_wheel_joint.setParam(ode.ParamHiStop, 0)
#right_wheel_joint.setParam(ode.ParamFMax2, 0.1)
#right_wheel_joint.setParam(ode.ParamSuspensionERP, 0.2)
#right_wheel_joint.setParam(ode.ParamSuspensionCFM, 0.1)
self._right_wheel_joints.append(right_wheel_joint)
front_wheel_joint = ode.BallJoint(world.world)
front_wheel_joint.attach(body, front_wheel_body)
front_wheel_joint.setAnchor(front_wheel_body.getPosition())
front_wheel_joint.setParam(ode.ParamFMax, 5000)
def create(self):
#create body
self.bodyCar = ode.Body(self.world)
mCar = ode.Mass()
mCar.setBoxTotal((self.passengers * PERSONWEIGHT + CARWEIGHT),
CARLENGTH, CARHEIGHT, CARWIDTH - WHEELWIDTH)
self.bodyCar.setMass(mCar)
self.geomCar = ode.GeomBox(
self.space, (CARLENGTH, CARHEIGHT, CARWIDTH - WHEELWIDTH))
self.geomCar.setBody(self.bodyCar)
self.bodyCar.setPosition(
(0 + self.position[0],
WHEELRADIUS + CARHEIGHT / 2 + self.position[1],
0 + self.position[2]))
self.viz.addGeom(self.geomCar)
self.viz.GetProperty(self.bodyCar).SetColor(self.bodyCarColor)
#create wheels
self.bodyFWL, self.geomFWL = self.createWheels(CARLENGTH / 2,
-CARWIDTH / 2)
self.bodyFWR, self.geomFWR = self.createWheels(CARLENGTH / 2,
CARWIDTH / 2)
self.bodyRWL, self.geomRWL = self.createWheels(-CARLENGTH / 2,
-CARWIDTH / 2)
self.bodyRWR, self.geomRWR = self.createWheels(-CARLENGTH / 2,
CARWIDTH / 2)
#create front left joint
self.flJoint = ode.Hinge2Joint(self.world)
self.flJoint.attach(self.bodyCar, self.bodyFWL)
self.flJoint.setAnchor(
(CARLENGTH / 2 + self.position[0], WHEELRADIUS + self.position[1],
-CARWIDTH / 2 + self.position[2]))
self.flJoint.setAxis2((0, 0, 1))
self.flJoint.setAxis1((0, 1, 0))
self.flJoint.setParam(ode.ParamSuspensionCFM, 0.5)
self.flJoint.setParam(ode.ParamSuspensionERP, 0.8)
#create front right joint
self.frJoint = ode.Hinge2Joint(self.world)
self.frJoint.attach(self.bodyCar, self.bodyFWR)
self.frJoint.setAnchor(
(CARLENGTH / 2 + self.position[0], WHEELRADIUS + self.position[1],
CARWIDTH / 2 + self.position[2]))
self.frJoint.setAxis2((0, 0, 1))
self.frJoint.setAxis1((0, 1, 0))
self.frJoint.setParam(ode.ParamSuspensionCFM, 0.5)
self.frJoint.setParam(ode.ParamSuspensionERP, 0.8)
#create rear left joint
self.rlJoint = ode.HingeJoint(self.world)
self.rlJoint.attach(self.bodyCar, self.bodyRWL)
self.rlJoint.setAnchor(
(-CARLENGTH / 2 + self.position[0], WHEELRADIUS + self.position[1],
-CARWIDTH / 2 + self.position[2]))
self.rlJoint.setAxis((0, 0, 1))
#create rear right joint
self.rrJoint = ode.HingeJoint(self.world)
self.rrJoint.attach(self.bodyCar, self.bodyRWR)
self.rrJoint.setAnchor(
(-CARLENGTH / 2 + self.position[0], WHEELRADIUS + self.position[1],
CARWIDTH / 2 + self.position[2]))
self.rrJoint.setAxis((0, 0, 1))
# add Joints to allGroups
allGroups.append([self.bodyFWL, self.bodyCar])
allGroups.append([self.bodyFWR, self.bodyCar])
allGroups.append([self.bodyRWL, self.bodyCar])
allGroups.append([self.bodyRWR, self.bodyCar])
#add Wheels and Road to allGroups
allGroups.append([self.bodyFWL, self.road])
allGroups.append([self.bodyFWR, self.road])
allGroups.append([self.bodyRWL, self.road])
allGroups.append([self.bodyRWR, self.road])
#create front left motor roll
self.flMotorRoll = ode.AMotor(self.world)
self.flMotorRoll.attach(self.bodyCar, self.bodyRWL)
self.flMotorRoll.setNumAxes(1)
self.flMotorRoll.setAxis(0, 2, (0, 0, 1))
self.flMotorRoll.enable()
self.flMotorRoll.setParam(ode.ParamSuspensionCFM, 0.5)
self.flMotorRoll.setParam(ode.ParamSuspensionERP, 0.8)
#create front left motor yaw
self.flMotorYaw = ode.AMotor(self.world)
self.flMotorYaw.attach(self.bodyCar, self.bodyRWL)
self.flMotorYaw.setNumAxes(1)
self.flMotorYaw.setAxis(0, 2, (0, 1, 0))
self.flMotorYaw.enable()
self.flMotorYaw.setParam(ode.ParamSuspensionCFM, 0.5)
self.flMotorYaw.setParam(ode.ParamSuspensionERP, 0.8)
#create front right motor
self.frMotorRoll = ode.AMotor(self.world)
self.frMotorRoll.attach(self.bodyCar, self.bodyRWR)
self.frMotorRoll.setNumAxes(1)
self.frMotorRoll.setAxis(0, 2, (0, 0, 1))
self.frMotorRoll.enable()
self.frMotorRoll.setParam(ode.ParamSuspensionCFM, 0.5)
self.frMotorRoll.setParam(ode.ParamSuspensionERP, 0.8)
#create front right motor
self.frMotorYaw = ode.AMotor(self.world)
self.frMotorYaw.attach(self.bodyCar, self.bodyRWR)
self.frMotorYaw.setNumAxes(1)
self.frMotorYaw.setAxis(0, 2, (0, 1, 0))
self.frMotorYaw.enable()
self.frMotorYaw.setParam(ode.ParamSuspensionCFM, 0.5)
self.frMotorYaw.setParam(ode.ParamSuspensionERP, 0.8)
# Create a world object world = ode.World() world.setGravity((0, -9.81, 0)) world.setERP(0.8) world.setCFM(1E-5) # Create a space object space = ode.Space() # Create a plane geom which prevent the objects from falling forever floor = ode.GeomPlane(space, (0, 1, 0), 0) floor.enable() body = ode.Body(world) M = ode.Mass() M.setBox(100, 1, 2, 3) #M.mass = 1.0 Tree = ode.Mass() Tree.setCylinder(100, 3, 3, 10) Treebody = ode.Body(world) Treebody.setMass(Tree) body.setMass(M) #body.addForce((0,10,0)) body.setPosition((0, 0, 0)) #body.addForce( (0,200,0) )
self.GetActiveCamera().SetPosition(0.28, 28.878, 50.654)
self.GetActiveCamera().SetFocalPoint(-0.1105, 2.8448, -0.4285)
self.GetActiveCamera().SetViewUp(0.002656, 0.9038, -0.4278)
self.SetSize(800, 600)
self.SetWindowName("Simulacao")
self.SetBackground(50. / 255, 153. / 255, 204. / 255)
def execute(self, caller, event):
idlefunc()
self.update()
viz = my_sim(world, [space], dt)
bodyCar = ode.Body(world)
mCar = ode.Mass()
mCar.setBoxTotal(800, 1.5, .10, 3.0)
bodyCar.setMass(mCar)
geomCar = ode.GeomBox(space, (1.5, .1, 3))
geomCar.setBody(bodyCar)
#bodyCar.setPosition((0 + position[0], wheelRadius + carHeight/2 + position[1], 0 + position[2]))
viz.addGeom(geomCar)
viz.GetProperty(bodyCar).SetColor(0, 0, 1)
w1, g1 = createWheels(-1, -1.5)
w2, g2 = createWheels(1, -1.5)
w3, g3 = createWheels(1, 1.5)
w4, g4 = createWheels(-1, 1.5)
j1 = ode.Hinge2Joint(world)
j1.setAnchor((-1, 0, -1.5))
j1.setAxis1((0, 1, 0))
def __init__(self, u, x, z):
Movable.__init__(self,
u) #inits ode, inventor etc and register with world
RawGLObject.__init__(
self, u
) #eg may want to overlay some HC output as GL graphics onto scene to show what I'm thinking about
mass = ode.Mass()
density = 100
axis = 0 #1x;2y;3z. But seems to be ignored? (maybe as physObject sets rotation elsewhere?)
radius = 0.5
depth = 0.1
mass.setCylinder(density, axis, radius, depth)
self.body = ode.Body(self.u.world)
self.body.setMass(mass)
self.body.setPosition((x, 0.3, z))
self.geom = ode.GeomCylinder(u.robotSpace, radius, depth)
self.geom.setBody(self.body)
th = math.pi / 2
a = math.sin(th / 2)
self.geom.setQuaternion((math.cos(th / 2), a * 1, a * 0, a * 0))
# self.sg.addChild(makeRobotSceneGraph())
# self.sg.addChild(makeRatSceneGraph())
#NB: ODE cylinders are in z-axis by default; Inventor are in y!
#It MUST be Inventor that is modified, as ODE rotations are drawn anyway by Inventor
rot = SoRotation()
th = SbRotation(SbVec3f(1, 0, 0), 3.142 / 2)
rot.rotation = th
self.sg.addChild(rot)
myCylinder = SoCylinder() #TODO rotate zo z axis height is default
myCylinder.radius = radius
myCylinder.height = depth
self.sg.addChild(myCylinder)
#simple 'head' to show the front
trans = SoTransform()
trans.translation.setValue(0.5, -0.2, 0.0)
trans.scaleFactor.setValue(.2, .2, .2)
self.sg.addChild(trans)
self.sg.addChild(SoSphere())
# self.controller = SimpleController(self)
self.controller = IceaController(self)
# self.controller = BrahmsController(self)
self.F_controller = (
0.0, 0.0, 0.0
) #Force due to controller. cache these so we dont have to run controller on every physics step
self.T_controller = (0.0, 0.0, 0.0) #Torque due to controller.
self.controller_bSetoff = False
u.robots.append(self)
self.sensors = [] #append sensors to this list!
self.segs = []
self.follicleL = Segment(u, self, 1)
self.follicleR = Segment(u, self, -1)
self.segs.append(self.follicleL)
self.segs.append(self.follicleR)
par = self.follicleL
for i in range(2, 4):
seg = Segment(u, par, i + 1)
self.segs.append(seg)
par = seg
par = self.follicleR
for i in range(2, 4):
seg = Segment(u, par, -i)
self.segs.append(seg)
par = seg
self.wheelL = Wheel(u, self, -0.6)
self.wheelR = Wheel(u, self, 0.6)
def newMass(self):
return ode.Mass()
def increase_mass(self, value):
assert self.mass
m = ode.Mass()
m.SetSphereTotal(value, 0.1)
self.mass.Add(m)
def addBP(self, bp, parent=None, joint_end=None):
"""Recursively add BodyParts to the simulation.
bp -- current BodyPart to process
parent -- parent BP to add to
joint_end -- which end of the parent ccylinder we should add to"""
log.debug('addBP')
body = ode.Body(self.world)
mass = ode.Mass()
if not parent:
# if this is the root we have an absolute length,
# root scale is relative to midpoint of min..max
bp.length = bp.scale * (
bpg.BP_MIN_LENGTH +
(bpg.BP_MAX_LENGTH - bpg.BP_MIN_LENGTH) / 2)
bp.isRoot = 1
else:
# otherwise child scale is relative to parent
bp.length = parent.length * bp.scale
bp.isRoot = 0
# limit the bp length
bp.length = min(bp.length, bpg.BP_MAX_LENGTH)
# mass along x axis, with length without caps==bp.length
# arg 3 means aligned along z-axis - must be same in renderer and Geoms
mass.setCappedCylinder(CYLINDER_DENSITY, 3, CYLINDER_RADIUS, bp.length)
# attach mass to body
body.setMass(mass)
# create Geom
# aligned along z-axis by default!!
geom = ode.GeomCCylinder(self.space, CYLINDER_RADIUS, bp.length)
self.geoms.append(geom)
self.geom_contact[geom] = 0
# remember parent for collison detection
if not parent:
geom.parent = None
else:
geom.parent = parent.geom
# attach geom to body
geom.setBody(body)
log.debug('created CappedCylinder(radius=%f, len=%f)', CYLINDER_RADIUS,
bp.length)
# assert(not in a loop)
assert not hasattr(bp, 'geom')
# ref geom from bodypart (used above to find parent geom)
bp.geom = geom
# set rotation
(radians, v) = bp.rotation
log.debug('radians,v = %f,%s', radians, str(v))
q = quat(radians, vec3(v))
rotmat = q.toMat3()
if parent:
# rotate relative to parent
p_r = mat3(parent.geom.getRotation()) # joint_end *
log.debug('parent rotation = %s', str(p_r))
rotmat = p_r * rotmat
geom.setRotation(rotmat.toList(rowmajor=1))
log.debug('r=%s', str(rotmat))
geom_axis = rotmat * vec3(0, 0, 1)
log.debug('set geom axis to %s', str(geom_axis))
(x, y, z) = geom.getBody().getRelPointPos((0, 0, bp.length / 2.0))
log.debug('real position of joint is %f,%f,%f', x, y, z)
# set position
if not parent:
# root - initially located at 0,0,0
# (once the model is constructed we translate it until all
# bodies have z>0)
geom.setPosition((0, 0, 0))
log.debug('set root geom x,y,z = 0,0,0')
else:
# child - located relative to the parent. from the
# parents position move along their axis of orientation to
# the joint position, then pick a random angle within the
# joint limits, move along that vector by half the length
# of the child cylinder, and we have the position of the
# child.
# vector from parent xyz is half of parent length along
# +/- x axis rotated by r
p_v = vec3(parent.geom.getPosition())
p_r = mat3(parent.geom.getRotation())
p_hl = parent.geom.getParams()[1] / 2.0 # half len of par
j_v = p_v + p_r * vec3(0, 0, p_hl * joint_end) # joint vector
# rotation is relative to parent
c_v = j_v + rotmat * vec3(0, 0, bp.length / 2.0)
geom.setPosition(tuple(c_v))
log.debug('set geom x,y,z = %f,%f,%f', c_v[0], c_v[1], c_v[2])
jointclass = {
'hinge': ode.HingeJoint,
'universal': ode.UniversalJoint,
'ball': ode.BallJoint
}
j = jointclass[bp.joint](self.world)
self.joints.append(j)
# attach bodies to joint
j.attach(parent.geom.getBody(), body)
# set joint position
j.setAnchor(j_v)
geom.parent_joint = j
# create motor and attach to this geom
motor = Motor(self.world, None)
motor.setJoint(j)
self.joints.append(motor)
bp.motor = motor
geom.motor = motor
motor.attach(parent.geom.getBody(), body)
motor.setMode(ode.AMotorEuler)
if bp.joint == 'hinge':
# we have 3 points - parent body, joint, child body
# find the normal to these points
# (hinge only has 1 valid axis!)
try:
axis1 = ((j_v - p_v).cross(j_v - c_v)).normalize()
except ZeroDivisionError:
v = (j_v - p_v).cross(j_v - c_v)
v.z = 1**-10
axis1 = v.normalize()
log.debug('setting hinge joint axis to %s', axis1)
log.debug('hinge axis = %s', j.getAxis())
axis_inv = rotmat.inverse() * axis1
axis2 = vec3((0, 0, 1)).cross(axis_inv)
log.debug('hinge axis2 = %s', axis2)
j.setAxis(tuple(axis1))
# some anomaly here.. if we change the order of axis2 and axis1,
# it should make no difference. instead there appears to be an
# instability when the angle switches from -pi to +pi
# so.. use parameter3 to control the hinge
# (maybe this is only a problem in the test case with perfect axis alignment?)
motor.setAxes(axis2, rotmat * axis1)
elif bp.joint == 'universal':
# bp.axis1/2 is relative to bp rotation, so rotate axes
axis1 = rotmat * vec3(bp.axis1)
axis2 = rotmat * vec3((0, 0, 1)).cross(vec3(bp.axis1))
j.setAxis1(tuple(axis1))
j.setAxis2(tuple(axis2))
motor.setAxes(axis1, axis2)
elif bp.joint == 'ball':
axis1 = rotmat * vec3(bp.axis1)
axis2 = rotmat * vec3((0, 0, 1)).cross(vec3(bp.axis1))
motor.setAxes(axis1, axis2)
log.debug('created joint with parent at %f,%f,%f', j_v[0], j_v[1],
j_v[2])
# recurse on children
geom.child_joint_ends = set([e.joint_end for e in bp.edges])
geom.parent_joint_end = joint_end
if joint_end == None:
# root
if -1 in geom.child_joint_ends:
geom.left = 'internal'
else:
geom.left = 'external'
if 1 in geom.child_joint_ends:
geom.right = 'internal'
else:
geom.right = 'external'
else:
# not root
geom.left = 'internal'
if 1 in geom.child_joint_ends:
geom.right = 'internal'
else:
geom.right = 'external'
for e in bp.edges:
self.addBP(e.child, bp, e.joint_end)
def __init__(self,
time_stop=20,
time_step=0.01,
segment_size=(1.5, 0.5, 0.5),
segment_density=0.1,
segment_count=8,
joint_lostop=-1.6,
joint_histop=1.6,
joint_fmax=10,
drag_coefficient=0.35,
vis_scale=100,
json_fname=None,
json_step=0.05):
super(Ribbot, self).__init__()
self.world = ode.World()
self.world.setGravity((0.0, 0.0, 0.0))
self.space = ode.Space()
self.contact_group = ode.JointGroup()
self.bodies = []
self.geoms = []
self.joints = []
self.ignore_collide_pairs = []
self.time = 0.0
self.time_step = time_step
self.time_stop = time_stop
json_time_step = max(time_step, json_step)
self.time_step_cnt = 0
self.json_step_mod = int(round(json_time_step / self.time_step))
self.drag_coefficient = drag_coefficient
# All segments share the same surface parameters
self.segment_surface_parameters = [{
"area":
segment_size[1] * segment_size[2],
"norm": (1, 0, 0)
}, {
"area":
segment_size[0] * segment_size[2],
"norm": (0, 1, 0)
}, {
"area":
segment_size[0] * segment_size[1],
"norm": (0, 0, 1)
}, {
"area":
segment_size[1] * segment_size[2],
"norm": (-1, 0, 0)
}, {
"area":
segment_size[0] * segment_size[2],
"norm": (0, -1, 0)
}, {
"area":
segment_size[0] * segment_size[1],
"norm": (0, 0, -1)
}]
#
# Create the ribbon robot
#
segment_mass = ode.Mass()
segment_mass.setBox(segment_density, *segment_size)
segment_position = [0.0, segment_size[1] * 1.5, 0.0]
# Create rigid bodies
for s in xrange(segment_count):
seg = ode.Body(self.world)
seg.setPosition(segment_position)
seg.setMass(segment_mass)
self.bodies.append(seg)
geom = ode.GeomBox(self.space, lengths=segment_size)
geom.setBody(seg)
self.geoms.append(geom)
segment_position[0] += segment_size[0]
# Create hinge joints
joint_position = [segment_size[0] / 2., segment_size[1] * 1.5, 0.0]
for seg1, seg2 in zip(self.bodies, self.bodies[1:]):
joint = ode.HingeJoint(self.world)
joint.attach(seg1, seg2)
joint.setAnchor(joint_position)
joint.setAxis((0, 1, 0))
joint.setParam(ode.ParamLoStop, joint_lostop)
joint.setParam(ode.ParamHiStop, joint_histop)
joint.setParam(ode.ParamFMax, joint_fmax)
self.joints.append(joint)
self.ignore_collide_pairs.append((seg1, seg2))
joint_position[0] += segment_size[0]
#
# Setup Visualization
#
self.json_fname = json_fname
if json_fname is not None:
self.VIS_SCALE = vis_scale
self.json_object = {}
self.json_object["time_start"] = 0
self.json_object["time_stop"] = time_stop
self.json_object["time_step"] = json_time_step
self.json_object["primitives"] = []
for s in xrange(segment_count):
seg = {}
seg["geometry"] = {}
seg["geometry"]["shape"] = "cube"
seg["geometry"]["sizeX"] = segment_size[0] * self.VIS_SCALE
seg["geometry"]["sizeY"] = segment_size[1] * self.VIS_SCALE
seg["geometry"]["sizeZ"] = segment_size[2] * self.VIS_SCALE
seg["dynamics"] = {}
seg["dynamics"]["position"] = []
seg["dynamics"]["quaternion"] = []
seg["dynamics"]["visible"] = [[0., time_stop]]
self.json_object["primitives"].append(seg)
self.update_json()
def __init__(self, max_simsecs=30, net=None, noise_sd=0.01, quick=0):
"""Creates the ODE and Geom bodies for this simulation"""
Sim.__init__(self, max_simsecs, noise_sd, quick)
log.debug('init PoleBalance sim')
self.network = net
CART_POSITION = (0, 0, 2)
POLE_POSITION = (0, 0, 3 + (5.0 / 2))
CART_SIZE = (8, 8, 2)
POLE_SIZE = (1, 1, 5)
HINGE_POSITION = (0, 0, 3)
CART_MASS = 10
POLE_MASS = 0.5
self.MAXF = 1000
self.INIT_U = [] # initial force, eg [10000]
self.cart_geom = ode.GeomBox(self.space, CART_SIZE)
self.geoms.append(self.cart_geom)
self.cart_body = ode.Body(self.world)
self.cart_body.setPosition(CART_POSITION)
cart_mass = ode.Mass()
cart_mass.setBoxTotal(CART_MASS, CART_SIZE[0], CART_SIZE[1],
CART_SIZE[2])
self.cart_body.setMass(cart_mass)
self.cart_geom.setBody(self.cart_body)
self.pole_geom = ode.GeomBox(self.space, POLE_SIZE)
self.geoms.append(self.pole_geom)
self.pole_body = ode.Body(self.world)
self.pole_body.setPosition(POLE_POSITION)
pole_mass = ode.Mass()
pole_mass.setBoxTotal(POLE_MASS, POLE_SIZE[0], POLE_SIZE[1],
POLE_SIZE[2])
self.pole_body.setMass(pole_mass)
self.pole_geom.setBody(self.pole_body)
self.cart_geom.setCategoryBits(long(0))
self.pole_geom.setCategoryBits(long(0))
# joint 0 - slide along 1D
self.slider_joint = ode.SliderJoint(self.world)
self.joints.append(self.slider_joint)
self.slider_joint.attach(self.cart_body, ode.environment)
self.slider_joint.setAxis((1, 0, 0))
self.slider_joint.setParam(ode.ParamLoStop, -5)
self.slider_joint.setParam(ode.ParamHiStop, 5)
# joint 1 - hinge between the two boxes
self.hinge_joint = ode.HingeJoint(self.world)
self.joints.append(self.hinge_joint)
self.hinge_joint.attach(self.cart_body, self.pole_body)
self.hinge_joint.setAnchor(HINGE_POSITION)
self.hinge_joint.setAxis((0, 1, 0))
self.last_hit = 0.0
self.init_u_count = 0
self.regular_random_force = 1
self.lqr = None
self.controlForce = 0
self.randomForce = 0
self.force_urge = 0
def loaddata(self, vpoints):
self.world = ode.World()
self.world.setGravity((0, -9.81, 0))
#vpoints = parse_vpoints(mechanism)
self.vlinks = {}
for i, vpoint in enumerate(vpoints):
for link in vpoint.links:
if link in self.vlinks:
self.vlinks[link].append(i)
else:
self.vlinks[link] = [i]
self.bodies = []
for name in tuple(self.vlinks):
if name == 'ground':
continue
vlink = self.vlinks[name]
while len(vlink) > 2:
n = vlink.pop()
for anchor in vlink[:2]:
i = 1
while 'link_{}'.format(i) in self.vlinks:
i += 1
self.vlinks['link_{}'.format(i)] = (anchor, n)
for name, vlink in self.vlinks.items():
#print(name, [(vpoints[i].cx, vpoints[i].cy) for i in vlink])
if name == 'ground':
continue
rad = 0.002
Mass = 1
link = ode.Body(self.world)
M = ode.Mass() # mass parameter
M.setZero() # init the Mass
M.setCylinderTotal(
Mass, 3, rad, self.__lenth(vpoints[vlink[0]],
vpoints[vlink[1]]))
link.setPosition(
self.__lenthCenter(vpoints[vlink[0]], vpoints[vlink[1]]))
print(vlink)
self.bodies.append(link)
self.joints = []
for name, vlink in self.vlinks.items():
link = list(vlink)
if name == 'ground':
for p in link:
if p in inputs:
print("input:", p)
j = ode.HingeJoint(self.world)
#j.attach(bodies[(vlinks[inputs[p][1]] - {p}).pop()], ode.environment)
j.attach(self.bodies[0], ode.environment)
j.setAxis((0, 0, 1))
j.setAnchor(self.__translate23d(vpoints[vlink[0]]))
j.setParam(ode.ParamVel, 2)
j.setParam(ode.ParamFMax, 22000)
else:
print("grounded:", p)
j = ode.BallJoint(self.world)
j.attach(self.bodies[p], ode.environment)
j.setAnchor(self.__translate23d(vpoints[vlink[1]]))
self.joints.append(j)
elif len(link) >= 2:
print("link:", link[0], link[1])
j = ode.BallJoint(self.world)
j.attach(self.bodies[link[0]], self.bodies[link[1]])
j.setAnchor(self.__translate23d(vpoints[link[0]]))
self.joints.append(j)
def _loadObjects(self): """ Spawns the Differential Drive robot based on the input pose. """ # Create a Box box_pos = [self.basepos[0], self.basepos[1] + self.cubesize, self.basepos[2]] rot = self.baserot boxbody = ode.Body(self.world) boxgeom = ode.GeomBox(space=self.space, lengths=(self.cubesize, self.cubesize*0.75, self.cubesize) ) boxgeom.setBody(boxbody) boxgeom.setPosition(box_pos) boxgeom.setRotation(rot) boxM = ode.Mass() boxM.setBox(self.cubemass,self.cubesize,self.cubesize*0.75,self.cubesize) boxbody.setMass(boxM) # Create two Cylindrical Wheels rot = self.multmatrix(self.genmatrix(math.pi/2,2),rot) leftwheel_pos = [box_pos[0] + 0.5*self.track, box_pos[1] - self.wheelradius*0.5, box_pos[2] + self.cubesize*0.2] rightwheel_pos = [box_pos[0] - 0.5*self.track, box_pos[1] - self.wheelradius*0.5, box_pos[2] + self.cubesize*0.2] leftwheelbody = ode.Body(self.world) leftwheelgeom = ode.GeomCylinder(space=self.space, radius = self.wheelradius, length = self.wheelradius/3.0 ) leftwheelgeom.setBody(leftwheelbody) leftwheelgeom.setPosition(leftwheel_pos) leftwheelgeom.setRotation(rot) leftwheelM = ode.Mass() leftwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius, self.wheelradius/3.0) leftwheelbody.setMass(leftwheelM) lefthinge = ode.HingeJoint(self.world) lefthinge.attach(leftwheelbody,boxbody) lefthinge.setAnchor(leftwheel_pos) lefthinge.setAxis(self.rotate((0,0,1),rot)) lefthinge.setParam(ode.ParamFMax,self.hingemaxforce) rightwheelbody = ode.Body(self.world) rightwheelgeom = ode.GeomCylinder(space=self.space, radius = self.wheelradius, length = self.wheelradius ) rightwheelgeom.setBody(rightwheelbody) rightwheelgeom.setPosition(rightwheel_pos) rightwheelgeom.setRotation(rot) rightwheelM = ode.Mass() rightwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius, self.cubemass/3.0) rightwheelbody.setMass(rightwheelM) righthinge = ode.HingeJoint(self.world) righthinge.attach(rightwheelbody,boxbody) righthinge.setAnchor(rightwheel_pos) righthinge.setAxis(self.rotate((0,0,1),rot)) righthinge.setParam(ode.ParamFMax,self.hingemaxforce) # Create a spherical wheel at the back for support caster_pos = [box_pos[0], box_pos[1] - self.cubesize*0.5, box_pos[2] - self.cubesize*0.5] casterbody = ode.Body(self.world) castergeom = ode.GeomSphere(space=self.space, radius = self.cubesize/5.0) castergeom.setBody(casterbody) castergeom.setPosition(caster_pos) castergeom.setRotation(rot) casterM = ode.Mass() casterM.setSphere(self.cubemass*100.0, self.cubesize/5.0) casterbody.setMass(casterM) self.fixed = ode.FixedJoint(self.world) self.fixed.attach(casterbody,boxbody) self.fixed.setFixed() # WHEW, THE END OF ALL THAT FINALLY! # Build the Geoms and Joints arrays for rendering. self.boxgeom = boxgeom self._geoms = [boxgeom, leftwheelgeom, rightwheelgeom, castergeom, self.ground] self.lefthinge = lefthinge self.righthinge = righthinge self._joints = [self.lefthinge, self.righthinge, self.fixed]
def __init__(self,
density=1.0,
bar_center=(0.0, 0.0, 0.0),
bar_dim=(1.0, 1.0, 1.0),
barycenters=None,
he=1.0,
cfl_target=0.9,
dt_init=0.001):
self.dt_init = dt_init
self.he = he
self.cfl_target = cfl_target
self.world = ode.World()
#self.world.setERP(0.8)
#self.world.setCFM(1E-5)
self.world.setGravity(g)
self.g = np.array([g[0], g[1], g[2]])
self.space = ode.Space()
eps_x = L[0] - 0.75 * L[0]
eps_y = L[1] - 0.75 * L[1]
#tank geometry
#self.tankWalls = [ode.GeomPlane(self.space, (1,0,0) ,x_ll[0]+eps_x),
# ode.GeomPlane(self.space, (-1,0,0),-(x_ll[0]+L[0]-eps_x)),
#ode.GeomPlane(self.space, (0,1,0) ,x_ll[1]+eps_y),
# ode.GeomPlane(self.space, (0,-1,0) ,-(x_ll[1]+L[1]-eps_y))]
#mass/intertial tensor of rigid bar
#eps_x = L[0]- 0.45*L[0]
#eps_y = L[1]- 0.45*L[1]
#self.tankWalls = [ode.GeomPlane(space, (1,0,0) ,x_ll[0]+eps_x),
# ode.GeomPlane(space, (-1,0,0),-(x_ll[0]+L[0]-eps_x)),
# ode.GeomPlane(space, (0,1,0) ,x_ll[1]+eps_y),
# ode.GeomPlane(space, (0,-1,0) ,-(x_ll[1]+L[1]-eps_y))]
#self.tank = ode.GeomBox(self.space,(0.45,0.45,0.3))
#self.tank.setPosition((0.5,0.5,0.6))
#self.contactgroup = ode.JointGroup()
self.M = ode.Mass()
self.totalMass = density * bar_dim[0] * bar_dim[1] * bar_dim[2]
self.M.setBox(density, bar_dim[0], bar_dim[1], bar_dim[2])
#bar body
self.body = ode.Body(self.world)
self.body.setMass(self.M)
self.body.setFiniteRotationMode(1)
#bar geometry
self.bar = ode.GeomBox(self.space, bar_dim)
self.bar.setBody(self.body)
self.bar.setPosition(bar_center)
self.boxsize = (bar_dim[0], bar_dim[1], bar_dim[2])
#contact joints
self.contactgroup = ode.JointGroup()
self.last_position = bar_center
self.position = bar_center
self.last_velocity = (0.0, 0.0, 0.0)
self.velocity = (0.0, 0.0, 0.0)
self.h = (0.0, 0.0, 0.0)
self.rotation = np.eye(3)
self.last_rotation = np.eye(3)
self.last_rotation_inv = np.eye(3)
self.barycenters = barycenters
self.init = True
self.bar_dim = bar_dim
self.last_F = np.zeros(3, 'd')
self.last_M = np.zeros(3, 'd')
def create_cylinder(self,
key,
density,
length,
radius,
pos,
base=0,
rot=0,
R=0.,
active_surfaces={
'x': 1,
'y': 1,
'z': 1,
'-x': 1,
'-y': 1,
'-z': 1
}):
"""Creates a cylinder body and corresponding geom.
Arguments:
key : number id to assign to the cylinder
density : density of the given body
length : length of the cylinder
radius : radius of the cylinder
pos : position of the center of the cylinder (x,y,z list)
base : place new object at negative end of base object
"""
# Auto label the joint key or not.
key = len(self.bodies) if key == -1 else key
# create cylinder body (aligned along the z-axis so that it matches the
# GeomCylinder created below, which is aligned along the z-axis by
# default)
body = ode.Body(self.world)
M = ode.Mass()
M.setCylinder(density, 3, radius, length)
body.setMass(M)
# create a cylinder geom for collision detection
geom = ode.GeomCylinder(self.space, radius, length)
geom.setBody(body)
# set the position of the cylinder
body.setPosition((pos[0], pos[1], pos[2]))
# set parameters for drawing the body
body.type = "cylinder"
body.shape = "cylinder"
body.length = length
body.radius = radius
# set the rotation of the cylinder
if (rot):
body.setRotation(self.form_rotation(rot))
# set the rotation of the cylinder directly
if R:
body.setRotation(R)
self.bodies[key] = body
self.geoms[key] = geom
if (base):
Placement.place_object(self.bodies[base], body)
if (self.fluid_dynamics):
self.create_surfaces(key, 1., active_surfaces)
return key
def __init__(self, world, space, position):
'''Yes, right now we're approximating the sub as a box
Not taking many parameters because this is the literal Sub8
thruster_layout: A list of tuples, each tuple formed as...
(thruster_name, relative_direction, position_wrt_sub_center-of-mass)
All of these in the FLU body frame
TODO:
- Independent publishing rates
See Annie for how the thrusters work
'''
lx, ly, lz = 0.5588, 0.5588, 0.381 # Meters
self.radius = max((lx, ly, lz)) * 0.365 # For spherical approximation
self.mass = 32.75 # kg
# super(self.__class__, self).__init__(world, space, position, density, lx, ly, lz)
self.body = ode.Body(world)
self.body.setPosition(position)
M = ode.Mass()
M.setBoxTotal(self.mass, lx, ly, lz)
self.body.setMass(M)
self.geom = ode.GeomBox(space, lengths=(lx, ly, lz))
self.geom.setBody(self.body)
self.truth_odom_pub = rospy.Publisher('truth/odom', Odometry, queue_size=1)
self.imu_sensor_pub = rospy.Publisher('imu', Imu, queue_size=1)
self.dvl_sensor_pub = rospy.Publisher('dvl', VelocityMeasurements, queue_size=1)
self.thruster_sub = rospy.Subscriber('thrusters/thrust', Thrust, self.thrust_cb, queue_size=2)
self.thrust_dict = {}
self.last_force = (0.0, 0.0, 0.0)
self.last_vel = np.array([0.0, 0.0, 0.0], dtype=np.float32)
self.cur_vel = np.array([0.0, 0.0, 0.0], dtype=np.float32)
self.space = space
self.g = np.array(world.getGravity())
# TODO: Fix position of DVL, IMU
# We assume that the DVL and IMU are xyz-FLU aligned (No rotation w.r.t sub)
self.dvl_position = np.array([0.0, 0.0, 0.0])
self.imu_position = np.array([0.0, 0.0, 0.0])
# Define 4 rays to implement a DVL sensor and the relative position of the sensor on the sub
self.dvl_ray_geoms = (
ode.GeomRay(None, 1e3),
ode.GeomRay(None, 1e3),
ode.GeomRay(None, 1e3),
ode.GeomRay(None, 1e3),
)
self.dvl_ray_orientations = (
np.array([+0.866, -.5, -1]),
np.array([+0.866, +.5, -1]),
np.array([-0.866, +.5, -1]),
np.array([-0.866, -.5, -1])
)
self.dvl_rays = zip(self.dvl_ray_geoms, self.dvl_ray_orientations)
# Make this a parameter
# name, relative direction, relative position (COM)
self.thruster_list = [
("FLV", np.array([+0.000, +0.0, -1.]), np.array([+0.1583, +0.1690, +0.0142])),
("FLL", np.array([-0.866, +0.5, +0.]), np.array([+0.2678, +0.2795, +0.0000])),
("FRV", np.array([+0.000, +0.0, -1.]), np.array([+0.1583, -0.1690, +0.0142])),
("FRL", np.array([-0.866, -0.5, +0.]), np.array([+0.2678, -0.2795, +0.0000])),
("BLV", np.array([+0.000, +0.0, +1.]), np.array([-0.1583, +0.1690, +0.0142])),
("BLL", np.array([+0.866, +0.5, +0.]), np.array([-0.2678, +0.2795, +0.0000])),
("BRV", np.array([+0.000, +0.0, +1.]), np.array([-0.1583, -0.1690, +0.0142])),
("BRL", np.array([+0.866, -0.5, +0.]), np.array([-0.2678, -0.2795, +0.0000])),
]
self.thrust_range = (-70, 100)
self.last_cmd_time = rospy.Time.now()
self.set_up_ros()
def __init__(self, world, body1, body2, p1, p2, hinge=None):
# Cap on one side
cap1 = ode.Body(world)
cap1.color = (0, 128, 0, 255)
m = ode.Mass()
m.setCappedCylinderTotal(1.0, 3, 0.01, 0.01)
cap1.setMass(m)
# set parameters for drawing the body
cap1.shape = "capsule"
cap1.length = .05
cap1.radius = .05
cap1.setPosition(p1)
# Attach the cap to the body
u1 = ode.BallJoint(world)
u1.attach(body1, cap1)
u1.setAnchor(p1)
u1.style = "ball"
# Cap on other side
cap2 = ode.Body(world)
cap2.color = (0, 128, 0, 255)
m = ode.Mass()
m.setCappedCylinderTotal(1.0, 3, 0.01, 0.01)
cap2.setMass(m)
# set parameters for drawing the body
cap2.shape = "capsule"
cap2.length = .05
cap2.radius = .05
cap2.setPosition(p2)
# Attach the cap to the body
u2 = ode.BallJoint(world)
u2.attach(body2, cap2)
u2.setAnchor(p2)
u2.style = "ball"
# The all-important slider joint
s = ode.SliderJoint(world)
s.attach(cap1, cap2)
s.setAxis(sub3(p1, p2))
s.setFeedback(True)
self.body1 = body1
self.body2 = body2
self.cap1 = cap1
self.cap2 = cap2
self.u1 = u1
self.u2 = u2
self.slider = s
self.gain = 1.0
self.neutral_length = dist3(p1, p2)
self.length_target = dist3(p1, p2)
if hinge is not None:
# Hinge is the joint this linear actuator controls
# This allows angular control
self.hinge = hinge
# Store these for later to save on math.
# TODO: I am being lazy and assuming u1->u2
# is orthogonal to the hinge axis
self.h_to_u1 = dist3(self.hinge.getAnchor(), self.u1.getAnchor())
self.h_to_u2 = dist3(self.hinge.getAnchor(), self.u2.getAnchor())
self.neutral_angle = thetaFromABC(self.h_to_u1, self.h_to_u2,
self.neutral_length)
def test_ode():
M_PI = 3.14159265358979323846
M_TWO_PI = 6.28318530717958647693 #// = 2 * pi
M_PI_SQRT2 = 2.22144146907918312351 # // = pi / sqrt(2)
M_PI_SQR = 9.86960440108935861883 #// = pi^2
M_RADIAN = 0.0174532925199432957692 #// = pi / 180
M_DEGREE = 57.2957795130823208768 # // = pi / 180
## vpWorld world;
# world = ode.World()
# space = ode.Space()
odeWorld = yow.OdeWorld()
world = odeWorld.world
space = odeWorld.space
## vpBody ground, pendulum;
## vpBody base, body1, body2;
base = ode.Body(world)
body1 = ode.Body(world)
body2 = ode.Body(world)
## vpBJoint J1;
## vpBJoint J2;
J1 = ode.BallJoint(world)
J2 = ode.BallJoint(world)
M1 = ode.AMotor(world)
M2 = ode.AMotor(world)
M1.setNumAxes(2)
M2.setNumAxes(2)
## Vec3 axis1(1,1,1);
## Vec3 axis2(1,0,0);
axis1 = numpy.array([1, 1, 0])
axis2 = numpy.array([1, 1, 1])
axisX = numpy.array([1, 0, 0])
axisY = numpy.array([0, 1, 0])
axisZ = numpy.array([0, 0, 1])
## scalar timeStep = .01;
## int deg1 = 0;
## int deg2 = 0;
timeStep = .01
deg1 = [0.]
deg2 = 0.
odeWorld.timeStep = timeStep
## ground.AddGeometry(new vpBox(Vec3(10, 10, 0)));
## ground.SetFrame(Vec3(0,0,-.5));
## ground.SetGround();
# // bodies
## base.AddGeometry(new vpBox(Vec3(3, 3, .5)));
## base.SetGround();
geom_base = ode.GeomBox(space, (3, .5, 3))
geom_base.setBody(base)
geom_base.setPosition((0, .5, 0))
# geom_base.setRotation(mm.SO3ToOdeSO3(mm.exp((1,0,0),math.pi/2)))
fj = ode.FixedJoint(world)
fj.attach(base, ode.environment)
fj.setFixed()
mass_base = ode.Mass()
mass_base.setBox(100, 3, .5, 3)
base.setMass(mass_base)
## body1.AddGeometry(new vpCapsule(0.2, 4), Vec3(0, 0, 2));
## body2.AddGeometry(new vpCapsule(0.2, 4), Vec3(0, 0, 2));
## body1.SetCollidable(false);
## body2.SetCollidable(false);
# geom_body1 = ode.GeomCapsule(space, .2, 4)
geom_body1 = ode.GeomBox(space, (.4, 4, .4))
geom_body1.setBody(body1)
geom_body1.setPosition((0, .5 + .25 + 2, 0))
mass_body1 = ode.Mass()
mass_body1.setBox(100, .4, 4, .4)
body1.setMass(mass_body1)
# init_ori = mm.exp((1,0,0),math.pi/2)
# geom_body1.setRotation(mm.SO3ToOdeSO3(init_ori))
# geom_body2 = ode.GeomCapsule(space, .2, 4)
geom_body2 = ode.GeomBox(space, (.4, 4, .4))
geom_body2.setBody(body2)
geom_body2.setPosition((0, .5 + .25 + 2 + 4.2, 0))
# geom_body2.setRotation(mm.SO3ToOdeSO3(mm.exp((1,0,0),math.pi/2)))
mass_body2 = ode.Mass()
mass_body2.setBox(100, .4, 4, .4)
body2.setMass(mass_body2)
## axis1.Normalize();
## axis2.Normalize();
axis1 = mm.normalize(axis1)
axis2 = mm.normalize(axis2)
## //J1.SetAxis(axis1);
## base.SetJoint(&J1, Vec3(0, 0, .1));
## body1.SetJoint(&J1, Vec3(0, 0, -.1));
J1.attach(base, body1)
J1.setAnchor((0, .5 + .25, 0))
M1.attach(base, body1)
## //J2.SetAxis(axis2);
## //body1.SetJoint(&J2, Vec3(0, 0, 4.1));
## //body2.SetJoint(&J2, Vec3(0, 0, -.1));
J2.attach(body1, body2)
J2.setAnchor((0, .5 + .25 + 4.1, 0))
M2.attach(body1, body2)
## world.AddBody(&ground);
## world.AddBody(&base);
## world.SetGravity(Vec3(0.0, 0.0, -10.0));
world.setGravity((0, 0, 0))
def PDControl(frame):
# global deg1[0], J1, J2, M1, M2
# scalar Kp = 1000.;
# scalar Kd = 100.;
Kp = 100.
Kd = 2.
# SE3 desiredOri1 = Exp(Axis(axis1), scalar(deg1[0] * M_RADIAN));
# SE3 desiredOri2 = Exp(Axis(axis2), scalar(deg2 * M_RADIAN));
desiredOri1 = mm.exp(axis1, deg1[0] * M_RADIAN)
# desiredOri2 = mm.exp(axis2, deg2 * M_RADIAN)
# se3 log1= Log(J1.GetOrientation() % desiredOri1);
# se3 log2= Log(J2.GetOrientation() % desiredOri2);
parent1 = J1.getBody(0)
child1 = J1.getBody(1)
parent1_desired_SO3 = mm.exp((0, 0, 0), 0)
child1_desired_SO3 = desiredOri1
# child1_desired_SO3 = parent1_desired_SO3
parent1_body_SO3 = mm.odeSO3ToSO3(parent1.getRotation())
child1_body_SO3 = mm.odeSO3ToSO3(child1.getRotation())
# init_ori = (mm.exp((1,0,0),math.pi/2))
# child1_body_SO3 = numpy.dot(mm.odeSO3ToSO3(child1.getRotation()), init_ori.transpose())
align_SO3 = numpy.dot(parent1_body_SO3,
parent1_desired_SO3.transpose())
child1_desired_SO3 = numpy.dot(align_SO3, child1_desired_SO3)
diff_rot = mm.logSO3(
numpy.dot(child1_desired_SO3, child1_body_SO3.transpose()))
# print diff_rot
parent_angleRate = parent1.getAngularVel()
child_angleRate = child1.getAngularVel()
# print child_angleRate
angleRate = numpy.array([
-parent_angleRate[0] + child_angleRate[0],
-parent_angleRate[1] + child_angleRate[1],
-parent_angleRate[2] + child_angleRate[2]
])
# torque1 = Kp*diff_rot - Kd*angleRate
# print torque1
# J1_ori =
# log1 = mm.logSO3_tuple(numpy.dot(desiredOri1 ,J1.GetOrientation().transpose()))
# log2 = mm.logSO3_tuple(numpy.dot(desiredOri1 ,J1.GetOrientation().transpose()))
# Vec3 torque1 = Kp*(Vec3(log1[0],log1[1],log1[2])) - Kd*J1.GetVelocity();
# Vec3 torque2 = Kp*(Vec3(log2[0],log2[1],log2[2])) - Kd*J2.GetVelocity();
M1.setAxis(0, 0, diff_rot)
M1.setAxis(1, 0, angleRate)
# M2.setAxis(0,0,torque1)
## J1.SetTorque(torque1);
## J2.SetTorque(torque2);
M1.addTorques(-Kp * mm.length(diff_rot), 0, 0)
M1.addTorques(0, Kd * mm.length(angleRate), 0)
# print diff_rot
# print angleRate
# M2.addTorques(torque2, 0, 0)
# cout << "SimulationTime " << world.GetSimulationTime() << endl;
# cout << "deg1[0] " << deg1[0] << endl;
# cout << "J1.vel " << J1.GetVelocity();
# cout << "J1.torq " << torque1;
# cout << endl;
def calcPDTorque(Rpd, Rcd, Rpc, Rcc, Wpc, Wcc, Kp, Kd, joint):
Rpc = mm.odeSO3ToSO3(Rpc)
Rcc = mm.odeSO3ToSO3(Rcc)
Ra = numpy.dot(Rpc, Rpd.transpose())
Rcd = numpy.dot(Ra, Rcd)
dR = mm.logSO3(numpy.dot(Rcd, Rcc.transpose()))
dW = numpy.array(
[-Wpc[0] + Wcc[0], -Wpc[1] + Wcc[1], -Wpc[2] + Wcc[2]])
# joint.setAxis(0,0,dR)
# joint.setAxis(1,0,dW)
joint.setAxis(0, 0, dR - dW)
# joint.addTorques(-Kp*mm.length(dR),0,0)
# joint.addTorques(0,Kd*mm.length(dW),0)
joint.addTorques(-Kp * mm.length(dR) - Kd * mm.length(dW), 0, 0)
def PDControl3(frame):
# Kp = 100.*70;
# Kd = 10.*3;
Kp = 1000.
Kd = 10.
desiredOri1 = mm.exp(axis1, deg1[0] * M_RADIAN)
desiredOriX = mm.exp(axisX, deg1[0] * M_RADIAN)
desiredOriY = mm.exp(axisY, deg1[0] * M_RADIAN)
desiredOriZ = mm.exp(axisZ, deg1[0] * M_RADIAN)
calcPDTorque(mm.I_SO3(), mm.exp(axisX, -90 * M_RADIAN),
base.getRotation(), body1.getRotation(),
base.getAngularVel(), body1.getAngularVel(), Kp, Kd,
M1)
# calcPDTorque(mm.I_SO3, desiredOriY, base.getRotation(), body1.getRotation(), base.getAngularVel(), body1.getAngularVel(), Kp, Kd, M1);
# calcPDTorque(mm.I_SO3, desiredOriX, body1.getRotation(), body2.getRotation(), body1.getAngularVel(), body2.getAngularVel(), Kp, Kd, M2);
# ground = ode.GeomPlane(space, (0,1,0), 0)
viewer = ysv.SimpleViewer()
viewer.setMaxFrame(100)
# viewer.record(False)
viewer.doc.addRenderer('object',
yr.OdeRenderer(space, (255, 255, 255)))
def preFrameCallback(frame):
# global deg1[0]
print 'deg1[0]', deg1[0]
deg1[0] += 1
# PDControl(frame)
PDControl3(frame)
viewer.setPreFrameCallback(preFrameCallback)
def simulateCallback(frame):
odeWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def create_ee(self, body, pos, collision):
body.setPosition(pos)
M = ode.Mass()
M.setCylinderTotal(0.000000001, 1, 0.000001, 0.000001)
body.setMass(M)
collision.setBody(body)
def _createBody(self, joint, parentT, posture):
T = parentT
P = mm.TransVToSE3(joint.offset)
T = numpy.dot(T, P)
# R = mm.SO3ToSE3(posture.localRMap[joint.name])
R = mm.SO3ToSE3(posture.localRs[posture.skeleton.getElementIndex(
joint.name)])
T = numpy.dot(T, R)
if len(joint.children) > 0 and joint.name in self.config.nodes:
offset = numpy.array([0., 0., 0.])
for childJoint in joint.children:
offset += childJoint.offset
offset = offset / len(joint.children)
boneT = mm.TransVToSE3(offset / 2.)
defaultBoneV = numpy.array([0, 0, 1])
boneR = mm.getSO3FromVectors(defaultBoneV, offset)
self.boneRs[joint.name] = boneR
boneT = numpy.dot(boneT, mm.SO3ToSE3(boneR))
node = OdeModel.Node(joint.name)
self.nodes[joint.name] = node
node.body = ode.Body(self.world)
mass = ode.Mass()
cfgNode = self.config.getNode(joint.name)
if cfgNode.length:
length = cfgNode.length * cfgNode.boneRatio
else:
length = mm.length(offset) * cfgNode.boneRatio
if cfgNode.width:
width = cfgNode.width
if cfgNode.mass:
height = (cfgNode.mass /
(cfgNode.density * length)) / width
else:
height = .1
else:
if cfgNode.mass:
width = (cfgNode.mass / (cfgNode.density * length))**.5
else:
width = .1
height = width
node.geom = ode.GeomBox(self.space, (width, height, length))
node.geom.name = joint.name
mass.setBox(cfgNode.density, width, height, length)
boneT = numpy.dot(boneT, mm.TransVToSE3(cfgNode.offset))
self.boneTs[joint.name] = boneT
newT = numpy.dot(T, boneT)
p = mm.SE3ToTransV(newT)
r = mm.SE3ToSO3(newT)
node.geom.setBody(node.body)
node.body.setMass(mass)
node.body.setPosition(p)
node.body.setRotation(mm.SO3ToOdeSO3(r))
for childJoint in joint.children:
self._createBody(childJoint, T, posture)
def __init__(self,
world,
space,
shape,
pos,
size=[],
bounciness=1,
friction=0,
vertices=None,
indices=None):
self.node3D = viz.addGroup()
# If it is NOT linked it updates seld.node3D pose with pos/quat pose on each frame
# If it is NOT linked it updates pos/quat pose with node3D pose on each frame
# This allows a linked phsynode to be moved around by an external source via the node3D
self.isLinked = 0
self.geom = 0
self.body = 0
self.parentWorld = []
self.parentSpace = []
self.bounciness = bounciness
self.friction = friction
# A list of bodies that it will stick to upon collision
self.stickTo_gIdx = []
self.collisionPosLocal_XYZ = []
if shape == 'plane':
# print 'phsEnv.createGeom(): type=plane expects pos=ABCD,and NO size. SIze is auto infinite.'
self.geom = ode.GeomPlane(space, [pos[0], pos[1], pos[2]], pos[3])
self.parentSpace = space
# No more work needed
elif shape == 'sphere':
#print 'Making sphere physNode'
# print 'phsEnv.createGeom(): type=sphere expects pos=XYZ, and size=RADIUS'
################################################################################################
################################################################################################
# Define the Body: something that moves as if under the
# influence of environmental physical forces
self.geomMass = ode.Mass()
# set sphere properties automatically assuming a mass of 1 and self.radius
mass = 1.0
self.geomMass.setSphereTotal(mass, size)
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
# Define the Geom: a geometric shape used to calculate collisions
#size = radius!
self.geom = ode.GeomSphere(space, size)
self.geom.setBody(self.body)
self.parentSpace = space
################################################################################################
################################################################################################
#elif shape == 'cylinder':
elif ('cylinder' in shape):
#print 'Making cylinder physNode'
# print 'phsEnv.createGeom(): type=sphere expects pos=XYZ, and size=RADIUS'
################################################################################################
################################################################################################
# Define the Body: something that moves as if under the
# influence of environmental physical forces
radius = size[1]
length = size[0]
self.geomMass = ode.Mass()
# set sphere properties automatically assuming a mass of 1 and self.radius
mass = 1.0
if (shape[-2:] == '_X'):
direction = 1
elif (shape[-2:] == '_Y'):
direction = 2
else:
direction = 3 # direction - The direction of the cylinder (1=x axis, 2=y axis, 3=z axis)
self.geomMass.setCylinderTotal(mass, direction, radius, length)
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
# Define the Geom: a geometric shape used to calculate collisions
#size = radius!
self.geom = ode.GeomCylinder(space, radius, length)
self.geom.setPosition(pos)
self.geom.setBody(self.body)
# This bit compensates for a problem with ODE
# Note how the body is created in line with any axis
# When I wrote this note, it was in-line with Y (direction=2)
# The geom, however, can only be made in-line with the Z axis
# This creates an offset to bring the two in-line
vizOffsetTrans = viz.Transform()
if (shape[-2:] == '_X'):
vizOffsetTrans.setAxisAngle(1, 0, 0, 90)
elif (shape[-2:] == '_Y'):
vizOffsetTrans.setAxisAngle(0, 0, 1, 90)
vizOffsetQuat = vizOffsetTrans.getQuat()
odeRotMat = self.vizQuatToRotationMat(vizOffsetQuat)
#print self.geom.getRotation()
self.geom.setOffsetRotation(odeRotMat)
self.parentSpace = space
elif shape == 'box':
################################################################################################
################################################################################################
# Define the Body: something that moves as if under the
# influence of environmental physical forces
length = size[1]
width = size[2]
height = size[0]
self.geomMass = ode.Mass()
# set sphere properties automatically assuming a mass of 1 and self.radius
mass = 1.0
self.geomMass.setBoxTotal(mass, length, width, height)
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
# Define the Geom: a geometric shape used to calculate collisions
#size = radius!
self.geom = ode.GeomBox(space, [length, width, height])
self.geom.setPosition(pos)
self.geom.setBody(self.body)
self.parentSpace = space
elif shape == 'trimesh':
if (vertices == None or indices == None):
print 'physNode.init(): For trimesh, must pass in vertices and indices'
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
triMeshData = ode.TrisMeshData()
triMeshData.build(vertices, indices)
self.geom = ode.GeomTriMesh(td, space)
self.geom.setBody(self.body)
## Set parameters for drawing the trimesh
body.shape = "trimesh"
body.geom = self.geom
self.parentSpace = space
else:
print 'physEnv.physNode.init(): ' + str(
type) + ' not implemented yet!'
return
pass
#print '**************UPDATING THE NODE *****************'
self.updateNodeAct = vizact.onupdate(viz.PRIORITY_PHYSICS,
self.updateNode3D)
def addBody(self, p1, p2, radius, type='cylinder'):
p1 = add3(p1, self.offset)
p2 = add3(p2, self.offset)
length = dist3(p1, p2)
body = ode.Body(self.world)
m = ode.Mass()
if type == 'cylinder':
m.setCylinder(self.density, 3, radius, length)
body.setMass(m)
body.shape = "cylinder"
body.length = length
body.radius = radius
geom = ode.GeomCylinder(self.space, radius, length)
geom.setBody(body)
if type == 'hbar':
m.setBox(self.density, H_BAR_LEN, radius, H_BAR_THICKNESS)
body.shape = "box"
body.length = length
body.width = radius
body.height = H_BAR_THICKNESS
body.len = H_BAR_LEN
geom = ode.GeomBox(self.space,
lengths=(H_BAR_LEN, radius, H_BAR_THICKNESS))
geom.setBody(body)
if type == 'vbar':
m.setBox(self.density, V_BAR_LEN, radius, V_BAR_THICKNESS)
body.shape = "box"
body.length = length
body.width = radius
body.height = V_BAR_THICKNESS
body.len = V_BAR_LEN
geom = ode.GeomBox(self.space,
lengths=(H_BAR_LEN, radius, H_BAR_THICKNESS))
geom.setBody(body)
# define body rotation automatically from body axis
za = norm3(sub3(p2, p1))
if (abs(dot3(za, (1.0, 0.0, 0.0))) < 0.7): xa = (1.0, 0.0, 0.0)
else: xa = (0.0, 1.0, 0.0)
ya = cross(za, xa)
xa = norm3(cross(ya, za))
ya = cross(za, xa)
rot = (xa[0], ya[0], za[0], xa[1], ya[1], za[1], xa[2], ya[2], za[2])
body.setPosition(mul3(add3(p1, p2), 0.5))
body.setRotation(rot)
self.bodies.append(body)
self.geoms.append(geom)
self.totalMass += body.getMass().mass
return body
def create_objects(self, world):
print 'chassis'
fmax = 5000 * 1.6
scale = 0.04
# chassis
density = 50.5
lx, ly, lz = (145 * scale, 10 * scale, 177 * scale)
# Create body
body = ode.Body(world.world)
mass = ode.Mass()
mass.setBox(density, lx, ly, lz)
body.setMass(mass)
chassis_mass = lx * ly * lz * density
print "chassis mass =", chassis_mass
# Set parameters for drawing the body
body.shape = "box"
body.boxsize = (lx, ly, lz)
# Create a box geom for collision detection
geom = ode.GeomBox(world.space, lengths=body.boxsize)
geom.setBody(body)
#body.setPosition((0, 3, 0))
world.add_body(body)
world.add_geom(geom)
self._chassis_body = body
density = 4
# left wheel
radius = 25 * scale
height = radius * 0.8
wheel_mass_ = math.pi * radius**2 * height * density
print "wheel mass =", wheel_mass_
px = lx / 2
py = 0
print 'left wheels'
for pz in (-(lz / 2), 0, lz / 2):
# cylinders
left_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
wheel_mass.setCylinder(density, 1, radius, height)
left_wheel_body.setMass(wheel_mass)
left_wheel_geom = ode.GeomCylinder(world.space,
radius=radius,
length=height)
left_wheel_geom.setBody(left_wheel_body)
left_wheel_body.setRotation((0, 0, 1, 0, 1, 0, -1, 0, 0))
left_wheel_body.setPosition((px - height / 2, py, pz))
# spheres
#left_wheel_body = ode.Body(world.world)
#wheel_mass = ode.Mass()
#wheel_mass.setSphere(density, radius)
#left_wheel_body.setMass(wheel_mass)
#left_wheel_geom = ode.GeomSphere(world.space, radius=radius)
#left_wheel_geom.setBody(left_wheel_body)
#left_wheel_body.setPosition((px, py, pz))
world.add_body(left_wheel_body)
world.add_geom(left_wheel_geom)
left_wheel_joint = ode.HingeJoint(world.world)
left_wheel_joint.attach(body, left_wheel_body)
left_wheel_joint.setAnchor(left_wheel_body.getPosition())
left_wheel_joint.setAxis((-1, 0, 0))
left_wheel_joint.setParam(ode.ParamFMax, fmax)
self._left_wheel_joints.append(left_wheel_joint)
print 'right wheels'
# right wheel
px = -(lx / 2)
for pz in (-(lz / 2), 0, lz / 2):
# cylinders
right_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
wheel_mass.setCylinder(density, 1, radius, height)
right_wheel_body.setMass(wheel_mass)
right_wheel_geom = ode.GeomCylinder(world.space,
radius=radius,
length=height)
right_wheel_geom.setBody(right_wheel_body)
right_wheel_body.setRotation((0, 0, 1, 0, 1, 0, -1, 0, 0))
right_wheel_body.setPosition((px - height / 2, py, pz))
# spheres
#right_wheel_body = ode.Body(world.world)
#wheel_mass = ode.Mass()
#wheel_mass.setSphere(density, radius)
#right_wheel_body.setMass(wheel_mass)
#right_wheel_geom = ode.GeomSphere(world.space, radius=radius)
#right_wheel_geom.setBody(right_wheel_body)
#right_wheel_body.setPosition((px, py, pz))
world.add_body(right_wheel_body)
world.add_geom(right_wheel_geom)
right_wheel_joint = ode.HingeJoint(world.world)
right_wheel_joint.attach(body, right_wheel_body)
right_wheel_joint.setAnchor(right_wheel_body.getPosition())
right_wheel_joint.setAxis((-1, 0, 0))
right_wheel_joint.setParam(ode.ParamFMax, fmax)
self._right_wheel_joints.append(right_wheel_joint)
print "total mass =", float(chassis_mass + 6 * wheel_mass_)
def create_ode_mass(self, total_mass):
mass = ode.Mass()
mass.setSphereTotal(total_mass, self.width, self.height, self.length)
return mass
