4507 lines
126 KiB
Cython
4507 lines
126 KiB
Cython
######################################################################
|
|
# 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()
|