ugo_zapad/horror
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
main
horror/thirdparty/ode-0.16.5/bindings/python/ode.pyx

4507 lines
126 KiB
Cython
Raw Permalink Normal View History

first commit
2024-06-10 17:48:14 +08:00
######################################################################
# Python Open Dynamics Engine Wrapper
# Copyright (C) 2004 PyODE developers (see file AUTHORS)
# All rights reserved.
#
# This library is free software; you can redistribute it and/or
# modify it under the terms of EITHER:
# (1) The GNU Lesser General Public License as published by the Free
# Software Foundation; either version 2.1 of the License, or (at
# your option) any later version. The text of the GNU Lesser
# General Public License is included with this library in the
# file LICENSE.
# (2) The BSD-style license that is included with this library in
# the file LICENSE-BSD.
#
# This library is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
# LICENSE and LICENSE-BSD for more details.
######################################################################
from ode cimport *
paramLoStop = 0
paramHiStop = 1
paramVel = 2
paramLoVel = 3
paramHiVel = 4
paramFMax = 5
paramFudgeFactor = 6
paramBounce = 7
paramCFM = 8
paramStopERP = 9
paramStopCFM = 10
paramSuspensionERP = 11
paramSuspensionCFM = 12
paramERP = 13
ParamLoStop = 0
ParamHiStop = 1
ParamVel = 2
ParamLoVel = 3
ParamHiVel = 4
ParamFMax = 5
ParamFudgeFactor = 6
ParamBounce = 7
ParamCFM = 8
ParamStopERP = 9
ParamStopCFM = 10
ParamSuspensionERP = 11
ParamSuspensionCFM = 12
ParamERP = 13
ParamLoStop2 = 256 + 0
ParamHiStop2 = 256 + 1
ParamVel2 = 256 + 2
ParamLoVel2 = 256 + 3
ParamHiVel2 = 256 + 4
ParamFMax2 = 256 + 5
ParamFudgeFactor2 = 256 + 6
ParamBounce2 = 256 + 7
ParamCFM2 = 256 + 8
ParamStopERP2 = 256 + 9
ParamStopCFM2 = 256 + 10
ParamSuspensionERP2 = 256 + 11
ParamSuspensionCFM2 = 256 + 12
ParamERP2 = 256 + 13
ParamLoStop3 = 512 + 0
ParamHiStop3 = 512 + 1
ParamVel3 = 512 + 2
ParamLoVel3 = 512 + 3
ParamHiVel3 = 512 + 4
ParamFMax3 = 512 + 5
ParamFudgeFactor3 = 512 + 6
ParamBounce3 = 512 + 7
ParamCFM3 = 512 + 8
ParamStopERP3 = 512 + 9
ParamStopCFM3 = 512 + 10
ParamSuspensionERP3 = 512 + 11
ParamSuspensionCFM3 = 512 + 12
ParamERP3 = 512 + 13
ParamGroup = 256
ContactMu2 = 0x001
ContactAxisDep = 0x001
ContactFDir1 = 0x002
ContactBounce = 0x004
ContactSoftERP = 0x008
ContactSoftCFM = 0x010
ContactMotion1 = 0x020
ContactMotion2 = 0x040
ContactMotionN = 0x080
ContactSlip1 = 0x100
ContactSlip2 = 0x200
ContactRolling = 0x400
ContactApprox0 = 0x0000
ContactApprox1_1 = 0x1000
ContactApprox1_2 = 0x2000
ContactApprox1_N = 0x4000
ContactApprox1 = 0x7000
AMotorUser = dAMotorUser
AMotorEuler = dAMotorEuler
Infinity = dInfinity
import weakref
_geom_c2py_lut = weakref.WeakValueDictionary()
cdef class Mass:
"""Mass parameters of a rigid body.
This class stores mass parameters of a rigid body which can be
accessed through the following attributes:
- mass: The total mass of the body (float)
- c: The center of gravity position in body frame (3-tuple of floats)
- I: The 3x3 inertia tensor in body frame (3-tuple of 3-tuples)
This class wraps the dMass structure from the C API.
@ivar mass: The total mass of the body
@ivar c: The center of gravity position in body frame (cx, cy, cz)
@ivar I: The 3x3 inertia tensor in body frame ((I11, I12, I13), (I12, I22, I23), (I13, I23, I33))
@type mass: float
@type c: 3-tuple of floats
@type I: 3-tuple of 3-tuples of floats
"""
cdef dMass _mass
def __cinit__(self):
dMassSetZero(&self._mass)
def setZero(self):
"""setZero()
Set all the mass parameters to zero."""
dMassSetZero(&self._mass)
def setParameters(self, mass, cgx, cgy, cgz, I11, I22, I33, I12, I13, I23):
"""setParameters(mass, cgx, cgy, cgz, I11, I22, I33, I12, I13, I23)
Set the mass parameters to the given values.
@param mass: Total mass of the body.
@param cgx: Center of gravity position in the body frame (x component).
@param cgy: Center of gravity position in the body frame (y component).
@param cgz: Center of gravity position in the body frame (z component).
@param I11: Inertia tensor
@param I22: Inertia tensor
@param I33: Inertia tensor
@param I12: Inertia tensor
@param I13: Inertia tensor
@param I23: Inertia tensor
@type mass: float
@type cgx: float
@type cgy: float
@type cgz: float
@type I11: float
@type I22: float
@type I33: float
@type I12: float
@type I13: float
@type I23: float
"""
dMassSetParameters(&self._mass, mass, cgx, cgy, cgz,
I11, I22, I33, I12, I13, I23)
def setSphere(self, density, radius):
"""setSphere(density, radius)
Set the mass parameters to represent a sphere of the given radius
and density, with the center of mass at (0,0,0) relative to the body.
@param density: The density of the sphere
@param radius: The radius of the sphere
@type density: float
@type radius: float
"""
dMassSetSphere(&self._mass, density, radius)
def setSphereTotal(self, total_mass, radius):
"""setSphereTotal(total_mass, radius)
Set the mass parameters to represent a sphere of the given radius
and mass, with the center of mass at (0,0,0) relative to the body.
@param total_mass: The total mass of the sphere
@param radius: The radius of the sphere
@type total_mass: float
@type radius: float
"""
dMassSetSphereTotal(&self._mass, total_mass, radius)
def setCapsule(self, density, direction, radius, length):
"""setCapsule(density, direction, radius, length)
Set the mass parameters to represent a capsule of the given parameters
and density, with the center of mass at (0,0,0) relative to the body.
The radius of the cylinder (and the spherical cap) is radius. The length
of the cylinder (not counting the spherical cap) is length. The
cylinder's long axis is oriented along the body's x, y or z axis
according to the value of direction (1=x, 2=y, 3=z). The first function
accepts the density of the object, the second accepts its total mass.
@param density: The density of the capsule
@param direction: The direction of the capsule's cylinder (1=x axis, 2=y axis, 3=z axis)
@param radius: The radius of the capsule's cylinder
@param length: The length of the capsule's cylinder (without the caps)
@type density: float
@type direction: int
@type radius: float
@type length: float
"""
dMassSetCapsule(&self._mass, density, direction, radius, length)
def setCapsuleTotal(self, total_mass, direction, radius, length):
"""setCapsuleTotal(total_mass, direction, radius, length)
Set the mass parameters to represent a capsule of the given parameters
and mass, with the center of mass at (0,0,0) relative to the body. The
radius of the cylinder (and the spherical cap) is radius. The length of
the cylinder (not counting the spherical cap) is length. The cylinder's
long axis is oriented along the body's x, y or z axis according to the
value of direction (1=x, 2=y, 3=z). The first function accepts the
density of the object, the second accepts its total mass.
@param total_mass: The total mass of the capsule
@param direction: The direction of the capsule's cylinder (1=x axis, 2=y axis, 3=z axis)
@param radius: The radius of the capsule's cylinder
@param length: The length of the capsule's cylinder (without the caps)
@type total_mass: float
@type direction: int
@type radius: float
@type length: float
"""
dMassSetCapsuleTotal(&self._mass, total_mass, direction,
radius, length)
def setCylinder(self, density, direction, r, h):
"""setCylinder(density, direction, r, h)
Set the mass parameters to represent a flat-ended cylinder of
the given parameters and density, with the center of mass at
(0,0,0) relative to the body. The radius of the cylinder is r.
The length of the cylinder is h. The cylinder's long axis is
oriented along the body's x, y or z axis according to the value
of direction (1=x, 2=y, 3=z).
@param density: The density of the cylinder
@param direction: The direction of the cylinder (1=x axis, 2=y axis, 3=z axis)
@param r: The radius of the cylinder
@param h: The length of the cylinder
@type density: float
@type direction: int
@type r: float
@type h: float
"""
dMassSetCylinder(&self._mass, density, direction, r, h)
def setCylinderTotal(self, total_mass, direction, r, h):
"""setCylinderTotal(total_mass, direction, r, h)
Set the mass parameters to represent a flat-ended cylinder of
the given parameters and mass, with the center of mass at
(0,0,0) relative to the body. The radius of the cylinder is r.
The length of the cylinder is h. The cylinder's long axis is
oriented along the body's x, y or z axis according to the value
of direction (1=x, 2=y, 3=z).
@param total_mass: The total mass of the cylinder
@param direction: The direction of the cylinder (1=x axis, 2=y axis, 3=z axis)
@param r: The radius of the cylinder
@param h: The length of the cylinder
@type total_mass: float
@type direction: int
@type r: float
@type h: float
"""
dMassSetCylinderTotal(&self._mass, total_mass, direction, r, h)
def setBox(self, density, lx, ly, lz):
"""setBox(density, lx, ly, lz)
Set the mass parameters to represent a box of the given
dimensions and density, with the center of mass at (0,0,0)
relative to the body. The side lengths of the box along the x,
y and z axes are lx, ly and lz.
@param density: The density of the box
@param lx: The length along the x axis
@param ly: The length along the y axis
@param lz: The length along the z axis
@type density: float
@type lx: float
@type ly: float
@type lz: float
"""
dMassSetBox(&self._mass, density, lx, ly, lz)
def setBoxTotal(self, total_mass, lx, ly, lz):
"""setBoxTotal(total_mass, lx, ly, lz)
Set the mass parameters to represent a box of the given
dimensions and mass, with the center of mass at (0,0,0)
relative to the body. The side lengths of the box along the x,
y and z axes are lx, ly and lz.
@param total_mass: The total mass of the box
@param lx: The length along the x axis
@param ly: The length along the y axis
@param lz: The length along the z axis
@type total_mass: float
@type lx: float
@type ly: float
@type lz: float
"""
dMassSetBoxTotal(&self._mass, total_mass, lx, ly, lz)
def adjust(self, newmass):
"""adjust(newmass)
Adjust the total mass. Given mass parameters for some object,
adjust them so the total mass is now newmass. This is useful
when using the setXyz() methods to set the mass parameters for
certain objects - they take the object density, not the total
mass.
@param newmass: The new total mass
@type newmass: float
"""
dMassAdjust(&self._mass, newmass)
def translate(self, t):
"""translate(t)
Adjust mass parameters. Given mass parameters for some object,
adjust them to represent the object displaced by (x,y,z)
relative to the body frame.
@param t: Translation vector (x, y, z)
@type t: 3-tuple of floats
"""
dMassTranslate(&self._mass, t[0], t[1], t[2])
# def rotate(self, R):
# """
# Given mass parameters for some object, adjust them to
# represent the object rotated by R relative to the body frame.
# """
# pass
def add(self, Mass b):
"""add(b)
Add the mass b to the mass object. Masses can also be added using
the + operator.
@param b: The mass to add to this mass
@type b: Mass
"""
dMassAdd(&self._mass, &b._mass)
def __getattr__(self, name):
if name == "mass":
return self._mass.mass
elif name == "c":
return self._mass.c[0], self._mass.c[1], self._mass.c[2]
elif name == "I":
return ((self._mass.I[0], self._mass.I[1], self._mass.I[2]),
(self._mass.I[4], self._mass.I[5], self._mass.I[6]),
(self._mass.I[8], self._mass.I[9], self._mass.I[10]))
else:
raise AttributeError("Mass object has no attribute '%s'" % name)
def __setattr__(self, name, value):
if name == "mass":
self.adjust(value)
elif name == "c":
raise AttributeError("Use the setParameter() method to change c")
elif name == "I":
raise AttributeError("Use the setParameter() method to change I")
else:
raise AttributeError("Mass object has no attribute '%s" % name)
def __add__(self, Mass b):
self.add(b)
return self
def __str__(self):
m = str(self._mass.mass)
sc0 = str(self._mass.c[0])
sc1 = str(self._mass.c[1])
sc2 = str(self._mass.c[2])
I11 = str(self._mass.I[0])
I22 = str(self._mass.I[5])
I33 = str(self._mass.I[10])
I12 = str(self._mass.I[1])
I13 = str(self._mass.I[2])
I23 = str(self._mass.I[6])
return ("Mass=%s\n"
"Cg=(%s, %s, %s)\n"
"I11=%s I22=%s I33=%s\n"
"I12=%s I13=%s I23=%s" %
(m, sc0, sc1, sc2, I11, I22, I33, I12, I13, I23))
# return ("Mass=%s / "
# "Cg=(%s, %s, %s) / "
# "I11=%s I22=%s I33=%s "
# "I12=%s I13=%s I23=%s" %
# (m, sc0, sc1, sc2, I11, I22, I33, I12, I13, I23))
cdef class Contact:
"""This class represents a contact between two bodies in one point.
A Contact object stores all the input parameters for a ContactJoint.
This class wraps the ODE dContact structure which has 3 components::
struct dContact {
dSurfaceParameters surface;
dContactGeom geom;
dVector3 fdir1;
};
This wrapper class provides methods to get and set the items of those
structures.
"""
cdef dContact _contact
def __cinit__(self):
self._contact.surface.mode = ContactBounce
self._contact.surface.mu = dInfinity
self._contact.surface.bounce = 0.1
# getMode
def getMode(self):
"""getMode() -> flags
Return the contact flags.
"""
return self._contact.surface.mode
# setMode
def setMode(self, flags):
"""setMode(flags)
Set the contact flags. The argument m is a combination of the
ContactXyz flags (ContactMu2, ContactBounce, ...).
@param flags: Contact flags
@type flags: int
"""
self._contact.surface.mode = flags
# getMu
def getMu(self):
"""getMu() -> float
Return the Coulomb friction coefficient.
"""
return self._contact.surface.mu
# setMu
def setMu(self, mu):
"""setMu(mu)
Set the Coulomb friction coefficient.
@param mu: Coulomb friction coefficient (0..Infinity)
@type mu: float
"""
self._contact.surface.mu = mu
# getMu2
def getMu2(self):
"""getMu2() -> float
Return the optional Coulomb friction coefficient for direction 2.
"""
return self._contact.surface.mu2
# setMu2
def setMu2(self, mu):
"""setMu2(mu)
Set the optional Coulomb friction coefficient for direction 2.
@param mu: Coulomb friction coefficient (0..Infinity)
@type mu: float
"""
self._contact.surface.mu2 = mu
# getBounce
def getBounce(self):
"""getBounce() -> float
Return the restitution parameter.
"""
return self._contact.surface.bounce
# setBounce
def setBounce(self, b):
"""setBounce(b)
@param b: Restitution parameter (0..1)
@type b: float
"""
self._contact.surface.bounce = b
# getBounceVel
def getBounceVel(self):
"""getBounceVel() -> float
Return the minimum incoming velocity necessary for bounce.
"""
return self._contact.surface.bounce_vel
# setBounceVel
def setBounceVel(self, bv):
"""setBounceVel(bv)
Set the minimum incoming velocity necessary for bounce. Incoming
velocities below this will effectively have a bounce parameter of 0.
@param bv: Velocity
@type bv: float
"""
self._contact.surface.bounce_vel = bv
# getSoftERP
def getSoftERP(self):
"""getSoftERP() -> float
Return the contact normal "softness" parameter.
"""
return self._contact.surface.soft_erp
# setSoftERP
def setSoftERP(self, erp):
"""setSoftERP(erp)
Set the contact normal "softness" parameter.
@param erp: Softness parameter
@type erp: float
"""
self._contact.surface.soft_erp = erp
# getSoftCFM
def getSoftCFM(self):
"""getSoftCFM() -> float
Return the contact normal "softness" parameter.
"""
return self._contact.surface.soft_cfm
# setSoftCFM
def setSoftCFM(self, cfm):
"""setSoftCFM(cfm)
Set the contact normal "softness" parameter.
@param cfm: Softness parameter
@type cfm: float
"""
self._contact.surface.soft_cfm = cfm
# getMotion1
def getMotion1(self):
"""getMotion1() -> float
Get the surface velocity in friction direction 1.
"""
return self._contact.surface.motion1
# setMotion1
def setMotion1(self, m):
"""setMotion1(m)
Set the surface velocity in friction direction 1.
@param m: Surface velocity
@type m: float
"""
self._contact.surface.motion1 = m
# getMotion2
def getMotion2(self):
"""getMotion2() -> float
Get the surface velocity in friction direction 2.
"""
return self._contact.surface.motion2
# setMotion2
def setMotion2(self, m):
"""setMotion2(m)
Set the surface velocity in friction direction 2.
@param m: Surface velocity
@type m: float
"""
self._contact.surface.motion2 = m
# getSlip1
def getSlip1(self):
"""getSlip1() -> float
Get the coefficient of force-dependent-slip (FDS) for friction
direction 1.
"""
return self._contact.surface.slip1
# setSlip1
def setSlip1(self, s):
"""setSlip1(s)
Set the coefficient of force-dependent-slip (FDS) for friction
direction 1.
@param s: FDS coefficient
@type s: float
"""
self._contact.surface.slip1 = s
# getSlip2
def getSlip2(self):
"""getSlip2() -> float
Get the coefficient of force-dependent-slip (FDS) for friction
direction 2.
"""
return self._contact.surface.slip2
# setSlip2
def setSlip2(self, s):
"""setSlip2(s)
Set the coefficient of force-dependent-slip (FDS) for friction
direction 1.
@param s: FDS coefficient
@type s: float
"""
self._contact.surface.slip2 = s
# getFDir1
def getFDir1(self):
"""getFDir1() -> (x, y, z)
Get the "first friction direction" vector that defines a direction
along which frictional force is applied.
"""
return (self._contact.fdir1[0],
self._contact.fdir1[1],
self._contact.fdir1[2])
# setFDir1
def setFDir1(self, fdir):
"""setFDir1(fdir)
Set the "first friction direction" vector that defines a direction
along which frictional force is applied. It must be of unit length
and perpendicular to the contact normal (so it is typically
tangential to the contact surface).
@param fdir: Friction direction
@type fdir: 3-sequence of floats
"""
self._contact.fdir1[0] = fdir[0]
self._contact.fdir1[1] = fdir[1]
self._contact.fdir1[2] = fdir[2]
# getContactGeomParams
def getContactGeomParams(self):
"""getContactGeomParams() -> (pos, normal, depth, geom1, geom2)
Get the ContactGeom structure of the contact.
The return value is a tuple (pos, normal, depth, geom1, geom2)
where pos and normal are 3-tuples of floats and depth is a single
float. geom1 and geom2 are the Geom objects of the geoms in contact.
"""
cdef size_t id1, id2
pos = (self._contact.geom.pos[0],
self._contact.geom.pos[1],
self._contact.geom.pos[2])
normal = (self._contact.geom.normal[0],
self._contact.geom.normal[1],
self._contact.geom.normal[2])
depth = self._contact.geom.depth
id1 = <size_t>self._contact.geom.g1
id2 = <size_t>self._contact.geom.g2
g1 = _geom_c2py_lut[id1]
g2 = _geom_c2py_lut[id2]
return pos, normal, depth, g1, g2
# setContactGeomParams
def setContactGeomParams(self, pos, normal, depth, g1=None, g2=None):
"""setContactGeomParams(pos, normal, depth, geom1=None, geom2=None)
Set the ContactGeom structure of the contact.
@param pos: Contact position, in global coordinates
@type pos: 3-sequence of floats
@param normal: Unit length normal vector
@type normal: 3-sequence of floats
@param depth: Depth to which the two bodies inter-penetrate
@type depth: float
@param geom1: Geometry object 1 that collided
@type geom1: Geom
@param geom2: Geometry object 2 that collided
@type geom2: Geom
"""
cdef size_t id
self._contact.geom.pos[0] = pos[0]
self._contact.geom.pos[1] = pos[1]
self._contact.geom.pos[2] = pos[2]
self._contact.geom.normal[0] = normal[0]
self._contact.geom.normal[1] = normal[1]
self._contact.geom.normal[2] = normal[2]
self._contact.geom.depth = depth
if g1 != None:
id = g1._id()
self._contact.geom.g1 = <dGeomID>id
else:
self._contact.geom.g1 = <dGeomID>0
if g2 != None:
id = g2._id()
self._contact.geom.g2 = <dGeomID>id
else:
self._contact.geom.g2 = <dGeomID>0
# World
cdef class World:
"""Dynamics world.
The world object is a container for rigid bodies and joints.
Constructor::
World()
"""
cdef dWorldID wid
def __cinit__(self):
self.wid = dWorldCreate()
def __dealloc__(self):
if self.wid != NULL:
dWorldDestroy(self.wid)
# setGravity
def setGravity(self, gravity):
"""setGravity(gravity)
Set the world's global gravity vector.
@param gravity: Gravity vector
@type gravity: 3-sequence of floats
"""
dWorldSetGravity(self.wid, gravity[0], gravity[1], gravity[2])
# getGravity
def getGravity(self):
"""getGravity() -> 3-tuple
Return the world's global gravity vector as a 3-tuple of floats.
"""
cdef dVector3 g
dWorldGetGravity(self.wid, g)
return g[0], g[1], g[2]
# setERP
def setERP(self, erp):
"""setERP(erp)
Set the global ERP value, that controls how much error
correction is performed in each time step. Typical values are
in the range 0.1-0.8. The default is 0.2.
@param erp: Global ERP value
@type erp: float
"""
dWorldSetERP(self.wid, erp)
# getERP
def getERP(self):
"""getERP() -> float
Get the global ERP value, that controls how much error
correction is performed in each time step. Typical values are
in the range 0.1-0.8. The default is 0.2.
"""
return dWorldGetERP(self.wid)
# setCFM
def setCFM(self, cfm):
"""setCFM(cfm)
Set the global CFM (constraint force mixing) value. Typical
values are in the range 10E-9 - 1. The default is 10E-5 if
single precision is being used, or 10E-10 if double precision
is being used.
@param cfm: Constraint force mixing value
@type cfm: float
"""
dWorldSetCFM(self.wid, cfm)
# getCFM
def getCFM(self):
"""getCFM() -> float
Get the global CFM (constraint force mixing) value. Typical
values are in the range 10E-9 - 1. The default is 10E-5 if
single precision is being used, or 10E-10 if double precision
is being used.
"""
return dWorldGetCFM(self.wid)
# step
def step(self, stepsize):
"""step(stepsize)
Step the world. This uses a "big matrix" method that takes
time on the order of O(m3) and memory on the order of O(m2), where m
is the total number of constraint rows.
For large systems this will use a lot of memory and can be
very slow, but this is currently the most accurate method.
@param stepsize: Time step
@type stepsize: float
"""
dWorldStep(self.wid, stepsize)
# quickStep
def quickStep(self, stepsize):
"""quickStep(stepsize)
Step the world. This uses an iterative method that takes time
on the order of O(m*N) and memory on the order of O(m), where m is
the total number of constraint rows and N is the number of
iterations.
For large systems this is a lot faster than dWorldStep, but it
is less accurate.
@param stepsize: Time step
@type stepsize: float
"""
dWorldQuickStep(self.wid, stepsize)
# setQuickStepNumIterations
def setQuickStepNumIterations(self, num):
"""setQuickStepNumIterations(num)
Set the number of iterations that the QuickStep method
performs per step. More iterations will give a more accurate
solution, but will take longer to compute. The default is 20
iterations.
@param num: Number of iterations
@type num: int
"""
dWorldSetQuickStepNumIterations(self.wid, num)
# getQuickStepNumIterations
def getQuickStepNumIterations(self):
"""getQuickStepNumIterations() -> int
Get the number of iterations that the QuickStep method
performs per step. More iterations will give a more accurate
solution, but will take longer to compute. The default is 20
iterations.
"""
return dWorldGetQuickStepNumIterations(self.wid)
# setQuickStepNumIterations
def setContactMaxCorrectingVel(self, vel):
"""setContactMaxCorrectingVel(vel)
Set the maximum correcting velocity that contacts are allowed
to generate. The default value is infinity (i.e. no
limit). Reducing this value can help prevent "popping" of
deeply embedded objects.
@param vel: Maximum correcting velocity
@type vel: float
"""
dWorldSetContactMaxCorrectingVel(self.wid, vel)
# getQuickStepNumIterations
def getContactMaxCorrectingVel(self):
"""getContactMaxCorrectingVel() -> float
Get the maximum correcting velocity that contacts are allowed
to generate. The default value is infinity (i.e. no
limit). Reducing this value can help prevent "popping" of
deeply embedded objects.
"""
return dWorldGetContactMaxCorrectingVel(self.wid)
# setContactSurfaceLayer
def setContactSurfaceLayer(self, depth):
"""setContactSurfaceLayer(depth)
Set the depth of the surface layer around all geometry
objects. Contacts are allowed to sink into the surface layer
up to the given depth before coming to rest. The default value
is zero. Increasing this to some small value (e.g. 0.001) can
help prevent jittering problems due to contacts being
repeatedly made and broken.
@param depth: Surface layer depth
@type depth: float
"""
dWorldSetContactSurfaceLayer(self.wid, depth)
# getContactSurfaceLayer
def getContactSurfaceLayer(self):
"""getContactSurfaceLayer()
Get the depth of the surface layer around all geometry
objects. Contacts are allowed to sink into the surface layer
up to the given depth before coming to rest. The default value
is zero. Increasing this to some small value (e.g. 0.001) can
help prevent jittering problems due to contacts being
repeatedly made and broken.
"""
return dWorldGetContactSurfaceLayer(self.wid)
# setAutoDisableFlag
def setAutoDisableFlag(self, flag):
"""setAutoDisableFlag(flag)
Set the default auto-disable flag for newly created bodies.
@param flag: True = Do auto disable
@type flag: bool
"""
dWorldSetAutoDisableFlag(self.wid, flag)
# getAutoDisableFlag
def getAutoDisableFlag(self):
"""getAutoDisableFlag() -> bool
Get the default auto-disable flag for newly created bodies.
"""
return dWorldGetAutoDisableFlag(self.wid)
# setAutoDisableLinearThreshold
def setAutoDisableLinearThreshold(self, threshold):
"""setAutoDisableLinearThreshold(threshold)
Set the default auto-disable linear threshold for newly created
bodies.
@param threshold: Linear threshold
@type threshold: float
"""
dWorldSetAutoDisableLinearThreshold(self.wid, threshold)
# getAutoDisableLinearThreshold
def getAutoDisableLinearThreshold(self):
"""getAutoDisableLinearThreshold() -> float
Get the default auto-disable linear threshold for newly created
bodies.
"""
return dWorldGetAutoDisableLinearThreshold(self.wid)
# setAutoDisableAngularThreshold
def setAutoDisableAngularThreshold(self, threshold):
"""setAutoDisableAngularThreshold(threshold)
Set the default auto-disable angular threshold for newly created
bodies.
@param threshold: Angular threshold
@type threshold: float
"""
dWorldSetAutoDisableAngularThreshold(self.wid, threshold)
# getAutoDisableAngularThreshold
def getAutoDisableAngularThreshold(self):
"""getAutoDisableAngularThreshold() -> float
Get the default auto-disable angular threshold for newly created
bodies.
"""
return dWorldGetAutoDisableAngularThreshold(self.wid)
# setAutoDisableSteps
def setAutoDisableSteps(self, steps):
"""setAutoDisableSteps(steps)
Set the default auto-disable steps for newly created bodies.
@param steps: Auto disable steps
@type steps: int
"""
dWorldSetAutoDisableSteps(self.wid, steps)
# getAutoDisableSteps
def getAutoDisableSteps(self):
"""getAutoDisableSteps() -> int
Get the default auto-disable steps for newly created bodies.
"""
return dWorldGetAutoDisableSteps(self.wid)
# setAutoDisableTime
def setAutoDisableTime(self, time):
"""setAutoDisableTime(time)
Set the default auto-disable time for newly created bodies.
@param time: Auto disable time
@type time: float
"""
dWorldSetAutoDisableTime(self.wid, time)
# getAutoDisableTime
def getAutoDisableTime(self):
"""getAutoDisableTime() -> float
Get the default auto-disable time for newly created bodies.
"""
return dWorldGetAutoDisableTime(self.wid)
# setLinearDamping
def setLinearDamping(self, scale):
"""setLinearDamping(scale)
Set the world's linear damping scale.
@param scale The linear damping scale that is to be applied to bodies.
Default is 0 (no damping). Should be in the interval [0, 1].
@type scale: float
"""
dWorldSetLinearDamping(self.wid, scale)
# getLinearDamping
def getLinearDamping(self):
"""getLinearDamping() -> float
Get the world's linear damping scale.
"""
return dWorldGetLinearDamping(self.wid)
# setAngularDamping
def setAngularDamping(self, scale):
"""setAngularDamping(scale)
Set the world's angular damping scale.
@param scale The angular damping scale that is to be applied to bodies.
Default is 0 (no damping). Should be in the interval [0, 1].
@type scale: float
"""
dWorldSetAngularDamping(self.wid, scale)
# getAngularDamping
def getAngularDamping(self):
"""getAngularDamping() -> float
Get the world's angular damping scale.
"""
return dWorldGetAngularDamping(self.wid)
# impulseToForce
def impulseToForce(self, stepsize, impulse):
"""impulseToForce(stepsize, impulse) -> 3-tuple
If you want to apply a linear or angular impulse to a rigid
body, instead of a force or a torque, then you can use this
function to convert the desired impulse into a force/torque
vector before calling the dBodyAdd... function.
@param stepsize: Time step
@param impulse: Impulse vector
@type stepsize: float
@type impulse: 3-tuple of floats
"""
cdef dVector3 force
dWorldImpulseToForce(self.wid, stepsize,
impulse[0], impulse[1], impulse[2], force)
return force[0], force[1], force[2]
# createBody
# def createBody(self):
# return Body(self)
# createBallJoint
# def createBallJoint(self, jointgroup=None):
# return BallJoint(self, jointgroup)
# createHingeJoint
# def createHingeJoint(self, jointgroup=None):
# return HingeJoint(self, jointgroup)
# createHinge2Joint
# def createHinge2Joint(self, jointgroup=None):
# return Hinge2Joint(self, jointgroup)
# createSliderJoint
# def createSliderJoint(self, jointgroup=None):
# return SliderJoint(self, jointgroup)
# createFixedJoint
# def createFixedJoint(self, jointgroup=None):
# return FixedJoint(self, jointgroup)
# createContactJoint
# def createContactJoint(self, jointgroup, contact):
# return ContactJoint(self, jointgroup, contact)
# Body
cdef class Body:
"""The rigid body class encapsulating the ODE body.
This class represents a rigid body that has a location and orientation
in space and that stores the mass properties of an object.
When creating a Body object you have to pass the world it belongs to
as argument to the constructor::
>>> import ode
>>> w = ode.World()
>>> b = ode.Body(w)
"""
cdef dBodyID bid
# A reference to the world so that the world won't be destroyed while
# there are still joints using it.
cdef object world
# A dictionary with user attributes
# (set via __getattr__ and __setattr__)
cdef object userattribs
def __cinit__(self, World world not None):
self.bid = dBodyCreate(world.wid)
def __init__(self, World world not None):
"""Constructor.
@param world: The world in which the body should be created.
@type world: World
"""
self.world = world
self.userattribs = {}
def __dealloc__(self):
if self.bid != NULL:
dBodyDestroy(self.bid)
def __getattr__(self, name):
try:
return self.userattribs[name]
except:
raise AttributeError("Body object has no attribute '%s'" % name)
def __setattr__(self, name, value):
self.userattribs[name] = value
def __delattr__(self, name):
try:
del self.userattribs[name]
except:
raise AttributeError("Body object has no attribute '%s'" % name)
# setPosition
def setPosition(self, pos):
"""setPosition(pos)
Set the position of the body.
@param pos: The new position
@type pos: 3-sequence of floats
"""
dBodySetPosition(self.bid, pos[0], pos[1], pos[2])
# getPosition
def getPosition(self):
"""getPosition() -> 3-tuple
Return the current position of the body.
"""
cdef dReal* p
# The "const" in the original return value is cast away
p = <dReal*>dBodyGetPosition(self.bid)
return p[0], p[1], p[2]
# setRotation
def setRotation(self, R):
"""setRotation(R)
Set the orientation of the body. The rotation matrix must be
given as a sequence of 9 floats which are the elements of the
matrix in row-major order.
@param R: Rotation matrix
@type R: 9-sequence of floats
"""
cdef dMatrix3 m
m[0] = R[0]
m[1] = R[1]
m[2] = R[2]
m[3] = 0
m[4] = R[3]
m[5] = R[4]
m[6] = R[5]
m[7] = 0
m[8] = R[6]
m[9] = R[7]
m[10] = R[8]
m[11] = 0
dBodySetRotation(self.bid, m)
# getRotation
def getRotation(self):
"""getRotation() -> 9-tuple
Return the current rotation matrix as a tuple of 9 floats (row-major
order).
"""
cdef dReal* m
# The "const" in the original return value is cast away
m = <dReal*>dBodyGetRotation(self.bid)
return m[0], m[1], m[2], m[4], m[5], m[6], m[8], m[9], m[10]
# getQuaternion
def getQuaternion(self):
"""getQuaternion() -> 4-tuple
Return the current rotation as a quaternion. The return value
is a list of 4 floats.
"""
cdef dReal* q
q = <dReal*>dBodyGetQuaternion(self.bid)
return q[0], q[1], q[2], q[3]
# setQuaternion
def setQuaternion(self, q):
"""setQuaternion(q)
Set the orientation of the body. The quaternion must be given as a
sequence of 4 floats.
@param q: Quaternion
@type q: 4-sequence of floats
"""
cdef dQuaternion w
w[0] = q[0]
w[1] = q[1]
w[2] = q[2]
w[3] = q[3]
dBodySetQuaternion(self.bid, w)
# setLinearVel
def setLinearVel(self, vel):
"""setLinearVel(vel)
Set the linear velocity of the body.
@param vel: New velocity
@type vel: 3-sequence of floats
"""
dBodySetLinearVel(self.bid, vel[0], vel[1], vel[2])
# getLinearVel
def getLinearVel(self):
"""getLinearVel() -> 3-tuple
Get the current linear velocity of the body.
"""
cdef dReal* p
# The "const" in the original return value is cast away
p = <dReal*>dBodyGetLinearVel(self.bid)
return p[0], p[1], p[2]
# setAngularVel
def setAngularVel(self, vel):
"""setAngularVel(vel)
Set the angular velocity of the body.
@param vel: New angular velocity
@type vel: 3-sequence of floats
"""
dBodySetAngularVel(self.bid, vel[0], vel[1], vel[2])
# getAngularVel
def getAngularVel(self):
"""getAngularVel() -> 3-tuple
Get the current angular velocity of the body.
"""
cdef dReal* p
# The "const" in the original return value is cast away
p = <dReal*>dBodyGetAngularVel(self.bid)
return p[0], p[1], p[2]
# setMass
def setMass(self, Mass mass):
"""setMass(mass)
Set the mass properties of the body. The argument mass must be
an instance of a Mass object.
@param mass: Mass properties
@type mass: Mass
"""
dBodySetMass(self.bid, &mass._mass)
# getMass
def getMass(self):
"""getMass() -> mass
Return the mass properties as a Mass object.
"""
cdef Mass m
m = Mass()
dBodyGetMass(self.bid, &m._mass)
return m
# addForce
def addForce(self, f):
"""addForce(f)
Add an external force f given in absolute coordinates. The force
is applied at the center of mass.
@param f: Force
@type f: 3-sequence of floats
"""
dBodyAddForce(self.bid, f[0], f[1], f[2])
# addTorque
def addTorque(self, t):
"""addTorque(t)
Add an external torque t given in absolute coordinates.
@param t: Torque
@type t: 3-sequence of floats
"""
dBodyAddTorque(self.bid, t[0], t[1], t[2])
# addRelForce
def addRelForce(self, f):
"""addRelForce(f)
Add an external force f given in relative coordinates
(relative to the body's own frame of reference). The force
is applied at the center of mass.
@param f: Force
@type f: 3-sequence of floats
"""
dBodyAddRelForce(self.bid, f[0], f[1], f[2])
# addRelTorque
def addRelTorque(self, t):
"""addRelTorque(t)
Add an external torque t given in relative coordinates
(relative to the body's own frame of reference).
@param t: Torque
@type t: 3-sequence of floats
"""
dBodyAddRelTorque(self.bid, t[0], t[1], t[2])
# addForceAtPos
def addForceAtPos(self, f, p):
"""addForceAtPos(f, p)
Add an external force f at position p. Both arguments must be
given in absolute coordinates.
@param f: Force
@param p: Position
@type f: 3-sequence of floats
@type p: 3-sequence of floats
"""
dBodyAddForceAtPos(self.bid, f[0], f[1], f[2], p[0], p[1], p[2])
# addForceAtRelPos
def addForceAtRelPos(self, f, p):
"""addForceAtRelPos(f, p)
Add an external force f at position p. f is given in absolute
coordinates and p in absolute coordinates.
@param f: Force
@param p: Position
@type f: 3-sequence of floats
@type p: 3-sequence of floats
"""
dBodyAddForceAtRelPos(self.bid, f[0], f[1], f[2], p[0], p[1], p[2])
# addRelForceAtPos
def addRelForceAtPos(self, f, p):
"""addRelForceAtPos(f, p)
Add an external force f at position p. f is given in relative
coordinates and p in relative coordinates.
@param f: Force
@param p: Position
@type f: 3-sequence of floats
@type p: 3-sequence of floats
"""
dBodyAddRelForceAtPos(self.bid, f[0], f[1], f[2], p[0], p[1], p[2])
# addRelForceAtRelPos
def addRelForceAtRelPos(self, f, p):
"""addRelForceAtRelPos(f, p)
Add an external force f at position p. Both arguments must be
given in relative coordinates.
@param f: Force
@param p: Position
@type f: 3-sequence of floats
@type p: 3-sequence of floats
"""
dBodyAddRelForceAtRelPos(self.bid, f[0], f[1], f[2], p[0], p[1], p[2])
# getForce
def getForce(self):
"""getForce() -> 3-tuple
Return the current accumulated force.
"""
cdef dReal* f
# The "const" in the original return value is cast away
f = <dReal*>dBodyGetForce(self.bid)
return f[0], f[1], f[2]
# getTorque
def getTorque(self):
"""getTorque() -> 3-tuple
Return the current accumulated torque.
"""
cdef dReal* f
# The "const" in the original return value is cast away
f = <dReal*>dBodyGetTorque(self.bid)
return f[0], f[1], f[2]
# setForce
def setForce(self, f):
"""setForce(f)
Set the body force accumulation vector.
@param f: Force
@type f: 3-tuple of floats
"""
dBodySetForce(self.bid, f[0], f[1], f[2])
# setTorque
def setTorque(self, t):
"""setTorque(t)
Set the body torque accumulation vector.
@param t: Torque
@type t: 3-tuple of floats
"""
dBodySetTorque(self.bid, t[0], t[1], t[2])
# getRelPointPos
def getRelPointPos(self, p):
"""getRelPointPos(p) -> 3-tuple
Utility function that takes a point p on a body and returns
that point's position in global coordinates. The point p
must be given in body relative coordinates.
@param p: Body point (local coordinates)
@type p: 3-sequence of floats
"""
cdef dVector3 res
dBodyGetRelPointPos(self.bid, p[0], p[1], p[2], res)
return res[0], res[1], res[2]
# getRelPointVel
def getRelPointVel(self, p):
"""getRelPointVel(p) -> 3-tuple
Utility function that takes a point p on a body and returns
that point's velocity in global coordinates. The point p
must be given in body relative coordinates.
@param p: Body point (local coordinates)
@type p: 3-sequence of floats
"""
cdef dVector3 res
dBodyGetRelPointVel(self.bid, p[0], p[1], p[2], res)
return res[0], res[1], res[2]
# getPointVel
def getPointVel(self, p):
"""getPointVel(p) -> 3-tuple
Utility function that takes a point p on a body and returns
that point's velocity in global coordinates. The point p
must be given in global coordinates.
@param p: Body point (global coordinates)
@type p: 3-sequence of floats
"""
cdef dVector3 res
dBodyGetPointVel(self.bid, p[0], p[1], p[2], res)
return res[0], res[1], res[2]
# getPosRelPoint
def getPosRelPoint(self, p):
"""getPosRelPoint(p) -> 3-tuple
This is the inverse of getRelPointPos(). It takes a point p in
global coordinates and returns the point's position in
body-relative coordinates.
@param p: Body point (global coordinates)
@type p: 3-sequence of floats
"""
cdef dVector3 res
dBodyGetPosRelPoint(self.bid, p[0], p[1], p[2], res)
return res[0], res[1], res[2]
# vectorToWorld
def vectorToWorld(self, v):
"""vectorToWorld(v) -> 3-tuple
Given a vector v expressed in the body coordinate system, rotate
it to the world coordinate system.
@param v: Vector in body coordinate system
@type v: 3-sequence of floats
"""
cdef dVector3 res
dBodyVectorToWorld(self.bid, v[0], v[1], v[2], res)
return res[0], res[1], res[2]
# vectorFromWorld
def vectorFromWorld(self, v):
"""vectorFromWorld(v) -> 3-tuple
Given a vector v expressed in the world coordinate system, rotate
it to the body coordinate system.
@param v: Vector in world coordinate system
@type v: 3-sequence of floats
"""
cdef dVector3 res
dBodyVectorFromWorld(self.bid, v[0], v[1], v[2], res)
return res[0], res[1], res[2]
# Enable
def enable(self):
"""enable()
Manually enable a body.
"""
dBodyEnable(self.bid)
# Disable
def disable(self):
"""disable()
Manually disable a body. Note that a disabled body that is connected
through a joint to an enabled body will be automatically re-enabled
at the next simulation step.
"""
dBodyDisable(self.bid)
# isEnabled
def isEnabled(self):
"""isEnabled() -> bool
Check if a body is currently enabled.
"""
return dBodyIsEnabled(self.bid)
# setFiniteRotationMode
def setFiniteRotationMode(self, mode):
"""setFiniteRotationMode(mode)
This function controls the way a body's orientation is updated at
each time step. The mode argument can be:
- 0: An "infinitesimal" orientation update is used. This is
fast to compute, but it can occasionally cause inaccuracies
for bodies that are rotating at high speed, especially when
those bodies are joined to other bodies. This is the default
for every new body that is created.
- 1: A "finite" orientation update is used. This is more
costly to compute, but will be more accurate for high speed
rotations. Note however that high speed rotations can result
in many types of error in a simulation, and this mode will
only fix one of those sources of error.
@param mode: Rotation mode (0/1)
@type mode: int
"""
dBodySetFiniteRotationMode(self.bid, mode)
# getFiniteRotationMode
def getFiniteRotationMode(self):
"""getFiniteRotationMode() -> mode (0/1)
Return the current finite rotation mode of a body (0 or 1).
See setFiniteRotationMode().
"""
return dBodyGetFiniteRotationMode(self.bid)
# setFiniteRotationAxis
def setFiniteRotationAxis(self, a):
"""setFiniteRotationAxis(a)
Set the finite rotation axis of the body. This axis only has a
meaning when the finite rotation mode is set
(see setFiniteRotationMode()).
@param a: Axis
@type a: 3-sequence of floats
"""
dBodySetFiniteRotationAxis(self.bid, a[0], a[1], a[2])
# getFiniteRotationAxis
def getFiniteRotationAxis(self):
"""getFiniteRotationAxis() -> 3-tuple
Return the current finite rotation axis of the body.
"""
cdef dVector3 p
# The "const" in the original return value is cast away
dBodyGetFiniteRotationAxis(self.bid, p)
return p[0], p[1], p[2]
# getNumJoints
def getNumJoints(self):
"""getNumJoints() -> int
Return the number of joints that are attached to this body.
"""
return dBodyGetNumJoints(self.bid)
# setGravityMode
def setGravityMode(self, mode):
"""setGravityMode(mode)
Set whether the body is influenced by the world's gravity
or not. If mode is True it is, otherwise it isn't.
Newly created bodies are always influenced by the world's gravity.
@param mode: Gravity mode
@type mode: bool
"""
dBodySetGravityMode(self.bid, mode)
# getGravityMode
def getGravityMode(self):
"""getGravityMode() -> bool
Return True if the body is influenced by the world's gravity.
"""
return dBodyGetGravityMode(self.bid)
def setDynamic(self):
"""setDynamic()
Set a body to the (default) "dynamic" state, instead of "kinematic".
See setKinematic() for more information.
"""
dBodySetDynamic(self.bid)
def setKinematic(self):
"""setKinematic()
Set the kinematic state of the body (change it into a kinematic body)
Kinematic bodies behave as if they had infinite mass. This means they don't react
to any force (gravity, constraints or user-supplied); they simply follow
velocity to reach the next position. [from ODE wiki]
"""
dBodySetKinematic(self.bid)
def isKinematic(self):
"""isKinematic() -> bool
Return True if the body is kinematic (not influenced by other forces).
Kinematic bodies behave as if they had infinite mass. This means they don't react
to any force (gravity, constraints or user-supplied); they simply follow
velocity to reach the next position. [from ODE wiki]
"""
return dBodyIsKinematic(self.bid)
def setMaxAngularSpeed(self, max_speed):
"""setMaxAngularSpeed(max_speed)
You can also limit the maximum angular speed. In contrast to the damping
functions, the angular velocity is affected before the body is moved.
This means that it will introduce errors in joints that are forcing the
body to rotate too fast. Some bodies have naturally high angular
velocities (like cars' wheels), so you may want to give them a very high
(like the default, dInfinity) limit.
"""
dBodySetMaxAngularSpeed(self.bid, max_speed)
# JointGroup
cdef class JointGroup:
"""Joint group.
Constructor::
JointGroup()
"""
# JointGroup ID
cdef dJointGroupID gid
# A list of Python joints that were added to the group
cdef object jointlist
def __cinit__(self):
self.gid = dJointGroupCreate(0)
def __init__(self):
self.jointlist = []
def __dealloc__(self):
if self.gid != NULL:
for j in self.jointlist:
j._destroyed()
dJointGroupDestroy(self.gid)
# empty
def empty(self):
"""empty()
Destroy all joints in the group.
"""
dJointGroupEmpty(self.gid)
for j in self.jointlist:
j._destroyed()
self.jointlist = []
def _addjoint(self, j):
"""_addjoint(j)
Add a joint to the group. This is an internal method that is
called by the joints. The group has to know the Python
wrappers because it has to notify them when the group is
emptied (so that the ODE joints won't get destroyed
twice). The notification is done by calling _destroyed() on
the Python joints.
@param j: The joint to add
@type j: Joint
"""
self.jointlist.append(j)
######################################################################
# Joint
cdef class Joint:
"""Base class for all joint classes."""
# Joint id as returned by dJointCreateXxx()
cdef dJointID jid
# A reference to the world so that the world won't be destroyed while
# there are still joints using it.
cdef object world
# The feedback buffer
cdef dJointFeedback* feedback
cdef object body1
cdef object body2
# A dictionary with user attributes
# (set via __getattr__ and __setattr__)
cdef object userattribs
def __cinit__(self, *a, **kw):
self.jid = NULL
self.world = None
self.feedback = NULL
self.body1 = None
self.body2 = None
self.userattribs = {}
def __init__(self, *a, **kw):
raise NotImplementedError("Joint base class can't be used directly")
def __dealloc__(self):
self.setFeedback(False)
if self.jid != NULL:
dJointDestroy(self.jid)
def __getattr__(self, name):
try:
return self.userattribs[name]
except:
raise AttributeError("Joint object has no attribute '%s'" % name)
def __setattr__(self, name, value):
self.userattribs[name] = value
def __delattr__(self, name):
try:
del self.userattribs[name]
except:
raise AttributeError("Joint object has no attribute '%s'" % name)
# _destroyed
def _destroyed(self):
"""Notify the joint object about an external destruction of the ODE joint.
This method has to be called when the underlying ODE object
was destroyed by someone else (e.g. by a joint group). The Python
wrapper will then refrain from destroying it again.
"""
self.jid = NULL
# enable
def enable(self):
"""enable()
Enable the joint. Disabled joints are completely ignored during the
simulation. Disabled joints don't lose the already computed information
like anchors and axes.
"""
dJointEnable(self.jid)
# disable
def disable(self):
"""disable()
Disable the joint. Disabled joints are completely ignored during the
simulation. Disabled joints don't lose the already computed information
like anchors and axes.
"""
dJointDisable(self.jid)
# isEnabled
def isEnabled(self):
"""isEnabled() -> bool
Determine whether the joint is enabled. Disabled joints are completely
ignored during the simulation. Disabled joints don't lose the already
computed information like anchors and axes.
"""
return dJointIsEnabled(self.jid)
# attach
def attach(self, Body body1, Body body2):
"""attach(body1, body2)
Attach the joint to some new bodies. A body can be attached
to the environment by passing None as second body.
@param body1: First body
@param body2: Second body
@type body1: Body
@type body2: Body
"""
cdef dBodyID id1, id2
if body1 == None:
id1 = NULL
else:
id1 = body1.bid
if body2 == None:
id2 = NULL
else:
id2 = body2.bid
self.body1 = body1
self.body2 = body2
dJointAttach(self.jid, id1, id2)
# getBody
def getBody(self, index):
"""getBody(index) -> Body
Return the bodies that this joint connects. If index is 0 the
"first" body will be returned, corresponding to the body1
argument of the attach() method. If index is 1 the "second" body
will be returned, corresponding to the body2 argument of the
attach() method.
@param index: Bodx index (0 or 1).
@type index: int
"""
if index == 0:
return self.body1
elif index == 1:
return self.body2
else:
raise IndexError()
# setFeedback
def setFeedback(self, flag=1):
"""setFeedback(flag=True)
Create a feedback buffer. If flag is True then a buffer is
allocated and the forces/torques applied by the joint can
be read using the getFeedback() method. If flag is False the
buffer is released.
@param flag: Specifies whether a buffer should be created or released
@type flag: bool
"""
if flag:
# Was there already a buffer allocated? then we're finished
if self.feedback != NULL:
return
# Allocate a buffer and pass it to ODE
self.feedback = <dJointFeedback*>malloc(sizeof(dJointFeedback))
if self.feedback == NULL:
raise MemoryError("can't allocate feedback buffer")
dJointSetFeedback(self.jid, self.feedback)
else:
if self.feedback != NULL:
# Free a previously allocated buffer
dJointSetFeedback(self.jid, NULL)
free(self.feedback)
self.feedback = NULL
# getFeedback
def getFeedback(self):
"""getFeedback() -> (force1, torque1, force2, torque2)
Get the forces/torques applied by the joint. If feedback is
activated (i.e. setFeedback(True) was called) then this method
returns a tuple (force1, torque1, force2, torque2) with the
forces and torques applied to body 1 and body 2. The
forces/torques are given as 3-tuples.
If feedback is deactivated then the method always returns None.
"""
cdef dJointFeedback* fb
fb = dJointGetFeedback(self.jid)
if fb == NULL:
return None
f1 = (fb.f1[0], fb.f1[1], fb.f1[2])
t1 = (fb.t1[0], fb.t1[1], fb.t1[2])
f2 = (fb.f2[0], fb.f2[1], fb.f2[2])
t2 = (fb.t2[0], fb.t2[1], fb.t2[2])
return f1, t1, f2, t2
######################################################################
# BallJoint
cdef class BallJoint(Joint):
"""Ball joint.
Constructor::
BallJoint(world, jointgroup=None)
"""
def __cinit__(self, World world not None, jointgroup=None):
cdef JointGroup jg
cdef dJointGroupID jgid
jgid = NULL
if jointgroup != None:
jg = jointgroup
jgid = jg.gid
self.jid = dJointCreateBall(world.wid, jgid)
def __init__(self, World world not None, jointgroup=None):
self.world = world
if jointgroup != None:
jointgroup._addjoint(self)
# setAnchor
def setAnchor(self, pos):
"""setAnchor(pos)
Set the joint anchor point which must be specified in world
coordinates.
@param pos: Anchor position
@type pos: 3-sequence of floats
"""
dJointSetBallAnchor(self.jid, pos[0], pos[1], pos[2])
# getAnchor
def getAnchor(self):
"""getAnchor() -> 3-tuple of floats
Get the joint anchor point, in world coordinates. This
returns the point on body 1. If the joint is perfectly
satisfied, this will be the same as the point on body 2.
"""
cdef dVector3 p
dJointGetBallAnchor(self.jid, p)
return p[0], p[1], p[2]
# getAnchor2
def getAnchor2(self):
"""getAnchor2() -> 3-tuple of floats
Get the joint anchor point, in world coordinates. This
returns the point on body 2. If the joint is perfectly
satisfied, this will be the same as the point on body 1.
"""
cdef dVector3 p
dJointGetBallAnchor2(self.jid, p)
return p[0], p[1], p[2]
# setParam
def setParam(self, param, value):
pass
# getParam
def getParam(self, param):
return 0.0
# HingeJoint
cdef class HingeJoint(Joint):
"""Hinge joint.
Constructor::
HingeJoint(world, jointgroup=None)
"""
def __cinit__(self, World world not None, jointgroup=None):
cdef JointGroup jg
cdef dJointGroupID jgid
jgid = NULL
if jointgroup != None:
jg = jointgroup
jgid = jg.gid
self.jid = dJointCreateHinge(world.wid, jgid)
def __init__(self, World world not None, jointgroup=None):
self.world = world
if jointgroup != None:
jointgroup._addjoint(self)
# setAnchor
def setAnchor(self, pos):
"""setAnchor(pos)
Set the hinge anchor which must be given in world coordinates.
@param pos: Anchor position
@type pos: 3-sequence of floats
"""
dJointSetHingeAnchor(self.jid, pos[0], pos[1], pos[2])
# getAnchor
def getAnchor(self):
"""getAnchor() -> 3-tuple of floats
Get the joint anchor point, in world coordinates. This returns
the point on body 1. If the joint is perfectly satisfied, this
will be the same as the point on body 2.
"""
cdef dVector3 p
dJointGetHingeAnchor(self.jid, p)
return p[0], p[1], p[2]
# getAnchor2
def getAnchor2(self):
"""getAnchor2() -> 3-tuple of floats
Get the joint anchor point, in world coordinates. This returns
the point on body 2. If the joint is perfectly satisfied, this
will be the same as the point on body 1.
"""
cdef dVector3 p
dJointGetHingeAnchor2(self.jid, p)
return p[0], p[1], p[2]
# setAxis
def setAxis(self, axis):
"""setAxis(axis)
Set the hinge axis.
@param axis: Hinge axis
@type axis: 3-sequence of floats
"""
dJointSetHingeAxis(self.jid, axis[0], axis[1], axis[2])
# getAxis
def getAxis(self):
"""getAxis() -> 3-tuple of floats
Get the hinge axis.
"""
cdef dVector3 a
dJointGetHingeAxis(self.jid, a)
return a[0], a[1], a[2]
# getAngle
def getAngle(self):
"""getAngle() -> float
Get the hinge angle. The angle is measured between the two
bodies, or between the body and the static environment. The
angle will be between -pi..pi.
When the hinge anchor or axis is set, the current position of
the attached bodies is examined and that position will be the
zero angle.
"""
return dJointGetHingeAngle(self.jid)
# getAngleRate
def getAngleRate(self):
"""getAngleRate() -> float
Get the time derivative of the angle.
"""
return dJointGetHingeAngleRate(self.jid)
# addTorque
def addTorque(self, torque):
"""addTorque(torque)
Applies the torque about the hinge axis.
@param torque: Torque magnitude
@type torque: float
"""
dJointAddHingeTorque(self.jid, torque)
# setParam
def setParam(self, param, value):
"""setParam(param, value)
Set limit/motor parameters for the joint.
param is one of ParamLoStop, ParamHiStop, ParamVel, ParamFMax,
ParamFudgeFactor, ParamBounce, ParamCFM, ParamStopERP, ParamStopCFM,
ParamSuspensionERP, ParamSuspensionCFM.
These parameter names can be optionally followed by a digit (2
or 3) to indicate the second or third set of parameters.
@param param: Selects the parameter to set
@param value: Parameter value
@type param: int
@type value: float
"""
dJointSetHingeParam(self.jid, param, value)
# getParam
def getParam(self, param):
"""getParam(param) -> float
Get limit/motor parameters for the joint.
param is one of ParamLoStop, ParamHiStop, ParamVel, ParamFMax,
ParamFudgeFactor, ParamBounce, ParamCFM, ParamStopERP, ParamStopCFM,
ParamSuspensionERP, ParamSuspensionCFM.
These parameter names can be optionally followed by a digit (2
or 3) to indicate the second or third set of parameters.
@param param: Selects the parameter to read
@type param: int
"""
return dJointGetHingeParam(self.jid, param)
# SliderJoint
cdef class SliderJoint(Joint):
"""Slider joint.
Constructor::
SlideJoint(world, jointgroup=None)
"""
def __cinit__(self, World world not None, jointgroup=None):
cdef JointGroup jg
cdef dJointGroupID jgid
jgid = NULL
if jointgroup != None:
jg = jointgroup
jgid = jg.gid
self.jid = dJointCreateSlider(world.wid, jgid)
def __init__(self, World world not None, jointgroup=None):
self.world = world
if jointgroup != None:
jointgroup._addjoint(self)
# setAxis
def setAxis(self, axis):
"""setAxis(axis)
Set the slider axis parameter.
@param axis: Slider axis
@type axis: 3-sequence of floats
"""
dJointSetSliderAxis(self.jid, axis[0], axis[1], axis[2])
# getAxis
def getAxis(self):
"""getAxis() -> 3-tuple of floats
Get the slider axis parameter.
"""
cdef dVector3 a
dJointGetSliderAxis(self.jid, a)
return a[0], a[1], a[2]
# getPosition
def getPosition(self):
"""getPosition() -> float
Get the slider linear position (i.e. the slider's "extension").
When the axis is set, the current position of the attached
bodies is examined and that position will be the zero
position.
"""
return dJointGetSliderPosition(self.jid)
# getPositionRate
def getPositionRate(self):
"""getPositionRate() -> float
Get the time derivative of the position.
"""
return dJointGetSliderPositionRate(self.jid)
# addForce
def addForce(self, force):
"""addForce(force)
Applies the given force in the slider's direction.
@param force: Force magnitude
@type force: float
"""
dJointAddSliderForce(self.jid, force)
# setParam
def setParam(self, param, value):
dJointSetSliderParam(self.jid, param, value)
# getParam
def getParam(self, param):
return dJointGetSliderParam(self.jid, param)
# UniversalJoint
cdef class UniversalJoint(Joint):
"""Universal joint.
Constructor::
UniversalJoint(world, jointgroup=None)
"""
def __cinit__(self, World world not None, jointgroup=None):
cdef JointGroup jg
cdef dJointGroupID jgid
jgid = NULL
if jointgroup != None:
jg = jointgroup
jgid = jg.gid
self.jid = dJointCreateUniversal(world.wid, jgid)
def __init__(self, World world not None, jointgroup=None):
self.world = world
if jointgroup != None:
jointgroup._addjoint(self)
# setAnchor
def setAnchor(self, pos):
"""setAnchor(pos)
Set the universal anchor.
@param pos: Anchor position
@type pos: 3-sequence of floats
"""
dJointSetUniversalAnchor(self.jid, pos[0], pos[1], pos[2])
# getAnchor
def getAnchor(self):
"""getAnchor() -> 3-tuple of floats
Get the joint anchor point, in world coordinates. This returns
the point on body 1. If the joint is perfectly satisfied, this
will be the same as the point on body 2.
"""
cdef dVector3 p
dJointGetUniversalAnchor(self.jid, p)
return p[0], p[1], p[2]
# getAnchor2
def getAnchor2(self):
"""getAnchor2() -> 3-tuple of floats
Get the joint anchor point, in world coordinates. This returns
the point on body 2. If the joint is perfectly satisfied, this
will be the same as the point on body 1.
"""
cdef dVector3 p
dJointGetUniversalAnchor2(self.jid, p)
return p[0], p[1], p[2]
# setAxis1
def setAxis1(self, axis):
"""setAxis1(axis)
Set the first universal axis. Axis 1 and axis 2 should be
perpendicular to each other.
@param axis: Joint axis
@type axis: 3-sequence of floats
"""
dJointSetUniversalAxis1(self.jid, axis[0], axis[1], axis[2])
# getAxis1
def getAxis1(self):
"""getAxis1() -> 3-tuple of floats
Get the first univeral axis.
"""
cdef dVector3 a
dJointGetUniversalAxis1(self.jid, a)
return a[0], a[1], a[2]
# setAxis2
def setAxis2(self, axis):
"""setAxis2(axis)
Set the second universal axis. Axis 1 and axis 2 should be
perpendicular to each other.
@param axis: Joint axis
@type axis: 3-sequence of floats
"""
dJointSetUniversalAxis2(self.jid, axis[0], axis[1], axis[2])
# getAxis2
def getAxis2(self):
"""getAxis2() -> 3-tuple of floats
Get the second univeral axis.
"""
cdef dVector3 a
dJointGetUniversalAxis2(self.jid, a)
return a[0], a[1], a[2]
# addTorques
def addTorques(self, torque1, torque2):
"""addTorques(torque1, torque2)
Applies torque1 about axis 1, and torque2 about axis 2.
@param torque1: Torque 1 magnitude
@param torque2: Torque 2 magnitude
@type torque1: float
@type torque2: float
"""
dJointAddUniversalTorques(self.jid, torque1, torque2)
def getAngle1(self):
return dJointGetUniversalAngle1(self.jid)
def getAngle2(self):
return dJointGetUniversalAngle2(self.jid)
def getAngle1Rate(self):
return dJointGetUniversalAngle1Rate(self.jid)
def getAngle2Rate(self):
return dJointGetUniversalAngle2Rate(self.jid)
# setParam
def setParam(self, param, value):
dJointSetUniversalParam(self.jid, param, value)
# getParam
def getParam(self, param):
return dJointGetUniversalParam(self.jid, param)
# Hinge2Joint
cdef class Hinge2Joint(Joint):
"""Hinge2 joint.
Constructor::
Hinge2Joint(world, jointgroup=None)
"""
def __cinit__(self, World world not None, jointgroup=None):
cdef JointGroup jg
cdef dJointGroupID jgid
jgid = NULL
if jointgroup != None:
jg = jointgroup
jgid = jg.gid
self.jid = dJointCreateHinge2(world.wid, jgid)
def __init__(self, World world, jointgroup=None):
self.world = world
if jointgroup != None:
jointgroup._addjoint(self)
# setAnchor
def setAnchor(self, pos):
"""setAnchor(pos)
Set the hinge-2 anchor.
@param pos: Anchor position
@type pos: 3-sequence of floats
"""
dJointSetHinge2Anchor(self.jid, pos[0], pos[1], pos[2])
# getAnchor
def getAnchor(self):
"""getAnchor() -> 3-tuple of floats
Get the joint anchor point, in world coordinates. This returns
the point on body 1. If the joint is perfectly satisfied, this
will be the same as the point on body 2.
"""
cdef dVector3 p
dJointGetHinge2Anchor(self.jid, p)
return p[0], p[1], p[2]
# getAnchor2
def getAnchor2(self):
"""getAnchor2() -> 3-tuple of floats
Get the joint anchor point, in world coordinates. This returns
the point on body 2. If the joint is perfectly satisfied, this
will be the same as the point on body 1.
"""
cdef dVector3 p
dJointGetHinge2Anchor2(self.jid, p)
return p[0], p[1], p[2]
# setAxis1
def setAxis1(self, axis):
"""setAxis1(axis)
Set the first hinge-2 axis. Axis 1 and axis 2 must not lie
along the same line.
@param axis: Joint axis
@type axis: 3-sequence of floats
"""
dJointSetHinge2Axis1(self.jid, axis[0], axis[1], axis[2])
# getAxis1
def getAxis1(self):
"""getAxis1() -> 3-tuple of floats
Get the first hinge-2 axis.
"""
cdef dVector3 a
dJointGetHinge2Axis1(self.jid, a)
return a[0], a[1], a[2]
# setAxis2
def setAxis2(self, axis):
"""setAxis2(axis)
Set the second hinge-2 axis. Axis 1 and axis 2 must not lie
along the same line.
@param axis: Joint axis
@type axis: 3-sequence of floats
"""
dJointSetHinge2Axis2(self.jid, axis[0], axis[1], axis[2])
# getAxis2
def getAxis2(self):
"""getAxis2() -> 3-tuple of floats
Get the second hinge-2 axis.
"""
cdef dVector3 a
dJointGetHinge2Axis2(self.jid, a)
return a[0], a[1], a[2]
# getAngle
def getAngle1(self):
"""getAngle1() -> float
Get the first hinge-2 angle (around axis 1).
When the anchor or axis is set, the current position of the
attached bodies is examined and that position will be the zero
angle.
"""
return dJointGetHinge2Angle1(self.jid)
# getAngle1Rate
def getAngle1Rate(self):
"""getAngle1Rate() -> float
Get the time derivative of the first hinge-2 angle.
"""
return dJointGetHinge2Angle1Rate(self.jid)
# getAngle2Rate
def getAngle2Rate(self):
"""getAngle2Rate() -> float
Get the time derivative of the second hinge-2 angle.
"""
return dJointGetHinge2Angle2Rate(self.jid)
# addTorques
def addTorques(self, torque1, torque2):
"""addTorques(torque1, torque2)
Applies torque1 about axis 1, and torque2 about axis 2.
@param torque1: Torque 1 magnitude
@param torque2: Torque 2 magnitude
@type torque1: float
@type torque2: float
"""
dJointAddHinge2Torques(self.jid, torque1, torque2)
# setParam
def setParam(self, param, value):
dJointSetHinge2Param(self.jid, param, value)
# getParam
def getParam(self, param):
return dJointGetHinge2Param(self.jid, param)
# FixedJoint
cdef class FixedJoint(Joint):
"""Fixed joint.
Constructor::
FixedJoint(world, jointgroup=None)
"""
def __cinit__(self, World world not None, jointgroup=None):
cdef JointGroup jg
cdef dJointGroupID jgid
jgid = NULL
if jointgroup != None:
jg = jointgroup
jgid = jg.gid
self.jid = dJointCreateFixed(world.wid, jgid)
def __init__(self, World world not None, jointgroup=None):
self.world = world
if jointgroup != None:
jointgroup._addjoint(self)
# setFixed
def setFixed(self):
"""setFixed()
Call this on the fixed joint after it has been attached to
remember the current desired relative offset and desired
relative rotation between the bodies.
"""
dJointSetFixed(self.jid)
# ContactJoint
cdef class ContactJoint(Joint):
"""Contact joint.
Constructor::
ContactJoint(world, jointgroup, contact)
"""
def __cinit__(self, World world not None, jointgroup, Contact contact):
cdef JointGroup jg
cdef dJointGroupID jgid
jgid = NULL
if jointgroup != None:
jg = jointgroup
jgid = jg.gid
self.jid = dJointCreateContact(world.wid, jgid, &contact._contact)
def __init__(self, World world not None, jointgroup, Contact contact):
self.world = world
if jointgroup != None:
jointgroup._addjoint(self)
# AMotor
cdef class AMotor(Joint):
"""AMotor joint.
Constructor::
AMotor(world, jointgroup=None)
"""
def __cinit__(self, World world not None, jointgroup=None):
cdef JointGroup jg
cdef dJointGroupID jgid
jgid = NULL
if jointgroup != None:
jg = jointgroup
jgid = jg.gid
self.jid = dJointCreateAMotor(world.wid, jgid)
def __init__(self, World world not None, jointgroup=None):
self.world = world
if jointgroup != None:
jointgroup._addjoint(self)
# setMode
def setMode(self, mode):
"""setMode(mode)
Set the angular motor mode. mode must be either AMotorUser or
AMotorEuler.
@param mode: Angular motor mode
@type mode: int
"""
dJointSetAMotorMode(self.jid, mode)
# getMode
def getMode(self):
"""getMode()
Return the angular motor mode (AMotorUser or AMotorEuler).
"""
return dJointGetAMotorMode(self.jid)
# setNumAxes
def setNumAxes(self, int num):
"""setNumAxes(num)
Set the number of angular axes that will be controlled by the AMotor.
num may be in the range from 0 to 3.
@param num: Number of axes (0-3)
@type num: int
"""
dJointSetAMotorNumAxes(self.jid, num)
# getNumAxes
def getNumAxes(self):
"""getNumAxes() -> int
Get the number of angular axes that are controlled by the AMotor.
"""
return dJointGetAMotorNumAxes(self.jid)
# setAxis
def setAxis(self, int anum, int rel, axis):
"""setAxis(anum, rel, axis)
Set an AMotor axis.
The anum argument selects the axis to change (0,1 or 2).
Each axis can have one of three "relative orientation" modes,
selected by rel:
0: The axis is anchored to the global frame.
1: The axis is anchored to the first body.
2: The axis is anchored to the second body.
The axis vector is always specified in global coordinates
regardless of the setting of rel.
@param anum: Axis number
@param rel: Relative orientation mode
@param axis: Axis
@type anum: int
@type rel: int
@type axis: 3-sequence of floats
"""
dJointSetAMotorAxis(self.jid, anum, rel, axis[0], axis[1], axis[2])
# getAxis
def getAxis(self, int anum):
"""getAxis(anum)
Get an AMotor axis.
@param anum: Axis index (0-2)
@type anum: int
"""
cdef dVector3 a
dJointGetAMotorAxis(self.jid, anum, a)
return a[0], a[1], a[2]
# getAxisRel
def getAxisRel(self, int anum):
"""getAxisRel(anum) -> int
Get the relative mode of an axis.
@param anum: Axis index (0-2)
@type anum: int
"""
return dJointGetAMotorAxisRel(self.jid, anum)
# setAngle
def setAngle(self, int anum, angle):
"""setAngle(anum, angle)
Tell the AMotor what the current angle is along axis anum.
@param anum: Axis index
@param angle: Angle
@type anum: int
@type angle: float
"""
dJointSetAMotorAngle(self.jid, anum, angle)
# getAngle
def getAngle(self, int anum):
"""getAngle(anum) -> float
Return the current angle for axis anum.
@param anum: Axis index
@type anum: int
"""
return dJointGetAMotorAngle(self.jid, anum)
# getAngleRate
def getAngleRate(self, int anum):
"""getAngleRate(anum) -> float
Return the current angle rate for axis anum.
@param anum: Axis index
@type anum: int
"""
return dJointGetAMotorAngleRate(self.jid, anum)
# addTorques
def addTorques(self, torque0, torque1, torque2):
"""addTorques(torque0, torque1, torque2)
Applies torques about the AMotor's axes.
@param torque0: Torque 0 magnitude
@param torque1: Torque 1 magnitude
@param torque2: Torque 2 magnitude
@type torque0: float
@type torque1: float
@type torque2: float
"""
dJointAddAMotorTorques(self.jid, torque0, torque1, torque2)
# setParam
def setParam(self, param, value):
dJointSetAMotorParam(self.jid, param, value)
# getParam
def getParam(self, param):
return dJointGetAMotorParam(self.jid, param)
# LMotor
cdef class LMotor(Joint):
"""LMotor joint.
Constructor::
LMotor(world, jointgroup=None)
"""
def __cinit__(self, World world not None, jointgroup=None):
cdef JointGroup jg
cdef dJointGroupID jgid
jgid = NULL
if jointgroup != None:
jg = jointgroup
jgid = jg.gid
self.jid = dJointCreateLMotor(world.wid, jgid)
def __init__(self, World world not None, jointgroup=None):
self.world = world
if jointgroup != None:
jointgroup._addjoint(self)
# setNumAxes
def setNumAxes(self, int num):
"""setNumAxes(num)
Set the number of angular axes that will be controlled by the LMotor.
num may be in the range from 0 to 3.
@param num: Number of axes (0-3)
@type num: int
"""
dJointSetLMotorNumAxes(self.jid, num)
# getNumAxes
def getNumAxes(self):
"""getNumAxes() -> int
Get the number of angular axes that are controlled by the LMotor.
"""
return dJointGetLMotorNumAxes(self.jid)
# setAxis
def setAxis(self, int anum, int rel, axis):
"""setAxis(anum, rel, axis)
Set an LMotor axis.
The anum argument selects the axis to change (0,1 or 2).
Each axis can have one of three "relative orientation" modes,
selected by rel:
0: The axis is anchored to the global frame.
1: The axis is anchored to the first body.
2: The axis is anchored to the second body.
@param anum: Axis number
@param rel: Relative orientation mode
@param axis: Axis
@type anum: int
@type rel: int
@type axis: 3-sequence of floats
"""
dJointSetLMotorAxis(self.jid, anum, rel, axis[0], axis[1], axis[2])
# getAxis
def getAxis(self, int anum):
"""getAxis(anum)
Get an LMotor axis.
@param anum: Axis index (0-2)
@type anum: int
"""
cdef dVector3 a
dJointGetLMotorAxis(self.jid, anum, a)
return a[0], a[1], a[2]
# setParam
def setParam(self, param, value):
dJointSetLMotorParam(self.jid, param, value)
# getParam
def getParam(self, param):
return dJointGetLMotorParam(self.jid, param)
# Plane2DJoint
cdef class Plane2DJoint(Joint):
"""Plane-2D Joint.
Constructor::
Plane2DJoint(world, jointgroup=None)
"""
def __cinit__(self, World world not None, jointgroup=None):
cdef JointGroup jg
cdef dJointGroupID jgid
jgid = NULL
if jointgroup != None:
jg = jointgroup
jgid = jg.gid
self.jid = dJointCreatePlane2D(world.wid, jgid)
def __init__(self, World world not None, jointgroup=None):
self.world = world
if jointgroup != None:
jointgroup._addjoint(self)
def setXParam(self, param, value):
dJointSetPlane2DXParam(self.jid, param, value)
def setYParam(self, param, value):
dJointSetPlane2DYParam(self.jid, param, value)
def setAngleParam(self, param, value):
dJointSetPlane2DAngleParam(self.jid, param, value)
# PRJoint
cdef class PRJoint(Joint):
"""Prismatic and Rotoide Joint.
Constructor::
PRJoint(world, jointgroup=None)
"""
def __cinit__(self, World world not None, jointgroup=None):
cdef JointGroup jg
cdef dJointGroupID jgid
jgid = NULL
if jointgroup != None:
jg = jointgroup
jgid = jg.gid
self.jid = dJointCreatePR(world.wid, jgid)
def __init__(self, World world not None, jointgroup=None):
self.world = world
if jointgroup != None:
jointgroup._addjoint(self)
def getPosition(self):
"""getPosition()
Get a PRJoint's linear extension. (i.e. the prismatic's extension)
"""
return dJointGetPRPosition(self.jid)
def setAnchor(self, pos):
"""setAnchor(pos)
Set a PRJoint anchor.
@param pos: Anchor position
@type pos: 3-sequence of floats
"""
dJointSetPRAnchor(self.jid, pos[0], pos[1], pos[2])
def getAnchor(self):
"""getAnchor()
Get a PRJoint anchor.
"""
cdef dVector3 a
dJointGetPRAnchor(self.jid, a)
return a[0], a[1], a[2]
def setAxis1(self, axis):
"""setAxis1(axis)
Set a PRJoint's prismatic axis.
@param axis: Axis
@type axis: 3-sequence of floats
"""
dJointSetPRAxis1(self.jid, axis[0], axis[1], axis[2])
def getAxis1(self):
"""getAxis1()
Get a PRJoint's prismatic axis.
"""
cdef dVector3 a
dJointGetPRAxis1(self.jid, a)
return a[0], a[1], a[2]
def setAxis2(self, axis):
"""setAxis2(axis)
Set a PRJoint's rotoide axis.
@param axis: Axis
@type axis: 3-sequence of floats
"""
dJointSetPRAxis2(self.jid, axis[0], axis[1], axis[2])
def getAxis2(self):
"""getAxis2()
Get a PRJoint's rotoide axis.
"""
cdef dVector3 a
dJointGetPRAxis2(self.jid, a)
return a[0], a[1], a[2]
# Geom base class
cdef class GeomObject:
"""This is the abstract base class for all geom objects.
"""
# The id of the geom object as returned by dCreateXxxx()
cdef dGeomID gid
# The space in which the geom was placed (or None). This reference
# is kept so that the space won't be destroyed while there are still
# geoms around that might use it.
cdef object space
# The body that the geom was attached to (or None).
cdef object body
# A dictionary with user defined attributes
cdef object attribs
cdef object __weakref__
def __cinit__(self, *a, **kw):
self.gid = NULL
self.space = None
self.body = None
self.attribs = {}
def __init__(self, *a, **kw):
raise NotImplementedError(
"GeomObject base class can't be used directly")
def __dealloc__(self):
if self.gid != NULL:
dGeomDestroy(self.gid)
self.gid = NULL
def __getattr__(self, name):
if name in self.attribs:
return self.attribs[name]
else:
raise AttributeError("geom has no attribute '%s'" % name)
def __setattr__(self, name, val):
self.attribs[name] = val
def __delattr__(self, name):
if name in self.attribs:
del self.attribs[name]
else:
raise AttributeError("geom has no attribute '%s'" % name)
def _id(self):
"""_id() -> int
Return the internal id of the geom (dGeomID) as returned by
the dCreateXyz() functions.
This method has to be overwritten in derived methods.
"""
raise NotImplementedError("Bug: The _id() method is not implemented")
def placeable(self):
"""placeable() -> bool
Returns True if the geom object is a placeable geom.
This method has to be overwritten in derived methods.
"""
return False
def setBody(self, Body body):
"""setBody(body)
Set the body associated with a placeable geom.
@param body: The Body object or None.
@type body: Body
"""
if not self.placeable():
raise ValueError(
"Non-placeable geoms cannot have a body associated to them")
if body == None:
dGeomSetBody(self.gid, NULL)
else:
dGeomSetBody(self.gid, body.bid)
self.body = body
def getBody(self):
"""getBody() -> Body
Get the body associated with this geom.
"""
if not self.placeable():
return environment
return self.body
def setPosition(self, pos):
"""setPosition(pos)
Set the position of the geom. If the geom is attached to a body,
the body's position will also be changed.
@param pos: Position
@type pos: 3-sequence of floats
"""
if not self.placeable():
raise ValueError("Cannot set a position on non-placeable geoms")
dGeomSetPosition(self.gid, pos[0], pos[1], pos[2])
def getPosition(self):
"""getPosition() -> 3-tuple
Get the current position of the geom. If the geom is attached to
a body the returned value is the body's position.
"""
if not self.placeable():
raise ValueError("Non-placeable geoms do not have a position")
cdef dReal* p
p = <dReal*>dGeomGetPosition(self.gid)
return p[0], p[1], p[2]
def setRotation(self, R):
"""setRotation(R)
Set the orientation of the geom. If the geom is attached to a body,
the body's orientation will also be changed.
@param R: Rotation matrix
@type R: 9-sequence of floats
"""
if not self.placeable():
raise ValueError("Cannot set a rotation on non-placeable geoms")
cdef dMatrix3 m
m[0] = R[0]
m[1] = R[1]
m[2] = R[2]
m[3] = 0
m[4] = R[3]
m[5] = R[4]
m[6] = R[5]
m[7] = 0
m[8] = R[6]
m[9] = R[7]
m[10] = R[8]
m[11] = 0
dGeomSetRotation(self.gid, m)
def getRotation(self):
"""getRotation() -> 9-tuple
Get the current orientation of the geom. If the geom is attached to
a body the returned value is the body's orientation.
"""
if not self.placeable():
raise ValueError("Non-placeable geoms do not have a rotation")
cdef dReal* m
m = <dReal*>dGeomGetRotation(self.gid)
return [m[0], m[1], m[2], m[4], m[5], m[6], m[8], m[9], m[10]]
def getQuaternion(self):
"""getQuaternion() -> (w,x,y,z)
Get the current orientation of the geom. If the geom is attached to
a body the returned value is the body's orientation.
"""
if not self.placeable():
raise ValueError("Non-placeable geoms do not have an orientation")
cdef dQuaternion q
dGeomGetQuaternion(self.gid, q)
return q[0], q[1], q[2], q[3]
def setQuaternion(self, q):
"""setQuaternion(q)
Set the orientation of the geom. If the geom is attached to a body,
the body's orientation will also be changed.
@param q: Quaternion (w,x,y,z)
@type q: 4-sequence of floats
"""
if not self.placeable():
raise ValueError("Cannot set a quaternion on non-placeable geoms")
cdef dQuaternion cq
cq[0] = q[0]
cq[1] = q[1]
cq[2] = q[2]
cq[3] = q[3]
dGeomSetQuaternion(self.gid, cq)
def setOffsetPosition(self, pos):
"""setOffsetPosition(pos)
Set the offset position of the geom. The geom must be attached to a
body. If the geom did not have an offset, it is automatically created.
This sets up an additional (local) transformation for the geom, since
geoms attached to a body share their global position and rotation.
@param pos: Position
@type pos: 3-sequence of floats
"""
if self.body == None:
raise ValueError("Cannot set an offset position on a geom before "
"calling setBody")
dGeomSetOffsetPosition(self.gid, pos[0], pos[1], pos[2])
def getOffsetPosition(self):
"""getOffsetPosition() -> 3-tuple
Get the offset position of the geom.
"""
cdef dReal* p
p = <dReal*>dGeomGetOffsetPosition(self.gid)
return (p[0],p[1],p[2])
def setOffsetRotation(self, R):
"""setOffsetRotation(R)
Set the offset rotation of the geom. The geom must be attached to a
body. If the geom did not have an offset, it is automatically created.
This sets up an additional (local) transformation for the geom, since
geoms attached to a body share their global position and rotation.
@param R: Rotation matrix
@type R: 9-sequence of floats
"""
if self.body == None:
raise ValueError("Cannot set an offset rotation on a geom before "
"calling setBody")
cdef dMatrix3 m
m[0] = R[0]
m[1] = R[1]
m[2] = R[2]
m[3] = 0
m[4] = R[3]
m[5] = R[4]
m[6] = R[5]
m[7] = 0
m[8] = R[6]
m[9] = R[7]
m[10] = R[8]
m[11] = 0
dGeomSetOffsetRotation(self.gid, m)
def getOffsetRotation(self):
"""getOffsetRotation() -> 9-tuple
Get the offset rotation of the geom.
"""
cdef dReal* m
m = <dReal*>dGeomGetOffsetRotation(self.gid)
return [m[0], m[1], m[2], m[4], m[5], m[6], m[8], m[9], m[10]]
def clearOffset(self):
"""clearOffset()
Disable the offset transform of the geom.
"""
dGeomClearOffset(self.gid)
def getAABB(self):
"""getAABB() -> 6-tuple
Return an axis aligned bounding box that surrounds the geom.
The return value is a 6-tuple (minx, maxx, miny, maxy, minz, maxz).
"""
cdef dReal aabb[6]
dGeomGetAABB(self.gid, aabb)
return aabb[0], aabb[1], aabb[2], aabb[3], aabb[4], aabb[5]
def isSpace(self):
"""isSpace() -> bool
Return 1 if the given geom is a space, or 0 if not."""
return dGeomIsSpace(self.gid)
def getSpace(self):
"""getSpace() -> Space
Return the space that the given geometry is contained in,
or return None if it is not contained in any space."""
return self.space
def setCollideBits(self, bits):
"""setCollideBits(bits)
Set the "collide" bitfields for this geom.
@param bits: Collide bit field
@type bits: int/long
"""
dGeomSetCollideBits(self.gid, long(bits))
def setCategoryBits(self, bits):
"""setCategoryBits(bits)
Set the "category" bitfields for this geom.
@param bits: Category bit field
@type bits: int/long
"""
dGeomSetCategoryBits(self.gid, long(bits))
def getCollideBits(self):
"""getCollideBits() -> long
Return the "collide" bitfields for this geom.
"""
return dGeomGetCollideBits(self.gid)
def getCategoryBits(self):
"""getCategoryBits() -> long
Return the "category" bitfields for this geom.
"""
return dGeomGetCategoryBits(self.gid)
def enable(self):
"""enable()
Enable the geom."""
dGeomEnable(self.gid)
def disable(self):
"""disable()
Disable the geom."""
dGeomDisable(self.gid)
def isEnabled(self):
"""isEnabled() -> bool
Return True if the geom is enabled."""
return dGeomIsEnabled(self.gid)
# _SpaceIterator
class _SpaceIterator:
"""Iterates over the geoms inside a Space.
"""
def __init__(self, space):
self.space = space
self.idx = 0
def __iter__(self):
return self
def next(self):
if self.idx >= self.space.getNumGeoms():
raise StopIteration
else:
res = self.space.getGeom(self.idx)
self.idx = self.idx + 1
return res
# SpaceBase
cdef class SpaceBase(GeomObject):
"""Space class (container for geometry objects).
A Space object is a container for geometry objects which are used
to do collision detection.
The space does high level collision culling, which means that it
can identify which pairs of geometry objects are potentially
touching.
This Space class can be used for both, a SimpleSpace and a HashSpace
(see ODE documentation).
>>> space = Space(type=0) # Create a SimpleSpace
>>> space = Space(type=1) # Create a HashSpace
"""
# The id of the space. Actually this is a copy of the value in self.gid
# (as the Space is derived from GeomObject) which can be used without
# casting whenever a *space* id is required.
cdef dSpaceID sid
# Dictionary with Geomobjects. Key is the ID (geom._id()) and the value
# is the geom object (Python wrapper). This is used in collide_callback()
# cdef object geom_dict
def __cinit__(self, *a, **kw):
pass
def __init__(self, *a, **kw):
raise NotImplementedError("The SpaceBase class can't be used directly")
def __dealloc__(self):
if self.gid != NULL:
dSpaceDestroy(self.sid)
self.sid = NULL
self.gid = NULL
# def _addgeom(self, geom):
# """Insert the geom object into the dictionary (internal method).
#
# This method has to called in the constructor of a geom object.
# """
# self.geom_dict[geom._id()]=geom
# def _id2geom(self, id):
# """Get the Python wrapper that corresponds to an ID.
#
# The ID is the integer value, as returned by geom._id().
# If the ID is unknown then None is returned.
# """
# if id in self.geom_dict:
# return self.geom_dict[id]
# else:
# return None
def _id(self):
cdef size_t id
id = <size_t>self.sid
return id
def __len__(self):
return self.getNumGeoms()
def __iter__(self):
return _SpaceIterator(self)
def add(self, GeomObject geom):
"""add(geom)
Add a geom to a space. This does nothing if the geom is
already in the space.
@param geom: Geom object to add
@type geom: GeomObject
"""
dSpaceAdd(self.sid, geom.gid)
def remove(self, GeomObject geom):
"""remove(geom)
Remove a geom from a space.
@param geom: Geom object to remove
@type geom: GeomObject
"""
dSpaceRemove(self.sid, geom.gid)
def query(self, GeomObject geom):
"""query(geom) -> bool
Return True if the given geom is in the space.
@param geom: Geom object to check
@type geom: GeomObject
"""
return dSpaceQuery(self.sid, geom.gid)
def getNumGeoms(self):
"""getNumGeoms() -> int
Return the number of geoms contained within the space.
"""
return dSpaceGetNumGeoms(self.sid)
def getGeom(self, int idx):
"""getGeom(idx) -> GeomObject
Return the geom with the given index contained within the space.
@param idx: Geom index (0,1,...,getNumGeoms()-1)
@type idx: int
"""
cdef dGeomID gid
# Check the index
if idx < 0 or idx >= dSpaceGetNumGeoms(self.sid):
raise IndexError("geom index out of range")
gid = dSpaceGetGeom(self.sid, idx)
if <size_t>gid not in _geom_c2py_lut:
raise RuntimeError(
"geom id cannot be translated to a Python object")
return _geom_c2py_lut[<size_t>gid]
def collide(self, arg, callback):
"""collide(arg, callback)
Call a callback function one or more times, for all
potentially intersecting objects in the space. The callback
function takes 3 arguments:
def NearCallback(arg, geom1, geom2):
The arg parameter is just passed on to the callback function.
Its meaning is user defined. The geom1 and geom2 arguments are
the geometry objects that may be near each other. The callback
function can call the function collide() (not the Space
method) on geom1 and geom2, perhaps first determining
whether to collide them at all based on other information.
@param arg: A user argument that is passed to the callback function
@param callback: Callback function
@type callback: callable
"""
cdef void* data
cdef object tup
tup = (callback, arg)
data = <void*>tup
dSpaceCollide(self.sid, data, collide_callback)
# Callback function for the dSpaceCollide() call in the Space.collide() method
# The data parameter is a tuple (Python-Callback, Arguments).
# The function calls a Python callback function with 3 arguments:
# def callback(UserArg, Geom1, Geom2)
# Geom1 and Geom2 are instances of GeomXyz classes.
cdef void collide_callback(void* data, dGeomID o1, dGeomID o2):
cdef object tup
# cdef Space space
cdef size_t id1, id2
# if (dGeomGetBody(o1)==dGeomGetBody(o2)):
# return
tup = <object>data
callback, arg = tup
id1 = <size_t>o1
id2 = <size_t>o2
g1 = _geom_c2py_lut[id1]
g2 = _geom_c2py_lut[id2]
callback(arg, g1, g2)
# SimpleSpace
cdef class SimpleSpace(SpaceBase):
"""Simple space.
This does not do any collision culling - it simply checks every
possible pair of geoms for intersection, and reports the pairs
whose AABBs overlap. The time required to do intersection testing
for n objects is O(n**2). This should not be used for large numbers
of objects, but it can be the preferred algorithm for a small
number of objects. This is also useful for debugging potential
problems with the collision system.
"""
def __cinit__(self, space=None):
cdef SpaceBase sp
cdef dSpaceID parentid
parentid = NULL
if space != None:
sp = space
parentid = sp.sid
self.sid = dSimpleSpaceCreate(parentid)
# Copy the ID
self.gid = <dGeomID>self.sid
dSpaceSetCleanup(self.sid, 0)
_geom_c2py_lut[<size_t>self.sid] = self
def __init__(self, space=None):
pass
# HashSpace
cdef class HashSpace(SpaceBase):
"""Multi-resolution hash table space.
This uses an internal data structure that records how each geom
overlaps cells in one of several three dimensional grids. Each
grid has cubical cells of side lengths 2**i, where i is an integer
that ranges from a minimum to a maximum value. The time required
to do intersection testing for n objects is O(n) (as long as those
objects are not clustered together too closely), as each object
can be quickly paired with the objects around it.
"""
def __cinit__(self, space=None):
cdef SpaceBase sp
cdef dSpaceID parentid
parentid = NULL
if space != None:
sp = space
parentid = sp.sid
self.sid = dHashSpaceCreate(parentid)
# Copy the ID
self.gid = <dGeomID>self.sid
dSpaceSetCleanup(self.sid, 0)
_geom_c2py_lut[<size_t>self.sid] = self
def __init__(self, space=None):
pass
def setLevels(self, int minlevel, int maxlevel):
"""setLevels(minlevel, maxlevel)
Sets the size of the smallest and largest cell used in the
hash table. The actual size will be 2^minlevel and 2^maxlevel
respectively.
"""
if minlevel > maxlevel:
raise ValueError(
"minlevel (%d) must be less than or equal to maxlevel (%d)" %
(minlevel, maxlevel))
dHashSpaceSetLevels(self.sid, minlevel, maxlevel)
def getLevels(self):
"""getLevels() -> (minlevel, maxlevel)
Gets the size of the smallest and largest cell used in the
hash table. The actual size is 2^minlevel and 2^maxlevel
respectively.
"""
cdef int minlevel
cdef int maxlevel
dHashSpaceGetLevels(self.sid, &minlevel, &maxlevel)
return minlevel, maxlevel
# QuadTreeSpace
cdef class QuadTreeSpace(SpaceBase):
"""Quadtree space.
This uses a pre-allocated hierarchical grid-based AABB tree to
quickly cull collision checks. It's exceptionally quick for large
amounts of objects in landscape-shaped worlds. The amount of
memory used is 4**depth * 32 bytes.
Currently getGeom() is not implemented for the quadtree space.
"""
def __cinit__(self, center, extents, depth, space=None):
cdef SpaceBase sp
cdef dSpaceID parentid
cdef dVector3 c
cdef dVector3 e
parentid = NULL
if space != None:
sp = space
parentid = sp.sid
c[0] = center[0]
c[1] = center[1]
c[2] = center[2]
e[0] = extents[0]
e[1] = extents[1]
e[2] = extents[2]
self.sid = dQuadTreeSpaceCreate(parentid, c, e, depth)
# Copy the ID
self.gid = <dGeomID>self.sid
dSpaceSetCleanup(self.sid, 0)
_geom_c2py_lut[<size_t>self.sid] = self
def __init__(self, center, extents, depth, space=None):
pass
def Space(space_type=0):
"""Space factory function.
Depending on the type argument this function either returns a
SimpleSpace (space_type=0) or a HashSpace (space_type=1).
This function is provided to remain compatible with previous
versions of PyODE where there was only one Space class.
>>> space = Space(space_type=0) # Create a SimpleSpace
>>> space = Space(space_type=1) # Create a HashSpace
"""
if space_type == 0:
return SimpleSpace()
elif space_type == 1:
return HashSpace()
else:
raise ValueError("Unknown space type (%d)" % space_type)
# GeomSphere
cdef class GeomSphere(GeomObject):
"""Sphere geometry.
This class represents a sphere centered at the origin.
Constructor::
GeomSphere(space=None, radius=1.0)
"""
def __cinit__(self, space=None, radius=1.0):
cdef SpaceBase sp
cdef dSpaceID sid
sid = NULL
if space != None:
sp = space
sid = sp.sid
self.gid = dCreateSphere(sid, radius)
# if space != None:
# space._addgeom(self)
_geom_c2py_lut[<size_t>self.gid] = self
def __init__(self, space=None, radius=1.0):
self.space = space
self.body = None
def placeable(self):
return True
def _id(self):
cdef size_t id
id = <size_t>self.gid
return id
def setRadius(self, radius):
"""setRadius(radius)
Set the radius of the sphere.
@param radius: New radius
@type radius: float
"""
dGeomSphereSetRadius(self.gid, radius)
def getRadius(self):
"""getRadius() -> float
Return the radius of the sphere.
"""
return dGeomSphereGetRadius(self.gid)
def pointDepth(self, p):
"""pointDepth(p) -> float
Return the depth of the point p in the sphere. Points inside
the geom will have positive depth, points outside it will have
negative depth, and points on the surface will have zero
depth.
@param p: Point
@type p: 3-sequence of floats
"""
return dGeomSpherePointDepth(self.gid, p[0], p[1], p[2])
# GeomBox
cdef class GeomBox(GeomObject):
"""Box geometry.
This class represents a box centered at the origin.
Constructor::
GeomBox(space=None, lengths=(1.0, 1.0, 1.0))
"""
def __cinit__(self, space=None, lengths=(1.0, 1.0, 1.0)):
cdef SpaceBase sp
cdef dSpaceID sid
sid = NULL
if space != None:
sp = space
sid = sp.sid
self.gid = dCreateBox(sid, lengths[0], lengths[1], lengths[2])
# if space != None:
# space._addgeom(self)
_geom_c2py_lut[<size_t>self.gid] = self
def __init__(self, space=None, lengths=(1.0, 1.0, 1.0)):
self.space = space
self.body = None
def placeable(self):
return True
def _id(self):
cdef size_t id
id = <size_t>self.gid
return id
def setLengths(self, lengths):
dGeomBoxSetLengths(self.gid, lengths[0], lengths[1], lengths[2])
def getLengths(self):
cdef dVector3 res
dGeomBoxGetLengths(self.gid, res)
return res[0], res[1], res[2]
def pointDepth(self, p):
"""pointDepth(p) -> float
Return the depth of the point p in the box. Points inside the
geom will have positive depth, points outside it will have
negative depth, and points on the surface will have zero
depth.
@param p: Point
@type p: 3-sequence of floats
"""
return dGeomBoxPointDepth(self.gid, p[0], p[1], p[2])
# GeomPlane
cdef class GeomPlane(GeomObject):
"""Plane geometry.
This class represents an infinite plane. The plane equation is:
n.x*x + n.y*y + n.z*z = dist
This object can't be attached to a body.
If you call getBody() on this object it always returns ode.environment.
Constructor::
GeomPlane(space=None, normal=(0,0,1), dist=0)
"""
def __cinit__(self, space=None, normal=(0, 0, 1), dist=0):
cdef SpaceBase sp
cdef dSpaceID sid
sid = NULL
if space != None:
sp = space
sid = sp.sid
self.gid = dCreatePlane(sid, normal[0], normal[1], normal[2], dist)
# if space != None:
# space._addgeom(self)
_geom_c2py_lut[<size_t>self.gid] = self
def __init__(self, space=None, normal=(0, 0, 1), dist=0):
self.space = space
def _id(self):
cdef size_t id
id = <size_t>self.gid
return id
def setParams(self, normal, dist):
dGeomPlaneSetParams(self.gid, normal[0], normal[1], normal[2], dist)
def getParams(self):
cdef dVector4 res
dGeomPlaneGetParams(self.gid, res)
return ((res[0], res[1], res[2]), res[3])
def pointDepth(self, p):
"""pointDepth(p) -> float
Return the depth of the point p in the plane. Points inside the
geom will have positive depth, points outside it will have
negative depth, and points on the surface will have zero
depth.
@param p: Point
@type p: 3-sequence of floats
"""
return dGeomPlanePointDepth(self.gid, p[0], p[1], p[2])
# GeomCapsule
cdef class GeomCapsule(GeomObject):
"""Capped cylinder geometry.
This class represents a capped cylinder aligned along the local Z axis
and centered at the origin.
Constructor::
GeomCapsule(space=None, radius=0.5, length=1.0)
The length parameter does not include the caps.
"""
def __cinit__(self, space=None, radius=0.5, length=1.0):
cdef SpaceBase sp
cdef dSpaceID sid
sid = NULL
if space != None:
sp = space
sid = sp.sid
self.gid = dCreateCapsule(sid, radius, length)
# if space != None:
# space._addgeom(self)
_geom_c2py_lut[<size_t>self.gid] = self
def __init__(self, space=None, radius=0.5, length=1.0):
self.space = space
self.body = None
def placeable(self):
return True
def _id(self):
cdef size_t id
id = <size_t>self.gid
return id
def setParams(self, radius, length):
dGeomCapsuleSetParams(self.gid, radius, length)
def getParams(self):
cdef dReal radius, length
dGeomCapsuleGetParams(self.gid, &radius, &length)
return radius, length
def pointDepth(self, p):
"""pointDepth(p) -> float
Return the depth of the point p in the cylinder. Points inside the
geom will have positive depth, points outside it will have
negative depth, and points on the surface will have zero
depth.
@param p: Point
@type p: 3-sequence of floats
"""
return dGeomCapsulePointDepth(self.gid, p[0], p[1], p[2])
GeomCCylinder = GeomCapsule # backwards compatibility
# GeomCylinder
cdef class GeomCylinder(GeomObject):
"""Plain cylinder geometry.
This class represents an uncapped cylinder aligned along the local Z axis
and centered at the origin.
Constructor::
GeomCylinder(space=None, radius=0.5, length=1.0)
"""
def __cinit__(self, space=None, radius=0.5, length=1.0):
cdef SpaceBase sp
cdef dSpaceID sid
sid = NULL
if space != None:
sp = space
sid = sp.sid
self.gid = dCreateCylinder(sid, radius, length)
# if space != None:
# space._addgeom(self)
_geom_c2py_lut[<size_t>self.gid] = self
def __init__(self, space=None, radius=0.5, length=1.0):
self.space = space
self.body = None
def placeable(self):
return True
def _id(self):
cdef size_t id
id = <size_t>self.gid
return id
def setParams(self, radius, length):
dGeomCylinderSetParams(self.gid, radius, length)
def getParams(self):
cdef dReal radius, length
dGeomCylinderGetParams(self.gid, &radius, &length)
return radius, length
## dGeomCylinderPointDepth not implemented upstream in ODE 0.7
# GeomRay
cdef class GeomRay(GeomObject):
"""Ray object.
A ray is different from all the other geom classes in that it does
not represent a solid object. It is an infinitely thin line that
starts from the geom's position and extends in the direction of
the geom's local Z-axis.
Constructor::
GeomRay(space=None, rlen=1.0)
"""
def __cinit__(self, space=None, rlen=1.0):
cdef SpaceBase sp
cdef dSpaceID sid
sid = NULL
if space != None:
sp = space
sid = sp.sid
self.gid = dCreateRay(sid, rlen)
# if space != None:
# space._addgeom(self)
_geom_c2py_lut[<size_t>self.gid] = self
def __init__(self, space=None, rlen=1.0):
self.space = space
self.body = None
def _id(self):
cdef size_t id
id = <size_t>self.gid
return id
def placeable(self):
return True
def setLength(self, rlen):
'''setLength(rlen)
Set length of the ray.
@param rlen: length of the ray
@type rlen: float'''
dGeomRaySetLength(self.gid, rlen)
def getLength(self):
'''getLength() -> length
Get the length of the ray.
@returns: length of the ray (float)'''
return dGeomRayGetLength(self.gid)
def set(self, p, u):
'''set(p, u)
Set the position and rotation of a ray.
@param p: position
@type p: 3-sequence of floats
@param u: rotation
@type u: 3-sequence of floats'''
dGeomRaySet(self.gid, p[0], p[1], p[2], u[0], u[1], u[2])
def get(self):
'''get() -> ((p[0], p[1], p[2]), (u[0], u[1], u[2]))
Return the position and rotation as a pair of
tuples.
@returns: position and rotation'''
cdef dVector3 start
cdef dVector3 dir
dGeomRayGet(self.gid, start, dir)
return (start[0], start[1], start[2]), (dir[0], dir[1], dir[2])
# GeomTransform
cdef class GeomTransform(GeomObject):
"""GeomTransform.
A geometry transform "T" is a geom that encapsulates another geom
"E", allowing E to be positioned and rotated arbitrarily with
respect to its point of reference.
Constructor::
GeomTransform(space=None)
"""
cdef object geom
def __cinit__(self, space=None):
cdef SpaceBase sp
cdef dSpaceID sid
sid = NULL
if space != None:
sp = space
sid = sp.sid
self.gid = dCreateGeomTransform(sid)
# Set cleanup mode to 0 as a contained geom will be deleted
# by its Python wrapper class
dGeomTransformSetCleanup(self.gid, 0)
# if space != None:
# space._addgeom(self)
_geom_c2py_lut[<size_t>self.gid] = self
def __init__(self, space=None):
self.space = space
self.body = None
self.geom = None
self.attribs = {}
def placeable(self):
return True
def _id(self):
cdef size_t id
id = <size_t>self.gid
return id
def setGeom(self, GeomObject geom not None):
"""setGeom(geom)
Set the geom that the geometry transform encapsulates.
A ValueError exception is thrown if a) the geom is not placeable,
b) the geom was already inserted into a space or c) the geom is
already associated with a body.
@param geom: Geom object to encapsulate
@type geom: GeomObject
"""
cdef size_t id
if not geom.placeable():
raise ValueError(
"Only placeable geoms can be encapsulated by a GeomTransform")
if dGeomGetSpace(geom.gid) != <dSpaceID>0:
raise ValueError(
"The encapsulated geom was already inserted into a space")
if dGeomGetBody(geom.gid) != <dBodyID>0:
raise ValueError(
"The encapsulated geom is already associated with a body")
id = geom._id()
dGeomTransformSetGeom(self.gid, <dGeomID>id)
self.geom = geom
def getGeom(self):
"""getGeom() -> GeomObject
Get the geom that the geometry transform encapsulates.
"""
return self.geom
def setInfo(self, int mode):
"""setInfo(mode)
Set the "information" mode of the geometry transform.
With mode 0, when a transform object is collided with another
object, the geom field of the ContactGeom structure is set to the
geom that is encapsulated by the transform object.
With mode 1, the geom field of the ContactGeom structure is set
to the transform object itself.
@param mode: Information mode (0 or 1)
@type mode: int
"""
if mode < 0 or mode > 1:
raise ValueError(
"Invalid information mode (%d). Must be either 0 or 1." % mode)
dGeomTransformSetInfo(self.gid, mode)
def getInfo(self):
"""getInfo() -> int
Get the "information" mode of the geometry transform (0 or 1).
With mode 0, when a transform object is collided with another
object, the geom field of the ContactGeom structure is set to the
geom that is encapsulated by the transform object.
With mode 1, the geom field of the ContactGeom structure is set
to the transform object itself.
"""
return dGeomTransformGetInfo(self.gid)
######################################################################
cdef class TriMeshData:
"""This class stores the mesh data.
"""
cdef dTriMeshDataID tmdid
cdef dReal* vertex_buffer
cdef int* face_buffer
def __cinit__(self):
self.tmdid = dGeomTriMeshDataCreate()
self.vertex_buffer = NULL
self.face_buffer = NULL
def __dealloc__(self):
if self.tmdid != NULL:
dGeomTriMeshDataDestroy(self.tmdid)
if self.vertex_buffer != NULL:
free(self.vertex_buffer)
if self.face_buffer != NULL:
free(self.face_buffer)
def build(self, verts, faces):
"""build(verts, faces)
@param verts: Vertices
@type verts: Sequence of 3-sequences of floats
@param faces: Face definitions (three indices per face)
@type faces: Sequence of 3-sequences of ints
"""
cdef int numverts
cdef int numfaces
cdef dReal* vp
cdef int* fp
cdef int a, b, c
numverts = len(verts)
numfaces = len(faces)
# Allocate the vertex and face buffer
self.vertex_buffer = <dReal*>malloc(numverts * 4 * sizeof(dReal))
self.face_buffer = <int*>malloc(numfaces * 3 * sizeof(int))
# Fill the vertex buffer
vp = self.vertex_buffer
for v in verts:
vp[0] = v[0]
vp[1] = v[1]
vp[2] = v[2]
vp[3] = 0
vp = vp + 4
# Fill the face buffer
fp = self.face_buffer
for f in faces:
a = f[0]
b = f[1]
c = f[2]
if (a < 0 or b < 0 or c < 0 or a >= numverts or b >= numverts or c >= numverts):
raise ValueError("Vertex index out of range")
fp[0] = a
fp[1] = b
fp[2] = c
fp = fp + 3
# Pass the data to ODE
dGeomTriMeshDataBuildSimple(self.tmdid, self.vertex_buffer, numverts,
self.face_buffer, numfaces * 3)
######################################################################
# GeomTriMesh
cdef class GeomTriMesh(GeomObject):
"""TriMesh object.
To construct the trimesh geom you need a TriMeshData object that
stores the actual mesh. This object has to be passed as first
argument to the constructor.
Constructor::
GeomTriMesh(data, space=None)
"""
# Keep a reference to the data
cdef TriMeshData data
def __cinit__(self, TriMeshData data not None, space=None):
cdef SpaceBase sp
cdef dSpaceID sid
self.data = data
sid = NULL
if space != None:
sp = space
sid = sp.sid
self.gid = dCreateTriMesh(sid, data.tmdid, NULL, NULL, NULL)
_geom_c2py_lut[<size_t>self.gid] = self
def __init__(self, TriMeshData data not None, space=None):
self.space = space
self.body = None
def placeable(self):
return True
def _id(self):
cdef size_t id
id = <size_t>self.gid
return id
def clearTCCache(self):
"""clearTCCache()
Clears the internal temporal coherence caches.
"""
dGeomTriMeshClearTCCache(self.gid)
def getTriangle(self, int idx):
"""getTriangle(idx) -> (v0, v1, v2)
@param idx: Triangle index
@type idx: int
"""
cdef dVector3 v0, v1, v2
cdef dVector3* vp0
cdef dVector3* vp1
cdef dVector3* vp2
vp0 = <dVector3*>v0
vp1 = <dVector3*>v1
vp2 = <dVector3*>v2
dGeomTriMeshGetTriangle(self.gid, idx, vp0, vp1, vp2)
return ((v0[0], v0[1], v0[2]),
(v1[0], v1[1], v1[2]),
(v2[0], v2[1], v2[2]))
def getTriangleCount(self):
"""getTriangleCount() -> n
Returns the number of triangles in the TriMesh."""
return dGeomTriMeshGetTriangleCount(self.gid)
######################################################################
def collide(geom1, geom2):
"""collide(geom1, geom2) -> contacts
Generate contact information for two objects.
Given two geometry objects that potentially touch (geom1 and geom2),
generate contact information for them. Internally, this just calls
the correct class-specific collision functions for geom1 and geom2.
[flags specifies how contacts should be generated if the objects
touch. Currently the lower 16 bits of flags specifies the maximum
number of contact points to generate. If this number is zero, this
function just pretends that it is one - in other words you can not
ask for zero contacts. All other bits in flags must be zero. In
the future the other bits may be used to select other contact
generation strategies.]
If the objects touch, this returns a list of Contact objects,
otherwise it returns an empty list.
@param geom1: First Geom
@type geom1: GeomObject
@param geom2: Second Geom
@type geom2: GeomObject
@returns: Returns a list of Contact objects.
"""
cdef dContactGeom c[150]
cdef size_t id1
cdef size_t id2
cdef int i, n
cdef Contact cont
id1 = geom1._id()
id2 = geom2._id()
n = dCollide(<dGeomID>id1, <dGeomID>id2, 150, c, sizeof(dContactGeom))
res = []
i = 0
while i < n:
cont = Contact()
cont._contact.geom = c[i]
res.append(cont)
i = i + 1
return res
def collide2(geom1, geom2, arg, callback):
"""collide2(geom1, geom2, arg, callback)
Calls the callback for all potentially intersecting pairs that contain
one geom from geom1 and one geom from geom2.
@param geom1: First Geom
@type geom1: GeomObject
@param geom2: Second Geom
@type geom2: GeomObject
@param arg: A user argument that is passed to the callback function
@param callback: Callback function
@type callback: callable
"""
cdef void* data
cdef object tup
cdef size_t id1
cdef size_t id2
id1 = geom1._id()
id2 = geom2._id()
tup = (callback, arg)
data = <void*>tup
# collide_callback is defined in space.pyx
dSpaceCollide2(<dGeomID>id1, <dGeomID>id2, data, collide_callback)
def areConnected(Body body1, Body body2):
"""areConnected(body1, body2) -> bool
Return True if the two bodies are connected together by a joint,
otherwise return False.
@param body1: First body
@type body1: Body
@param body2: Second body
@type body2: Body
@returns: True if the bodies are connected
"""
if body1 is environment:
return False
if body2 is environment:
return False
return bool(dAreConnected(<dBodyID> body1.bid, <dBodyID> body2.bid))
def CloseODE():
"""CloseODE()
Deallocate some extra memory used by ODE that can not be deallocated
using the normal destroy functions.
"""
dCloseODE()
def InitODE():
'''InitODE()
Initialize some ODE internals. This will be called for you when you
"import ode", but you should call this again if you CloseODE().'''
dInitODE()
#environment = Body(None)
environment = None
InitODE()
Reference in New Issue Copy Permalink