def addContact(self, geom1, geom2, c):
# add a contact between a capped cylinder BP and the ground
(cpos, cnor, cdep, cg1, cg2) = c.getContactGeomParams()
# figure out which cylinder foot this contact describes
if type(geom1) is ode.GeomCCylinder:
cylinder = geom1
elif type(geom2) is ode.GeomCCylinder:
cylinder = geom2
# find endpoints of cylinder
r = mat3(cylinder.getRotation())
p = vec3(cylinder.getPosition())
(radius, length) = cylinder.getParams()
# is collision point c in an endpoint?
ep0 = p + r * vec3(0, 0, -length / 2)
ep1 = p + r * vec3(0, 0, length / 2)
# is cpos in sphere around ep0 or ep1?
for ep in ep0, ep1:
cpos = vec3(cpos)
d2 = (cpos - ep).length()
if (d2 <= radius + 0.1):
epc = ep
# we will get two addContact() calls for each real contact, one for each
# of the joined capped cylinders, so only add a contact for the
# 'furthest out' from the root. If geom is the root then always add the
# contact.
if not cylinder.parent or epc == ep1:
mu = 300
self.points.append((cpos, (0, 1, 1), mu / 1000.0))
c.setMu(mu)
j = ode.ContactJoint(self.world, self.contactGroup, c)
j.attach(geom1.getBody(), geom2.getBody())
# remember contact for touch sensors
self.geom_contact[geom1] = 1
self.geom_contact[geom2] = 1
def calculate_quaternion(self):
from cgkit.cgtypes import quat, mat3, slerp
q = quat()
mat = mat3([num for row in self.transform[0:3] for num in row[0:3]])
mat = mat.decompose()[0]
q.fromMat(mat)
self.q = np.dot(np.array([q.x,q.y,q.z]),q.w).tolist()
def update_normal(self, regime): r_hat, u_hat, f_hat = self.get_basis(regime) norm = cgt.mat3([ r_hat[0], r_hat[1], r_hat[2], u_hat[0], u_hat[1], u_hat[2], f_hat[0], f_hat[1], f_hat[2]]) self._normal_matrix_[:] = np.array(norm).flatten() return norm
def _createWall(self, objName, objData):
lx, ly, lz = vec3(objData['lengths'])
pos = vec3(objData['position'])
x, y, z= objData['rotation']
rot = mat3().fromEulerXYZ(radians(x), radians(y), radians(z))
obj = Box(name=objName, lx=lx, ly=ly, lz=lz, pos=pos, rot=rot,
static=True)
logger.info("using box lengths: (%s, %s, %s)" % (lx, ly, lz))
logger.info("using box position: %s" % pos)
return obj
def _getInitialValues(self, type):
lx = self.cfg['link']['lengths'][0]
axis = vec3(0, 1, 0)
#distance = 0.75*lx
distance = 0.5*lx
if type == ACTIVE:
# base position and rotation
pos = self.pos + vec3(-distance, 0, 0)
rot = mat3().rotation(radians(90), axis)
elif type == PASSIVE:
# base position
pos = self.pos + vec3(distance, 0, 0)
rot = mat3().rotation(radians(-90), axis)
elif type == LINK:
# base position
pos = self.pos
rot = mat3(1.0)
else:
raise ValueError("Invalid object type (%s)." % type)
return (pos, rot)
def rbm_to_dualquat(rbm):
import cgkit.cgtypes as cg
q0 = cg.quat().fromMat(cg.mat3(rbm[:3, :3].T.tolist()))
q0 = q0.normalize()
q0 = np.array([q0.w, q0.x, q0.y, q0.z])
t = rbm[:3, 3]
q1 = np.array([
-0.5 * (t[0] * q0[1] + t[1] * q0[2] + t[2] * q0[3]),
0.5 * (t[0] * q0[0] + t[1] * q0[3] - t[2] * q0[2]),
0.5 * (-t[0] * q0[3] + t[1] * q0[0] + t[2] * q0[1]),
0.5 * (t[0] * q0[2] - t[1] * q0[1] + t[2] * q0[0])
])
return np.array(q0.tolist() + q1.tolist())
def __init__( self , q=[1,0,0,0], A=None ):
if ( A != None ):
# case where we have provided a numpy array as a rotation matrix
# convert to mat3
A = mat3( [ A[0,0], A[0,1], A[0,2],
A[1,0], A[1,1], A[1,2],
A[2,0], A[2,1], A[2,2] ] )
# initialise the quaternion
cgquat.__init__( self, A )
else:
# default is the four elements
cgquat.__init__( self, q )
def calculate_quaternion(self):
from cgkit.cgtypes import quat, mat3, slerp
q = quat()
mat = mat3([num for row in self.transform[0:3] for num in row[0:3]])
try:
mat = mat.decompose()[0]
q.fromMat(mat)
except ZeroDivisionError:
q = quat(0,0,0,0)
self.do_not_use = True
a = 2*math.acos(q.w)
t = np.dot(np.array([q.x,q.y,q.z]),a)
self.rot_axis = t.tolist()
self.q = np.dot(np.array([q.x,q.y,q.z]),q.w).tolist()
def __init__(self,
world,
name = "Module",
pos = vec3(),
rot = mat3(),
robot = None,
cfg = None,
data = None):
self._initialized = False
Module.__init__(self, world, name=name, pos=pos, rot=rot, robot=robot, cfg=cfg, data=data)
self._modules = data['class']['module']
self._index = 0
self.data = data
moduleClassName = self._modules[0]
moduleClass = getattr(__import__('mrsim.module', None, None, ['']), moduleClassName)
self._current = moduleClass
self._initialized = True
def __init__(self, q=[1, 0, 0, 0], A=None):
if (A != None):
# case where we have provided a numpy array as a rotation matrix
# convert to mat3
A = mat3([
A[0, 0], A[0, 1], A[0, 2], A[1, 0], A[1, 1], A[1, 2], A[2, 0],
A[2, 1], A[2, 2]
])
# initialise the quaternion
cgquat.__init__(self, A)
else:
# default is the four elements
cgquat.__init__(self, q)
def _createModule(self, name, cfg, data=None):
moduleData = self.data['module'][name]
pos = vec3(moduleData['position'])
dX, dY, dZ = moduleData['rotation']
rot = mat3().fromEulerXYZ(radians(dX), radians(dY), radians(dZ))
lightSource = self._getLightSources()[0]
obstacles = self._getObstacles()
cfg['sensor'] = cfg.setdefault('sensor', {'direction':{}, 'obstacle':{}})
cfg['sensor']['direction']['emitter'] = lightSource
cfg['sensor']['obstacle']['obstacles'] = obstacles
cfg['block']['sensor'] = cfg['block'].setdefault('sensor', {})
cfg['block']['sensor']['emitter'] = lightSource
cfg['link']['motor']['angle'] = {'active': moduleData['angleActive'],
'passive': moduleData['anglePassive']}
cfg['link']['motor']['class'] = self.data['class']['motor']
moduleClass = getattr(__import__('mrsim.module', None, None, ['']), self.data['class']['module'])
module = moduleClass(self.world, name=name, pos=pos, rot=rot, cfg=cfg, data=data)
logger.info("using module pos: %s" % pos)
return module
def __init__(self,
world,
name = "Module",
pos = vec3(),
rot = mat3(),
robot = None,
cfg = None,
data = None):
Component.__init__(self, name=name)
self.world = world
self.pos = pos
self.rot = rot
self.robot = robot
self.cfg = cfg
self.data = data
self.sensor = {}
self._createBlocks()
self._createLink()
self._connectObjects()
self._positionObjects()
self._configureSensors()
def renderGeoms(self):
log.debug('rendering %d geoms', self.sim.space.getNumGeoms())
if self.wireframe:
gluQuadricDrawStyle(self.quadratic, GLU_SILHOUETTE)
else:
gluQuadricDrawStyle(self.quadratic, GLU_FILL)
for geom in self.sim.space:
glPushMatrix()
if type(geom) is ode.GeomSphere:
log.debug('draw sphere')
glColor(0, 0, 1)
x, y, z = geom.getPosition()
glTranslate(x, y, z)
glutSolidSphere(geom.getRadius(), 20, 20)
elif type(geom) is ode.GeomPlane:
pass
elif type(geom) is ode.GeomBox:
log.debug('draw box(%s) @(%s)', str(geom.getLengths()),
str(geom.getPosition()))
glColor(0.8, 0, 0)
x, y, z = geom.getPosition()
# create openGL 4x4 transform matrix from ODE 3x3 rotation matrix
R = geom.getRotation()
log.debug('ROTATE = %s', str(R)) # R is a 3x3 matrix
T = mat4()
T.setMat3(mat3(R))
T.setColumn(3, vec4(x, y, z, 1.0))
glMultMatrixd(T.toList())
(sx, sy, sz) = geom.getLengths()
log.debug('size (%f,%f,%f)', sx, sy, sz)
glScale(sx, sy, sz)
if self.wireframe:
glutWireCube(1)
else:
glutSolidCube(1)
elif type(geom) is ode.GeomCCylinder:
log.debug('draw ccylinder')
def red():
glColor(1, 0, 0)
def green():
glColor(0, 1, 0)
def blue():
glColor(0, 0, 1)
def plot_axes(ax, ay, az):
for axis, colour in (ax, red), (ay, blue), (az, green):
if axis:
x, y, z = axis
colour()
glBegin(GL_LINES)
glVertex(0, 0, 0)
glVertex(x * 5, y * 5, z * 5)
glEnd()
if self.render_axes and hasattr(geom, 'motor'):
glPushMatrix()
glDisable(GL_LIGHTING)
m = geom.motor
x, y, z = m.joint.getAnchor()
glTranslate(x, y, z)
ax = None
ay = None
az = None
if isinstance(m.joint, ode.HingeJoint):
ax = m.joint.getAxis()
elif isinstance(m.joint, ode.UniversalJoint):
ax = m.joint.getAxis1()
ay = m.joint.getAxis2()
plot_axes(ax, ay, az)
# plot motor axes
# these should be aligned with the joint axes above
ax, ay, az = m.getAxis(0), m.getAxis(1), m.getAxis(2)
plot_axes(ax, ay, az)
glEnable(GL_LIGHTING)
glPopMatrix()
# construct transformation matrix from position and rotation
x, y, z = geom.getPosition()
rotmat = mat3(geom.getRotation())
log.debug('r=%s', geom.getRotation())
# ode ccylinders are aligned along z axis by default
T = mat4()
T.setMat3(rotmat)
T.setColumn(3, vec4(x, y, z, 1.0))
log.debug('geom matrix T is %s', str(T))
glMultMatrixd(T.toList())
(radius, length) = geom.getParams()
log.debug('geom len=%f xyz=%f,%f,%f', length, x, y, z)
# plot the geom
self.plotAxes()
if self.render_bps:
glTranslate(0, 0, -length / 2)
b = 0.8
if not geom.parent:
b = 0.0
glColor(0, 0, b)
gluCylinder(self.quadratic, radius, radius, length, 16, 16)
if geom.left == 'internal':
glColor(0, 1, 0)
else:
glColor(1, 0, 0)
gluSphere(self.quadratic, radius, 10, 10)
glTranslate(0, 0, length)
if geom.right == 'internal':
glColor(0, 1, 0)
else:
glColor(1, 0, 0)
gluSphere(self.quadratic, radius, 10, 10)
else:
log.critical('dont know how to render geom %s', str(geom))
glPopMatrix()
def get_quaternion_from_matrix(matrix):
q = quat()
mat = mat3([num for row in matrix for num in row])
mat = mat.decompose()[0]
q.fromMat(mat)
return q
def get_quaternion_from_matrix(matrix):
q = quat()
mat = mat3([num for row in matrix for num in row])
mat = mat.decompose()[0]
q.fromMat(mat)
return q
def addBP(self, bp, parent=None, joint_end=None):
"""Recursively add BodyParts to the simulation.
bp -- current BodyPart to process
parent -- parent BP to add to
joint_end -- which end of the parent ccylinder we should add to"""
log.debug('addBP')
body = ode.Body(self.world)
mass = ode.Mass()
if not parent:
# if this is the root we have an absolute length,
# root scale is relative to midpoint of min..max
bp.length = bp.scale * (
bpg.BP_MIN_LENGTH +
(bpg.BP_MAX_LENGTH - bpg.BP_MIN_LENGTH) / 2)
bp.isRoot = 1
else:
# otherwise child scale is relative to parent
bp.length = parent.length * bp.scale
bp.isRoot = 0
# limit the bp length
bp.length = min(bp.length, bpg.BP_MAX_LENGTH)
# mass along x axis, with length without caps==bp.length
# arg 3 means aligned along z-axis - must be same in renderer and Geoms
mass.setCappedCylinder(CYLINDER_DENSITY, 3, CYLINDER_RADIUS, bp.length)
# attach mass to body
body.setMass(mass)
# create Geom
# aligned along z-axis by default!!
geom = ode.GeomCCylinder(self.space, CYLINDER_RADIUS, bp.length)
self.geoms.append(geom)
self.geom_contact[geom] = 0
# remember parent for collison detection
if not parent:
geom.parent = None
else:
geom.parent = parent.geom
# attach geom to body
geom.setBody(body)
log.debug('created CappedCylinder(radius=%f, len=%f)', CYLINDER_RADIUS,
bp.length)
# assert(not in a loop)
assert not hasattr(bp, 'geom')
# ref geom from bodypart (used above to find parent geom)
bp.geom = geom
# set rotation
(radians, v) = bp.rotation
log.debug('radians,v = %f,%s', radians, str(v))
q = quat(radians, vec3(v))
rotmat = q.toMat3()
if parent:
# rotate relative to parent
p_r = mat3(parent.geom.getRotation()) # joint_end *
log.debug('parent rotation = %s', str(p_r))
rotmat = p_r * rotmat
geom.setRotation(rotmat.toList(rowmajor=1))
log.debug('r=%s', str(rotmat))
geom_axis = rotmat * vec3(0, 0, 1)
log.debug('set geom axis to %s', str(geom_axis))
(x, y, z) = geom.getBody().getRelPointPos((0, 0, bp.length / 2.0))
log.debug('real position of joint is %f,%f,%f', x, y, z)
# set position
if not parent:
# root - initially located at 0,0,0
# (once the model is constructed we translate it until all
# bodies have z>0)
geom.setPosition((0, 0, 0))
log.debug('set root geom x,y,z = 0,0,0')
else:
# child - located relative to the parent. from the
# parents position move along their axis of orientation to
# the joint position, then pick a random angle within the
# joint limits, move along that vector by half the length
# of the child cylinder, and we have the position of the
# child.
# vector from parent xyz is half of parent length along
# +/- x axis rotated by r
p_v = vec3(parent.geom.getPosition())
p_r = mat3(parent.geom.getRotation())
p_hl = parent.geom.getParams()[1] / 2.0 # half len of par
j_v = p_v + p_r * vec3(0, 0, p_hl * joint_end) # joint vector
# rotation is relative to parent
c_v = j_v + rotmat * vec3(0, 0, bp.length / 2.0)
geom.setPosition(tuple(c_v))
log.debug('set geom x,y,z = %f,%f,%f', c_v[0], c_v[1], c_v[2])
jointclass = {
'hinge': ode.HingeJoint,
'universal': ode.UniversalJoint,
'ball': ode.BallJoint
}
j = jointclass[bp.joint](self.world)
self.joints.append(j)
# attach bodies to joint
j.attach(parent.geom.getBody(), body)
# set joint position
j.setAnchor(j_v)
geom.parent_joint = j
# create motor and attach to this geom
motor = Motor(self.world, None)
motor.setJoint(j)
self.joints.append(motor)
bp.motor = motor
geom.motor = motor
motor.attach(parent.geom.getBody(), body)
motor.setMode(ode.AMotorEuler)
if bp.joint == 'hinge':
# we have 3 points - parent body, joint, child body
# find the normal to these points
# (hinge only has 1 valid axis!)
try:
axis1 = ((j_v - p_v).cross(j_v - c_v)).normalize()
except ZeroDivisionError:
v = (j_v - p_v).cross(j_v - c_v)
v.z = 1**-10
axis1 = v.normalize()
log.debug('setting hinge joint axis to %s', axis1)
log.debug('hinge axis = %s', j.getAxis())
axis_inv = rotmat.inverse() * axis1
axis2 = vec3((0, 0, 1)).cross(axis_inv)
log.debug('hinge axis2 = %s', axis2)
j.setAxis(tuple(axis1))
# some anomaly here.. if we change the order of axis2 and axis1,
# it should make no difference. instead there appears to be an
# instability when the angle switches from -pi to +pi
# so.. use parameter3 to control the hinge
# (maybe this is only a problem in the test case with perfect axis alignment?)
motor.setAxes(axis2, rotmat * axis1)
elif bp.joint == 'universal':
# bp.axis1/2 is relative to bp rotation, so rotate axes
axis1 = rotmat * vec3(bp.axis1)
axis2 = rotmat * vec3((0, 0, 1)).cross(vec3(bp.axis1))
j.setAxis1(tuple(axis1))
j.setAxis2(tuple(axis2))
motor.setAxes(axis1, axis2)
elif bp.joint == 'ball':
axis1 = rotmat * vec3(bp.axis1)
axis2 = rotmat * vec3((0, 0, 1)).cross(vec3(bp.axis1))
motor.setAxes(axis1, axis2)
log.debug('created joint with parent at %f,%f,%f', j_v[0], j_v[1],
j_v[2])
# recurse on children
geom.child_joint_ends = set([e.joint_end for e in bp.edges])
geom.parent_joint_end = joint_end
if joint_end == None:
# root
if -1 in geom.child_joint_ends:
geom.left = 'internal'
else:
geom.left = 'external'
if 1 in geom.child_joint_ends:
geom.right = 'internal'
else:
geom.right = 'external'
else:
# not root
geom.left = 'internal'
if 1 in geom.child_joint_ends:
geom.right = 'internal'
else:
geom.right = 'external'
for e in bp.edges:
self.addBP(e.child, bp, e.joint_end)
