def __init__(self, world, config): """ Create articulated rigid body fly object for dynamic simulation. The fly's body is initialized so that it's center of mass is (0,0,0) in world coordinates. Inputs: world = ODE simulation world object config = simulation configuration dictionary """ # Create fly body self.body = ode.Body(world) # Create wings self.r_wing = ode.Body(world) self.l_wing = ode.Body(world) # Create wing joints self.r_joint = ode.BallJoint(world) self.l_joint = ode.BallJoint(world) # Create angular motors self.r_amotor = ode.AMotor(world) self.l_amotor = ode.AMotor(world) # Set body according to configuration self.set2Config(config)
def buildObjects(): world = ode.World() world.setGravity((0, -9.81, 0)) body1 = ode.Body(world) M = ode.Mass() M.setSphere(2500, 0.05) body1.setMass(M) body1.setPosition((1, 2, 0)) body2 = ode.Body(world) M = ode.Mass() M.setSphere(2500, 0.05) body2.setMass(M) body2.setPosition((2, 2, 0)) j1 = ode.BallJoint(world) j1.attach(body1, ode.environment) j1.setAnchor((0, 2, 0)) #j1 = ode.HingeJoint(world) #j1.attach(body1, ode.environment) #j1.setAnchor( (0,2,0) ) #j1.setAxis( (0,0,1) ) #j1.setParam(ode.ParamVel, 3) #j1.setParam(ode.ParamFMax, 22) j2 = ode.BallJoint(world) j2.attach(body1, body2) j2.setAnchor((1, 2, 0)) return world, body1, body2, j1, j2
def addBallJoint(self, body1, body2, anchor, baseAxis, baseTwistUp, flexLimit = pi, twistLimit = pi, flexForce = 0.0, twistForce = 0.0): anchor = add3(anchor, self.offset) # create the joint joint = ode.BallJoint(self.world) joint.attach(body1, body2) joint.setAnchor(anchor) # store the base orientation of the joint in the local coordinate system # of the primary body (because baseAxis and baseTwistUp may not be # orthogonal, the nearest vector to baseTwistUp but orthogonal to # baseAxis is calculated and stored with the joint) joint.baseAxis = getBodyRelVec(body1, baseAxis) tempTwistUp = getBodyRelVec(body1, baseTwistUp) baseSide = norm3(cross(tempTwistUp, joint.baseAxis)) joint.baseTwistUp = norm3(cross(joint.baseAxis, baseSide)) # store the base twist up vector (original version) in the local # coordinate system of the secondary body joint.baseTwistUp2 = getBodyRelVec(body2, baseTwistUp) # store joint rotation limits and resistive force factors joint.flexLimit = flexLimit joint.twistLimit = twistLimit joint.flexForce = flexForce joint.twistForce = twistForce joint.style = "ball" self.joints.append(joint) return joint
def attach(self, frame_no): '''Attach marker bodies to the corresponding skeleton bodies. Attachments are only made for markers that are not in a dropout state in the given frame. Parameters ---------- frame_no : int The frame of data we will use for attaching marker bodies. ''' assert not self.joints for label, j in self.channels.items(): target = self.targets.get(label) if target is None: continue if self.visibility[frame_no, j] < 0: continue if np.linalg.norm(self.velocities[frame_no, j]) > 10: continue joint = ode.BallJoint(self.world.ode_world, self.jointgroup) joint.attach(self.bodies[label].ode_body, target.ode_body) joint.setAnchor1Rel([0, 0, 0]) joint.setAnchor2Rel(self.offsets[label]) joint.setParam(ode.ParamCFM, self.cfms[frame_no, j]) joint.setParam(ode.ParamERP, self.erp) joint.name = label self.joints[label] = joint self._frame_no = frame_no
def end(name): if (name == 'ball'): joint = ode.BallJoint(world, self._jg) joint.attach(link1, link2) if (anchor[0] is not None): joint.setAnchor(anchor[0]) self.setODEObject(joint) self._parser.pop()
def addBallJoint(self, body1, body2, anchor): anchor = add3(anchor, self.offset) # create the joint joint = ode.BallJoint(self.sim.world) joint.attach(body1, body2) joint.setAnchor(anchor) joint.style = "ball" self.joints.append(joint) return joint
def joinBody(self, world, space, body, type, index): if pygame.time.get_ticks() - self.pickupdelay < 500: return self.pickupJoint = ode.BallJoint(world) self.pickupJoint.attach(body, self.mainbody.body) pos = self.mainbody.body.getPosition() realpos = (pos[0], pos[1], 0) self.pickupJoint.setAnchor(realpos) self.carryingitem = 1 self.pickupdelay = pygame.time.get_ticks() self.carrytype = type self.carryindex = index
def addBallJoint(self, body1, body2, anchor, baseAxis=None, baseTwistUp=None, flexLimit=pi, twistLimit=pi, flexForce=0.0, twistForce=0.0): def getBodyRelVec(b, v): """ Returns the 3-vector v transformed into the local coordinate system of ODE body b. """ return rotate3(invert3x3(b.getRotation()), v) anchor = add3(anchor, self.offset) # create the joint joint = ode.BallJoint(self.world) joint.attach(body1, body2) joint.setAnchor(anchor) if baseAxis is not None: joint.haveAxis = True # store the base orientation of the joint in the local coordinate system # of the primary body (because baseAxis and baseTwistUp may not be # orthogonal, the nearest vector to baseTwistUp but orthogonal to # baseAxis is calculated and stored with the joint) joint.baseAxis = getBodyRelVec(body1, baseAxis) tempTwistUp = getBodyRelVec(body1, baseTwistUp) baseSide = norm3(cross(tempTwistUp, joint.baseAxis)) joint.baseTwistUp = norm3(cross(joint.baseAxis, baseSide)) # store the base twist up vector (original version) in the local # coordinate system of the secondary body joint.baseTwistUp2 = getBodyRelVec(body2, baseTwistUp) # store joint rotation limits and resistive force factors joint.flexLimit = flexLimit joint.twistLimit = twistLimit joint.flexForce = flexForce joint.twistForce = twistForce else: joint.haveAxis = False joint.style = "ball" self.joints.append(joint) return joint
def create_ball(self, key, pos, bod_keys): """ Create a hinge joint. Arguments: key : number id to assign the hinge pos : position of the joint bod_keys : list of two bodies to attach the joint to """ # Auto label the joint key or not. key = len(self.joints) if key == -1 else key j = ode.BallJoint(self.world) j.attach(self.bodies[bod_keys[0]], self.bodies[bod_keys[1]]) j.setAnchor((pos)) j.style = "ball" self.joints[key] = j return key
def __init__(self,num): # Create a world object self.world = ode.World() self.world.setGravity( (0,-self.g,0) ) self.world.setERP(0.8) self.world.setCFM(1.e-5) self.space = ode.Space() self.contactgroup = ode.JointGroup() # create objects self.anchor = Sphere(self.world,self.space,-2,1.e30,0.001,False) self.pendulum = Sphere(self.world,self.space,-1,self.d,self.r,True) self.joint = ode.BallJoint(self.world) self.joint.attach(self.anchor.body,self.pendulum.body) self.sat = [Sphere(self.world,self.space,s,self.ds,self.rs,False) for s in range(num)] self.walls = ( ode.GeomPlane(self.space, ( 1, 0, 0), -(self.l+self.r)*1.1), ode.GeomPlane(self.space, ( 0, 1, 0), -(self.l+self.r)*1.1), ode.GeomPlane(self.space, ( 0, 0, 1), -(self.l+self.r)*1.1), ode.GeomPlane(self.space, (-1, 0, 0), -(self.l+self.r)*1.1), ode.GeomPlane(self.space, ( 0,-1, 0), -self.l*0.1), ode.GeomPlane(self.space, ( 0, 0,-1), -(self.l+self.r)*1.1) ) # remember bodies self.geoms = {} self.geoms[self.anchor.geom] = -2 self.geoms[self.pendulum.geom] = -1 for s in range(num): self.geoms[self.sat[s].geom] = s for s in range(len(self.walls)): self.geoms[self.walls[s]] = 1000+s self.reset_1()
def loaddata(self, vpoints): self.world = ode.World() self.world.setGravity((0, -9.81, 0)) #vpoints = parse_vpoints(mechanism) self.vlinks = {} for i, vpoint in enumerate(vpoints): for link in vpoint.links: if link in self.vlinks: self.vlinks[link].append(i) else: self.vlinks[link] = [i] self.bodies = [] for name in tuple(self.vlinks): if name == 'ground': continue vlink = self.vlinks[name] while len(vlink) > 2: n = vlink.pop() for anchor in vlink[:2]: i = 1 while 'link_{}'.format(i) in self.vlinks: i += 1 self.vlinks['link_{}'.format(i)] = (anchor, n) for name, vlink in self.vlinks.items(): #print(name, [(vpoints[i].cx, vpoints[i].cy) for i in vlink]) if name == 'ground': continue rad = 0.002 Mass = 1 link = ode.Body(self.world) M = ode.Mass() # mass parameter M.setZero() # init the Mass M.setCylinderTotal( Mass, 3, rad, self.__lenth(vpoints[vlink[0]], vpoints[vlink[1]])) link.setPosition( self.__lenthCenter(vpoints[vlink[0]], vpoints[vlink[1]])) print(vlink) self.bodies.append(link) self.joints = [] for name, vlink in self.vlinks.items(): link = list(vlink) if name == 'ground': for p in link: if p in inputs: print("input:", p) j = ode.HingeJoint(self.world) #j.attach(bodies[(vlinks[inputs[p][1]] - {p}).pop()], ode.environment) j.attach(self.bodies[0], ode.environment) j.setAxis((0, 0, 1)) j.setAnchor(self.__translate23d(vpoints[vlink[0]])) j.setParam(ode.ParamVel, 2) j.setParam(ode.ParamFMax, 22000) else: print("grounded:", p) j = ode.BallJoint(self.world) j.attach(self.bodies[p], ode.environment) j.setAnchor(self.__translate23d(vpoints[vlink[1]])) self.joints.append(j) elif len(link) >= 2: print("link:", link[0], link[1]) j = ode.BallJoint(self.world) j.attach(self.bodies[link[0]], self.bodies[link[1]]) j.setAnchor(self.__translate23d(vpoints[link[0]])) self.joints.append(j)
link = list(vlink) print(link) if name == 'ground': for p in link: if p in inputs: print("input:", p) j = ode.HingeJoint(world) #j.attach(bodies[(vlinks[inputs[p][1]] - {p}).pop()], ode.environment) j.attach(bodies[1], ode.environment) j.setAxis((0, 0, 1)) j.setAnchor(bodies[0].getPosition()) j.setParam(ode.ParamVel, 2) j.setParam(ode.ParamFMax, 22000) else: print("grounded:", p) j = ode.BallJoint(world) j.attach(bodies[p], ode.environment) j.setAnchor(bodies[p].getPosition()) joints.append(j) elif len(link) >= 2: print("link:", link[0], link[1]) j = ode.BallJoint(world) j.attach(bodies[link[0]], bodies[link[1]]) j.setAnchor(bodies[link[0]].getPosition()) joints.append(j) # TODO : need to add select method in this joint type for p in link[2:]: print("other:", p, link[0], link[1]) for k in range(2): j = ode.BallJoint(world) j.attach(bodies[p], bodies[link[k]])
brasfunc() jambefunc() cuissefunc() avantbrasfunc() cuisseL_jambeL = ode.HingeJoint(world) cuisseL_jambeL.attach(cuisseL, jambeL) cuisseL_jambeL.setAnchor((0.5, 1, 0)) cuisseL_jambeL.setAxis((1, 0, 0)) cuisseG_jambeG = ode.HingeJoint(world) cuisseG_jambeG.attach(cuisseG, jambeG) cuisseG_jambeG.setAnchor((-0.5, 1, 0)) cuisseG_jambeG.setAxis((1, 0, 0)) tronc_cuisseG = ode.BallJoint(world) tronc_cuisseG.attach(cuisseG, tronc) tronc_cuisseG.setAnchor((-0.5, 2, 0)) tronc_cuisseL = ode.BallJoint(world) tronc_cuisseL.attach(cuisseL, tronc) tronc_cuisseL.setAnchor((0.5, 2, 0)) tronc_brasG = ode.BallJoint(world) tronc_brasG.attach(brasG, tronc) tronc_brasG.setAnchor((-1, 4, 0)) tronc_brasL = ode.BallJoint(world) tronc_brasL.attach(brasL, tronc) tronc_brasL.setAnchor((1, 4, 0))
def __init__(self, world, body1, body2, p1, p2, hinge=None): # Cap on one side cap1 = ode.Body(world) cap1.color = (0, 128, 0, 255) m = ode.Mass() m.setCappedCylinderTotal(1.0, 3, 0.01, 0.01) cap1.setMass(m) # set parameters for drawing the body cap1.shape = "capsule" cap1.length = .05 cap1.radius = .05 cap1.setPosition(p1) # Attach the cap to the body u1 = ode.BallJoint(world) u1.attach(body1, cap1) u1.setAnchor(p1) u1.style = "ball" # Cap on other side cap2 = ode.Body(world) cap2.color = (0, 128, 0, 255) m = ode.Mass() m.setCappedCylinderTotal(1.0, 3, 0.01, 0.01) cap2.setMass(m) # set parameters for drawing the body cap2.shape = "capsule" cap2.length = .05 cap2.radius = .05 cap2.setPosition(p2) # Attach the cap to the body u2 = ode.BallJoint(world) u2.attach(body2, cap2) u2.setAnchor(p2) u2.style = "ball" # The all-important slider joint s = ode.SliderJoint(world) s.attach(cap1, cap2) s.setAxis(sub3(p1, p2)) s.setFeedback(True) self.body1 = body1 self.body2 = body2 self.cap1 = cap1 self.cap2 = cap2 self.u1 = u1 self.u2 = u2 self.slider = s self.gain = 1.0 self.neutral_length = dist3(p1, p2) self.length_target = dist3(p1, p2) if hinge is not None: # Hinge is the joint this linear actuator controls # This allows angular control self.hinge = hinge # Store these for later to save on math. # TODO: I am being lazy and assuming u1->u2 # is orthogonal to the hinge axis self.h_to_u1 = dist3(self.hinge.getAnchor(), self.u1.getAnchor()) self.h_to_u2 = dist3(self.hinge.getAnchor(), self.u2.getAnchor()) self.neutral_angle = thetaFromABC(self.h_to_u1, self.h_to_u2, self.neutral_length)
body4.setPosition((2,2, 0)) # Connect body1 with the static environment #j1 = ode.BallJoint(world) #j1.attach(body1, ode.environment) #j1.setAnchor( (0,2,0) ) j1 = ode.HingeJoint(world) j1.attach(body1, ode.environment) j1.setAnchor( (0,0,0) ) j1.setAxis( (0,0,1) ) j1.setParam(ode.ParamVel, 0) j1.setParam(ode.ParamFMax, 220) # Connect body2 with body1 j2 = ode.BallJoint(world) j2.attach(body1, body2) j2.setAnchor( (2,0,0) ) j3 = ode.BallJoint(world) j3.attach(body2, body3) j3.setAnchor((2, 0, 0)) j4 = ode.BallJoint(world) j4.attach(body3, ode.environment) j4.setAnchor((2, 2, 0)) # Simulation loop... fps = 50
def __init__(self, bones: List[mmdpy_bone.mmdpyBone], bodies: List[mmdpy_type.mmdpyTypePhysicsBody], joints: List[mmdpy_type.mmdpyTypePhysicsJoint]): self.bones: List[mmdpy_bone.mmdpyBone] = bones self.bodies: List[mmdpy_type.mmdpyTypePhysicsBody] = bodies self.joints: List[mmdpy_type.mmdpyTypePhysicsJoint] = joints self.ode_bodies: List[Any] = [] self.ode_joints: List[Any] = [] self.ode_geoms: List[Any] = [] # 世界生成 self.world = ode.World() self.world.setGravity([0, -9.81, 0]) self.world.setERP(0.80) self.world.setCFM(1e-5) # Create a space object self.space = ode.Space() # A joint group for the contact joints that are generated whenever # two bodies collide self.contactgroup = ode.JointGroup() # 剛体 for i, body in enumerate(self.bodies): body.cid = i body.bone = self.bones[body.bone_id] ode_body = ode.Body(self.world) mass = ode.Mass() density: float = 120.0 ode_geom = None if body.type_id == 0: # 球 mass.setSphere(density, body.sizes[0]) ode_geom = ode.GeomSphere(self.space, radius=body.sizes[0]) if body.type_id == 1: # 箱 mass.setBox(density, body.sizes[0], body.sizes[1], body.sizes[2]) ode_geom = ode.GeomBox(self.space, lengths=body.sizes) if body.type_id == 2: # カプセル mass.setCylinder(density, 2, body.sizes[0], body.sizes[1]) ode_geom = ode.GeomCylinder(self.space, radius=body.sizes[0], length=body.sizes[1]) mass.mass = (1 if body.calc == 0 else body.mass / density) ode_body.setMass(mass) if ode_geom is not None: ode_geom.setBody(ode_body) ode_body.setPosition(body.pos) quat: np.ndarray = scipy.spatial.transform.Rotation.from_rotvec( body.rot).as_matrix().reshape(9) ode_body.setRotation(quat) # debug body.calc = 0 self.ode_bodies.append(ode_body) self.ode_geoms.append(ode_geom) # joint # https://so-zou.jp/robot/tech/physics-engine/ode/joint/ for i, joint in enumerate(self.joints[:1]): joint.cid = i body_a = self.ode_bodies[joint.rigidbody_a] body_b = self.ode_bodies[joint.rigidbody_b] ode_joint = ode.BallJoint(self.world) # ode_joint = ode.FixedJoint(self.world) # ode_joint = ode.HingeJoint(self.world) # ode_joint.attach(ode.environment, body_b) ode_joint.attach(body_a, body_b) ode_joint.setAnchor(joint.pos) # ode_joint = ode.UniversalJoint(self.world) # ode_joint.attach(body_a, body_b) # ode_joint.setAnchor(joint.pos) # ode_joint.setAxis1(joint.rot) # ode_joint.setParam(ode.ParamLoStop, joint.) self.ode_joints.append(ode_joint) # # 接続確認 # print( # self.bodies[joint.rigidbody_a].name, # self.bodies[joint.rigidbody_b].name, # ode.areConnected(body_a, body_b) # ) # print(joint.pos, body_b.getPosition(), self.bodies[joint.rigidbody_b].pos) # debug self.bodies[joint.rigidbody_b].calc = 1 for i, body in enumerate(self.bodies): ode_body = self.ode_bodies[i] if body.calc == 0: ode_joint = ode.BallJoint(self.world) ode_joint.attach(ode.environment, ode_body) ode_joint.setAnchor(body.pos) self.ode_joints.append(ode_joint) # 時刻保存 self.prev_time: float = time.time()
def create_objects(self, world): print 'chassis' # chassis density = 10 lx, ly, lz = (8, 0.5, 8) # Create body body = ode.Body(world.world) mass = ode.Mass() mass.setBox(density, lx, ly, lz) body.setMass(mass) # Set parameters for drawing the body body.shape = "box" body.boxsize = (lx, ly, lz) # Create a box geom for collision detection geom = ode.GeomBox(world.space, lengths=body.boxsize) geom.setBody(body) #body.setPosition((0, 3, 0)) world.add_body(body) world.add_geom(geom) self._chassis_body = body density = 1 print 'left wheel' # left wheel radius = 1 height = 0.2 px, py, pz = (lx / 2, 0, -(lz / 2)) left_wheel_body = ode.Body(world.world) wheel_mass = ode.Mass() #wheel_mass.setSphere(density, radius) wheel_mass.setCylinder(density, 1, radius, height) left_wheel_body.setMass(wheel_mass) #left_wheel_geom = ode.GeomSphere(world.space, radius=radius) left_wheel_geom = ode.GeomCylinder(world.space, radius=radius, length=height) left_wheel_geom.setBody(left_wheel_body) #left_wheel_body.setPosition((px, py, pz)) left_wheel_body.setRotation((0, 0, 1, 0, 1, 0, -1, 0, 0)) left_wheel_body.setPosition((px - height / 2, py, pz)) world.add_body(left_wheel_body) world.add_geom(left_wheel_geom) print 'right wheel' # right wheel #radius = 1 px = -lx / 2 right_wheel_body = ode.Body(world.world) wheel_mass = ode.Mass() #wheel_mass.setSphere(density, radius) wheel_mass.setCylinder(density, 1, radius, height) right_wheel_body.setMass(wheel_mass) #right_wheel_geom = ode.GeomSphere(world.space, radius=radius) right_wheel_geom = ode.GeomCylinder(world.space, radius=radius, length=height) right_wheel_geom.setBody(right_wheel_body) #right_wheel_body.setPosition((px, py, pz)) right_wheel_body.setRotation((0, 0, 1, 0, 1, 0, -1, 0, 0)) right_wheel_body.setPosition((px - height / 2, py, pz)) world.add_body(right_wheel_body) world.add_geom(right_wheel_geom) print 'front wheel' # front wheel #radius = 1 px, py, pz = (0, 0, lz / 2) front_wheel_body = ode.Body(world.world) wheel_mass = ode.Mass() wheel_mass.setSphere(density, radius) front_wheel_body.setMass(wheel_mass) front_wheel_geom = ode.GeomSphere(world.space, radius=radius) front_wheel_geom.setBody(front_wheel_body) front_wheel_body.setPosition((px, py, pz)) world.add_body(front_wheel_body) world.add_geom(front_wheel_geom) #left_wheel_joint = ode.Hinge2Joint(world.world) left_wheel_joint = ode.HingeJoint(world.world) left_wheel_joint.attach(body, left_wheel_body) left_wheel_joint.setAnchor(left_wheel_body.getPosition()) left_wheel_joint.setAxis((-1, 0, 0)) #left_wheel_joint.setAxis1((0, 1, 0)) #left_wheel_joint.setAxis2((1, 0, 0)) left_wheel_joint.setParam(ode.ParamFMax, 500000) #left_wheel_joint.setParam(ode.ParamLoStop, 0) #left_wheel_joint.setParam(ode.ParamHiStop, 0) #left_wheel_joint.setParam(ode.ParamFMax2, 0.1) #left_wheel_joint.setParam(ode.ParamSuspensionERP, 0.2) #left_wheel_joint.setParam(ode.ParamSuspensionCFM, 0.1) self._left_wheel_joints.append(left_wheel_joint) #right_wheel_joint = ode.Hinge2Joint(world.world) right_wheel_joint = ode.HingeJoint(world.world) right_wheel_joint.attach(body, right_wheel_body) right_wheel_joint.setAnchor(right_wheel_body.getPosition()) right_wheel_joint.setAxis((-1, 0, 0)) #right_wheel_joint.setAxis1((0, 1, 0)) #right_wheel_joint.setAxis2((1, 0, 0)) right_wheel_joint.setParam(ode.ParamFMax, 500000) #right_wheel_joint.setParam(ode.ParamLoStop, 0) #right_wheel_joint.setParam(ode.ParamHiStop, 0) #right_wheel_joint.setParam(ode.ParamFMax2, 0.1) #right_wheel_joint.setParam(ode.ParamSuspensionERP, 0.2) #right_wheel_joint.setParam(ode.ParamSuspensionCFM, 0.1) self._right_wheel_joints.append(right_wheel_joint) front_wheel_joint = ode.BallJoint(world.world) front_wheel_joint.attach(body, front_wheel_body) front_wheel_joint.setAnchor(front_wheel_body.getPosition()) front_wheel_joint.setParam(ode.ParamFMax, 5000)
def _createODEjoint(self): # Create the ODE joint self.odejoint = ode.BallJoint(self.odedynamics.world)
def _createJoint(self, joint, parentT, posture): P = mm.TransVToSE3(joint.offset) T = numpy.dot(parentT, P) # R = mm.SO3ToSE3(posture.localRMap[joint.name]) R = mm.SO3ToSE3(posture.localRs[posture.skeleton.getElementIndex( joint.name)]) T = numpy.dot(T, R) temp_joint = joint nodeExistParentJoint = None while True: if temp_joint.parent == None: nodeExistParentJoint = None break elif temp_joint.parent.name in self.nodes: nodeExistParentJoint = temp_joint.parent break else: temp_joint = temp_joint.parent # if joint.parent and len(joint.children)>0: if nodeExistParentJoint and len(joint.children) > 0: if joint.name in self.config.nodes: p = mm.SE3ToTransV(T) node = self.nodes[joint.name] cfgNode = self.config.getNode(joint.name) if cfgNode.jointAxes != None: if len(cfgNode.jointAxes) == 0: node.joint = ode.BallJoint(self.world) node.motor = ode.AMotor(self.world) # node.joint.attach(self.nodes[joint.parent.name].body, node.body) node.joint.attach( self.nodes[nodeExistParentJoint.name].body, node.body) node.joint.setAnchor(p) # node.motor.attach(self.nodes[joint.parent.name].body, node.body) node.motor.attach( self.nodes[nodeExistParentJoint.name].body, node.body) node.motor.setNumAxes(3) node.motor.setAxis(0, 0, (1, 0, 0)) node.motor.setAxis(1, 0, (0, 1, 0)) node.motor.setAxis(2, 0, (0, 0, 1)) node.motor.setParam(ode.ParamLoStop, cfgNode.jointLoStop) node.motor.setParam(ode.ParamHiStop, cfgNode.jointHiStop) node.motor.setParam(ode.ParamLoStop2, cfgNode.jointLoStop) node.motor.setParam(ode.ParamHiStop2, cfgNode.jointHiStop) node.motor.setParam(ode.ParamLoStop3, cfgNode.jointLoStop) node.motor.setParam(ode.ParamHiStop3, cfgNode.jointHiStop) elif len(cfgNode.jointAxes) == 1: node.joint = ode.HingeJoint(self.world) # node.joint.attach(self.nodes[joint.parent.name].body, node.body) node.joint.attach( self.nodes[nodeExistParentJoint.name].body, node.body) node.joint.setAnchor(p) newR = mm.SE3ToSO3(self.newTs[joint.name]) node.joint.setAxis( numpy.dot(newR, cfgNode.jointAxes[0])) node.joint.setParam(ode.ParamLoStop, cfgNode.jointLoStop) node.joint.setParam(ode.ParamHiStop, cfgNode.jointHiStop) elif len(cfgNode.jointAxes) == 2: pass node.joint = ode.UniversalJoint(self.world) # # node.joint.attach(self.nodes[joint.parent.name].body, node.body) node.joint.attach( self.nodes[nodeExistParentJoint.name].body, node.body) node.joint.setAnchor(p) newR = mm.SE3ToSO3(self.newTs[joint.name]) node.joint.setAxis1( numpy.dot(newR, cfgNode.jointAxes[0])) node.joint.setAxis2( numpy.dot(newR, cfgNode.jointAxes[1])) node.joint.setParam(ode.ParamLoStop, cfgNode.jointLoStop) node.joint.setParam(ode.ParamHiStop, cfgNode.jointHiStop) node.joint.setParam(ode.ParamLoStop2, cfgNode.jointLoStop) node.joint.setParam(ode.ParamHiStop2, cfgNode.jointHiStop) else: node.joint = ode.FixedJoint(self.world) # node.joint.attach(self.nodes[joint.parent.name].body, node.body) node.joint.attach( self.nodes[nodeExistParentJoint.name].body, node.body) # node.joint.setFixed() node.Kp = cfgNode.Kp node.Kd = cfgNode.Kd else: pass for childJoint in joint.children: self._createJoint(childJoint, T, posture)
def test_ode(): M_PI = 3.14159265358979323846 M_TWO_PI = 6.28318530717958647693 #// = 2 * pi M_PI_SQRT2 = 2.22144146907918312351 # // = pi / sqrt(2) M_PI_SQR = 9.86960440108935861883 #// = pi^2 M_RADIAN = 0.0174532925199432957692 #// = pi / 180 M_DEGREE = 57.2957795130823208768 # // = pi / 180 ## vpWorld world; # world = ode.World() # space = ode.Space() odeWorld = yow.OdeWorld() world = odeWorld.world space = odeWorld.space ## vpBody ground, pendulum; ## vpBody base, body1, body2; base = ode.Body(world) body1 = ode.Body(world) body2 = ode.Body(world) ## vpBJoint J1; ## vpBJoint J2; J1 = ode.BallJoint(world) J2 = ode.BallJoint(world) M1 = ode.AMotor(world) M2 = ode.AMotor(world) M1.setNumAxes(2) M2.setNumAxes(2) ## Vec3 axis1(1,1,1); ## Vec3 axis2(1,0,0); axis1 = numpy.array([1, 1, 0]) axis2 = numpy.array([1, 1, 1]) axisX = numpy.array([1, 0, 0]) axisY = numpy.array([0, 1, 0]) axisZ = numpy.array([0, 0, 1]) ## scalar timeStep = .01; ## int deg1 = 0; ## int deg2 = 0; timeStep = .01 deg1 = [0.] deg2 = 0. odeWorld.timeStep = timeStep ## ground.AddGeometry(new vpBox(Vec3(10, 10, 0))); ## ground.SetFrame(Vec3(0,0,-.5)); ## ground.SetGround(); # // bodies ## base.AddGeometry(new vpBox(Vec3(3, 3, .5))); ## base.SetGround(); geom_base = ode.GeomBox(space, (3, .5, 3)) geom_base.setBody(base) geom_base.setPosition((0, .5, 0)) # geom_base.setRotation(mm.SO3ToOdeSO3(mm.exp((1,0,0),math.pi/2))) fj = ode.FixedJoint(world) fj.attach(base, ode.environment) fj.setFixed() mass_base = ode.Mass() mass_base.setBox(100, 3, .5, 3) base.setMass(mass_base) ## body1.AddGeometry(new vpCapsule(0.2, 4), Vec3(0, 0, 2)); ## body2.AddGeometry(new vpCapsule(0.2, 4), Vec3(0, 0, 2)); ## body1.SetCollidable(false); ## body2.SetCollidable(false); # geom_body1 = ode.GeomCapsule(space, .2, 4) geom_body1 = ode.GeomBox(space, (.4, 4, .4)) geom_body1.setBody(body1) geom_body1.setPosition((0, .5 + .25 + 2, 0)) mass_body1 = ode.Mass() mass_body1.setBox(100, .4, 4, .4) body1.setMass(mass_body1) # init_ori = mm.exp((1,0,0),math.pi/2) # geom_body1.setRotation(mm.SO3ToOdeSO3(init_ori)) # geom_body2 = ode.GeomCapsule(space, .2, 4) geom_body2 = ode.GeomBox(space, (.4, 4, .4)) geom_body2.setBody(body2) geom_body2.setPosition((0, .5 + .25 + 2 + 4.2, 0)) # geom_body2.setRotation(mm.SO3ToOdeSO3(mm.exp((1,0,0),math.pi/2))) mass_body2 = ode.Mass() mass_body2.setBox(100, .4, 4, .4) body2.setMass(mass_body2) ## axis1.Normalize(); ## axis2.Normalize(); axis1 = mm.normalize(axis1) axis2 = mm.normalize(axis2) ## //J1.SetAxis(axis1); ## base.SetJoint(&J1, Vec3(0, 0, .1)); ## body1.SetJoint(&J1, Vec3(0, 0, -.1)); J1.attach(base, body1) J1.setAnchor((0, .5 + .25, 0)) M1.attach(base, body1) ## //J2.SetAxis(axis2); ## //body1.SetJoint(&J2, Vec3(0, 0, 4.1)); ## //body2.SetJoint(&J2, Vec3(0, 0, -.1)); J2.attach(body1, body2) J2.setAnchor((0, .5 + .25 + 4.1, 0)) M2.attach(body1, body2) ## world.AddBody(&ground); ## world.AddBody(&base); ## world.SetGravity(Vec3(0.0, 0.0, -10.0)); world.setGravity((0, 0, 0)) def PDControl(frame): # global deg1[0], J1, J2, M1, M2 # scalar Kp = 1000.; # scalar Kd = 100.; Kp = 100. Kd = 2. # SE3 desiredOri1 = Exp(Axis(axis1), scalar(deg1[0] * M_RADIAN)); # SE3 desiredOri2 = Exp(Axis(axis2), scalar(deg2 * M_RADIAN)); desiredOri1 = mm.exp(axis1, deg1[0] * M_RADIAN) # desiredOri2 = mm.exp(axis2, deg2 * M_RADIAN) # se3 log1= Log(J1.GetOrientation() % desiredOri1); # se3 log2= Log(J2.GetOrientation() % desiredOri2); parent1 = J1.getBody(0) child1 = J1.getBody(1) parent1_desired_SO3 = mm.exp((0, 0, 0), 0) child1_desired_SO3 = desiredOri1 # child1_desired_SO3 = parent1_desired_SO3 parent1_body_SO3 = mm.odeSO3ToSO3(parent1.getRotation()) child1_body_SO3 = mm.odeSO3ToSO3(child1.getRotation()) # init_ori = (mm.exp((1,0,0),math.pi/2)) # child1_body_SO3 = numpy.dot(mm.odeSO3ToSO3(child1.getRotation()), init_ori.transpose()) align_SO3 = numpy.dot(parent1_body_SO3, parent1_desired_SO3.transpose()) child1_desired_SO3 = numpy.dot(align_SO3, child1_desired_SO3) diff_rot = mm.logSO3( numpy.dot(child1_desired_SO3, child1_body_SO3.transpose())) # print diff_rot parent_angleRate = parent1.getAngularVel() child_angleRate = child1.getAngularVel() # print child_angleRate angleRate = numpy.array([ -parent_angleRate[0] + child_angleRate[0], -parent_angleRate[1] + child_angleRate[1], -parent_angleRate[2] + child_angleRate[2] ]) # torque1 = Kp*diff_rot - Kd*angleRate # print torque1 # J1_ori = # log1 = mm.logSO3_tuple(numpy.dot(desiredOri1 ,J1.GetOrientation().transpose())) # log2 = mm.logSO3_tuple(numpy.dot(desiredOri1 ,J1.GetOrientation().transpose())) # Vec3 torque1 = Kp*(Vec3(log1[0],log1[1],log1[2])) - Kd*J1.GetVelocity(); # Vec3 torque2 = Kp*(Vec3(log2[0],log2[1],log2[2])) - Kd*J2.GetVelocity(); M1.setAxis(0, 0, diff_rot) M1.setAxis(1, 0, angleRate) # M2.setAxis(0,0,torque1) ## J1.SetTorque(torque1); ## J2.SetTorque(torque2); M1.addTorques(-Kp * mm.length(diff_rot), 0, 0) M1.addTorques(0, Kd * mm.length(angleRate), 0) # print diff_rot # print angleRate # M2.addTorques(torque2, 0, 0) # cout << "SimulationTime " << world.GetSimulationTime() << endl; # cout << "deg1[0] " << deg1[0] << endl; # cout << "J1.vel " << J1.GetVelocity(); # cout << "J1.torq " << torque1; # cout << endl; def calcPDTorque(Rpd, Rcd, Rpc, Rcc, Wpc, Wcc, Kp, Kd, joint): Rpc = mm.odeSO3ToSO3(Rpc) Rcc = mm.odeSO3ToSO3(Rcc) Ra = numpy.dot(Rpc, Rpd.transpose()) Rcd = numpy.dot(Ra, Rcd) dR = mm.logSO3(numpy.dot(Rcd, Rcc.transpose())) dW = numpy.array( [-Wpc[0] + Wcc[0], -Wpc[1] + Wcc[1], -Wpc[2] + Wcc[2]]) # joint.setAxis(0,0,dR) # joint.setAxis(1,0,dW) joint.setAxis(0, 0, dR - dW) # joint.addTorques(-Kp*mm.length(dR),0,0) # joint.addTorques(0,Kd*mm.length(dW),0) joint.addTorques(-Kp * mm.length(dR) - Kd * mm.length(dW), 0, 0) def PDControl3(frame): # Kp = 100.*70; # Kd = 10.*3; Kp = 1000. Kd = 10. desiredOri1 = mm.exp(axis1, deg1[0] * M_RADIAN) desiredOriX = mm.exp(axisX, deg1[0] * M_RADIAN) desiredOriY = mm.exp(axisY, deg1[0] * M_RADIAN) desiredOriZ = mm.exp(axisZ, deg1[0] * M_RADIAN) calcPDTorque(mm.I_SO3(), mm.exp(axisX, -90 * M_RADIAN), base.getRotation(), body1.getRotation(), base.getAngularVel(), body1.getAngularVel(), Kp, Kd, M1) # calcPDTorque(mm.I_SO3, desiredOriY, base.getRotation(), body1.getRotation(), base.getAngularVel(), body1.getAngularVel(), Kp, Kd, M1); # calcPDTorque(mm.I_SO3, desiredOriX, body1.getRotation(), body2.getRotation(), body1.getAngularVel(), body2.getAngularVel(), Kp, Kd, M2); # ground = ode.GeomPlane(space, (0,1,0), 0) viewer = ysv.SimpleViewer() viewer.setMaxFrame(100) # viewer.record(False) viewer.doc.addRenderer('object', yr.OdeRenderer(space, (255, 255, 255))) def preFrameCallback(frame): # global deg1[0] print 'deg1[0]', deg1[0] deg1[0] += 1 # PDControl(frame) PDControl3(frame) viewer.setPreFrameCallback(preFrameCallback) def simulateCallback(frame): odeWorld.step() viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
# Create two bodies body1 = ode.Body(world) M = ode.Mass() M.setSphere(2500, 0.05) body1.setMass(M) body1.setPosition((1, 2, 0)) body2 = ode.Body(world) M = ode.Mass() M.setSphere(2500, 0.05) body2.setMass(M) body2.setPosition((2, 2, 0)) # Connect body1 with the static environment j1 = ode.BallJoint(world) j1.attach(body1, ode.environment) j1.setAnchor((0, 2, 0)) # Connect body2 with body1 j2 = ode.BallJoint(world) j2.attach(body1, body2) j2.setAnchor((1, 2, 0)) # Simulation loop... fps = 50 dt = 1.0 / fps loopFlag = True clk = pygame.time.Clock()
def loaddata(self, vpoints): self.world = ode.World() self.world.setGravity((0, -9.81, 0)) #vpoints = parse_vpoints(mechanism) self.vlinks = {} for i, vpoint in enumerate(vpoints): for link in vpoint.links: if link in self.vlinks: self.vlinks[link].add(i) else: self.vlinks[link] = {i} print(self.vlinks) self.bodies = [] for i, vpoint in enumerate(vpoints): body = ode.Body(self.world) M = ode.Mass() M.setSphere(250, 0.05) body.setMass(M) x, y = vpoint body.setPosition((x, y, 0)) self.bodies.append(body) self.bodies[0].setGravityMode(False) self.joints = [] for name, vlink in self.vlinks.items(): link = list(vlink) #print(link) if name == 'ground': for p in link: if p in inputs: #print("input:", p) j = ode.HingeJoint(self.world) #j.attach(bodies[(vlinks[inputs[p][1]] - {p}).pop()], ode.environment) j.attach(self.bodies[1], ode.environment) j.setAxis((0, 0, 1)) j.setAnchor(self.bodies[0].getPosition()) j.setParam(ode.ParamVel, 2) j.setParam(ode.ParamFMax, 22000) else: #print("grounded:", p) j = ode.BallJoint(self.world) j.attach(self.bodies[p], ode.environment) j.setAnchor(self.bodies[p].getPosition()) self.joints.append(j) elif len(link) >= 2: #print("link:", link[0], link[1]) j = ode.BallJoint(self.world) j.attach(self.bodies[link[0]], self.bodies[link[1]]) j.setAnchor(self.bodies[link[0]].getPosition()) self.joints.append(j) # TODO : need to add select method in this joint type for p in link[2:]: #print("other:", p, link[0], link[1]) for k in range(2): j = ode.BallJoint(self.world) j.attach(self.bodies[p], self.bodies[link[k]]) self.joints.append(j)