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 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)