def __init__(self, world, space, node1, node2, radius=0.1, mass=1.0, rest_length=1, elasticity=0, color=(100., 0., 100.), fixed=False): self.nodes = [node1, node2] self.radius = radius self.mass = mass self.rest_length = rest_length self.elasticity = elasticity self.color = color self.joints = [] is_strut = elasticity == 0 self.body = ode.Body(world) positions = [node.get_position() for node in self.nodes] center = [(positions[0][i] + positions[1][i]) / 2 for i in range(len(positions[0]))] lengths = [(positions[1][i] - positions[0][i]) for i in range(len(positions[0]))] length = math.sqrt(dot(lengths, lengths)) self.update() print("lengths=", lengths) print("center=", center) if is_strut: geom_length = 0.9 * (length - (node1.radius + node2.radius)) self.geom = ode.GeomBox(space, lengths=[radius, geom_length, radius]) self.geom.setBody(self.body) for node in self.nodes: joint = ode.FixedJoint(world) joint.attach(self.body, node.body) joint.setFixed() self.joints.append(joint) if fixed: joint = ode.FixedJoint(world) joint.attach(self.body, ode.environment) joint.setFixed() self.joints.append(joint)
def reset(self): # Object self.obj.setPosition(0, 0, 0) # Wall for i in range(nWall): self.wall[i] = Box(self.space, self.world, (WALL_LENGTH[i], WALL_THICK, WALL_TALL), None, [0.3, 0.7, 0.1]) self.wall[i].setPosition(WALL_POS[i, 0], WALL_POS[i, 1], WALL_TALL / 2) if WALL_DIR[i] == 1: Q = axisangle_to_quat(np.array([0, 0, 1]), np.pi / 2) self.wall[i].setQuat(*Q) # Robots for i in range(4): self.robot[i].setPosition(0.33 - i * 0.22, 0.13, ROBOT_RADIUS) for i in range(4, 8): self.robot[i].setPosition(0.33 - (i - 4) * 0.22, -0.13, ROBOT_RADIUS) for i in range(8, 10): self.robot[i].setPosition(0.33, 0.047 - (i - 8) * 0.087, ROBOT_RADIUS) for i in range(10, 12): self.robot[i].setPosition(-0.33, 0.047 - (i - 10) * 0.087, ROBOT_RADIUS) for i in range(self.n_robots): self.joint[i] = ode.FixedJoint(self.world) self.joint[i].attach(self.obj.body, self.robot[i].body) self.joint[i].setFixed() # Enemy bot for ebot in self.enemy_bot: loc = (self.np_random.rand(), self.np_random.rand()) ebot.setPosition(loc[0], loc[1], ROBOT_RADIUS)
def __init__(self, gravity=9.8, mass=1.0, tau=0.02, size_pole=(1.0, .1)): self.gravity = gravity self.mass = mass self.dt = tau self.viewer = None self.viewerSize = 500 self.spaceSize = 7. self.size_pole = size_pole self.max_force = 3. self.theta_threshold = 3.2 self.action_space = spaces.Box(low=-self.max_force, high=self.max_force, shape=(1, )) high = np.array([self.theta_threshold * 2, np.finfo(np.float32).max]) self.observation_space = spaces.Box(-high, high) self.world = ode.World() self.world.setGravity((0, -9.81, 0)) self.body1 = ode.Body(self.world) self.body2 = ode.Body(self.world) self.create_basebox(self.body1, (0., 0., 0.)) self.create_link(self.body2, (0., 0.5, 0.), self.mass, size_pole) self.space = ode.Space() self.j1 = ode.FixedJoint(self.world) self.j1.attach(self.body1, ode.environment) self.j1.setFixed() self.j2 = ode.HingeJoint(self.world) self.j2.attach(self.body1, self.body2) self.j2.setAnchor((0., 0., 0.)) self.j2.setAxis((0, 0, -1)) self.j2.setFeedback(1)
def create_tensegrity(): global nodes, links, node_bodies nodes, links = load_spr() for node in nodes: endcap, geom = create_sphere(world, space, 10, .2) endcap.setPosition([val / 50000.0 for val in node.position]) node_bodies.append(endcap) bodies.append(endcap) print("link length=", len(links)) for link in links: is_strut = link.elasticity > 200 if link.elasticity > 100: end_points = [nodes[node_id].position for node_id in link.nodes] dx = end_points[0][0] - end_points[1][0] dy = end_points[0][1] - end_points[1][1] dz = end_points[0][2] - end_points[1][2] length = sqrt(dx * dx + dy * dy + dz * dz) if is_strut: bar, geom = create_box(world, space, 1, .2, length / 50000.0, .2) else: bar, geom = create_box(world, space, 1, .05, length / 50000.0, .05) center = [(end_points[0][i] + end_points[1][i]) / 100000.0 for i in range(3)] bar.setPosition(center) c1 = sqrt(dx * dx + dy * dy); s1 = dz; c2 = dx / c1 if c1 != 0 else 1.0 s2 = dy / c1 if c1 != 0 else 0.0 quat = rotation_quat([0, 1, 0], normalize([dx, dy, dz])) bar.setQuaternion(quat) bodies.append(bar) link_bodies.append(bar) if is_strut: joint = ode.FixedJoint(world) joint.attach(bar, node_bodies[link.nodes[0]]) joint.setFixed() joints.append(joint) joint = ode.FixedJoint(world) joint.attach(bar, node_bodies[link.nodes[1]]) joint.setFixed() joints.append(joint) geoms.append(geom)
def create_fixed_joint(sim, body1, body2=ode.environment): """ Create a fixed joint between two bodies. """ fixed = ode.FixedJoint(sim.world) fixed.attach(body1, body2) fixed.setFixed() sim.fixed.append(fixed)
def addFixedJoint(self, body1, body2): joint = ode.FixedJoint(self.sim.world) joint.attach(body1, body2) joint.setFixed() joint.style = "fixed" self.joints.append(joint) return joint
def __init__(self): self.dt=.005 self.viewer = None self.viewerSize = 500 self.spaceSize = 6.4 self.resolution = self.viewerSize/self.spaceSize self.init_rod_template() self.seed() self.world = ode.World() #self.world.setGravity((0,-9.81,0)) self.world.setGravity((0,0,0)) self.body1 = ode.Body(self.world) self.body2 = ode.Body(self.world) self.body3 = ode.Body(self.world) self.body4 = ode.Body(self.world) self.create_link(self.body1,(0.5,0,0)) self.create_link(self.body2,(1.5,0,0)) self.create_link(self.body3,(2.5,0,0)) self.space = ode.Space() self.body4_col = ode.GeomSphere(self.space,radius=0.1) self.create_ee(self.body4,(3,0,0),self.body4_col) # Connect body1 with the static environment self.j1 = ode.HingeJoint(self.world) self.j1.attach(self.body1, ode.environment) self.j1.setAnchor( (0,0,0) ) self.j1.setAxis( (0,0,1) ) self.j1.setFeedback(1) # Connect body2 with body1 self.j2 = ode.HingeJoint(self.world) self.j2.attach(self.body1, self.body2) self.j2.setAnchor( (1,0,0) ) self.j2.setAxis( (0,0,-1) ) self.j2.setFeedback(1) #connect body3 with body2 self.j3 = ode.HingeJoint(self.world) self.j3.attach(self.body2, self.body3) self.j3.setAnchor( (2,0,0) ) self.j3.setAxis( (0,0,-1) ) self.j3.setFeedback(1) #connect end effector self.j4 = ode.FixedJoint(self.world) self.j4.attach(self.body3,self.body4) self.j4.setFixed() self.controlMode = "POS" self.targetPos = self.rand_target() self.targetTime = 0 self.P_gains = np.array([1000,1000,1000]) self.D_gains = np.array([70,50,20])
def _parseFixedJoint(self, world, link1, link2): def start(name, attrs): raise errors.ChildError('fixed', name) def end(name): if (name == 'fixed'): self._parser.pop() joint = ode.FixedJoint(world, self._jg) joint.attach(link1, link2) self.setODEObject(joint) self._parser.push(startElement=start, endElement=end)
def construct(self): self.body = ode.Body(objects.world) x, y, _ = self.helper.getPosition() self.body.setPosition((x, y, 0)) M = ode.Mass() M.setSphereTotal(self.helper.strong_mass, self.helper.rad) self.body.setMass(M) objects.ODEThing.construct(self) self.body.setAngularVel(self.helper.getAngularVel()) self.body.setLinearVel(self.helper.body.getLinearVel()) self.fixed_joint = ode.FixedJoint(objects.world, self.jointgroup) self.fixed_joint.attach(self.body, self.helper.body) self.fixed_joint.setFixed()
def build_world(): # Create the world through ode density = 1 start_up = True world = ode.World() world.setGravity((0, -gravity, 0)) # Make the cart cart = ode.Body(world) M = ode.Mass() M.setBox(density, cart_width, cart_height, cart_height) M.mass = cart_mass cart.setMass(M) cart.setPosition((0, 0, 0)) print 'cart mass = ', cart.getMass().mass # Make a heavy ball ball = ode.Body(world) M = ode.Mass() M.setSphere(density, 0.5) M.mass = ball_mass ball.setMass(M) ball.setPosition((0, 1, 0)) # And a rod with a negligible weight rod = ode.Body(world) M = ode.Mass() M.setCylinder(0.01, 2, 0.01, rod_length) M.mass = 1e-2 rod.setMass(M) rod.setPosition((0, 0.5, 0)) ## Connect the cart to the world through a slider joint cart_joint = ode.SliderJoint(world) cart_joint.setAxis((1, 0, 0)) cart_joint.attach(cart, ode.environment) # Connect the rod with the cart through a Hinge joint. pendulum_joint = ode.HingeJoint(world) pendulum_joint.attach(rod, cart) pendulum_joint.setAnchor((0, 0, 0)) pendulum_joint.setAxis((0, 0, 1)) # Connect rod with ball with a fixed joint rod_ball_joint = ode.FixedJoint(world) rod_ball_joint.attach(rod, ball) rod_ball_joint.setFixed() return world, cart, ball, rod, pendulum_joint, cart_joint, rod_ball_joint
def __init__(self, world, space, pos, color, capsules, radius, mass=2, fixed=False, orientation=v(1, 0, 0, 0)): "capsules is a list of (start, end) points" self.capsules = capsules self.body = ode.Body(world) self.body.setPosition(pos) self.body.setQuaternion(orientation) m = ode.Mass() # computing MOI assuming sphere with .5 m radius m.setSphere(mass / (4 / 3 * math.pi * .5**3), .5) # setSphereTotal is broken self.body.setMass(m) self.geoms = [] self.geoms2 = [] for start, end in capsules: self.geoms.append(ode.GeomTransform(space)) x = ode.GeomCapsule(None, radius, (end - start).mag()) self.geoms2.append(x) self.geoms[-1].setGeom(x) self.geoms[-1].setBody(self.body) x.setPosition((start + end) / 2 + v(random.gauss( 0, .01), random.gauss(0, .01), random.gauss(0, .01))) a = (end - start).unit() b = v(0, 0, 1) x.setQuaternion( sim_math_helpers.axisangle_to_quat((a % b).unit(), -math.acos(a * b))) self.color = color self.radius = radius if fixed: self.joint = ode.FixedJoint(world) self.joint.attach(self.body, None) self.joint.setFixed()
def create_fixed(self, key, pos, bod): """ Create a fixed joint with the world. Arguments: key : number id to assign the hinge pos : position of the joint bod : body to attach the world to """ # Auto label the joint key or not. key = len(self.joints) if key == -1 else key j = ode.FixedJoint(self.world) j.attach(self.bodies[bod], ode.environment) j.setFixed() j.style = "fixed" self.joints[key] = j return key
def do_joint_motor(self, jointtype, record=0): b = TestBodyPartGraph(new_network_args, 2, jointtype) b.bodyparts[0].rotation = (0, (1, 0, 0)) b.bodyparts[1].rotation = (math.pi / 2, (0, 1, 0)) for i in 0, 1: b.bodyparts[i].motor_input = None s = sim.BpgSim(SECONDS) s.relaxed = 1 s.add(b) s.bpgs[0].bodyparts[0].motor_input = None s.bpgs[0].bodyparts[1].motor_input = None s.world.setGravity((0, 0, 0)) s.space.getGeom(0).getBody() b1 = s.space.getGeom(1).getBody() s.moveBps(0, 0, 5) # fix bp so it cant move j = ode.FixedJoint(s.world) j.attach(b1, ode.environment) j.setFixed() # start motor m = s.bpgs[0].bodyparts[1].motor motor_data = m.log('test/' + jointtype) m.dangle[0] = math.pi / 2 m.dangle[1] = math.pi / 4 m.dangle[2] = -math.pi / 2 s.initSignalLog('test/signal.log') if not record: runSim(s) else: runSim(s, 1, '%s/record_test.avi' % TESTDIR) m.endlog() t = Template(file='plot_motor.t') t.data = motor_data t.out = motor_data[:-4] + '.pdf' t.joint = jointtype f = open('tmp.r', 'w') f.write(t.respond()) f.close() os.system('R -q --no-save < tmp.r >> r.out 2>&1')
def __init__(self, world, position=(0.0, 0.0, 0.0)): #stores the initial position self.initPos = position color = getColor() #inits the cube with player position, mass, size and color objects.Cube.__init__(self, world, position, 50, (2.0, 2.0, 2.0), getRGBValues(color)) self.geom.name = self.setName(color) #inits the trigger with according values #trigger box is around player cube self.trigger = TriggerBox(world, position, (2.2, 2.2, 2.2), self.geom.name) #joins the player body with the trigger self.j = ode.FixedJoint(world) self.j.attach(self.body, self.trigger.body) self.j.setFixed() self.min = [0, 0] self.max = [0, 0] #gets the largest and smallest coordinates of the players square a = 10.0 * position[0] / abs(position[0]) b = 10.0 * position[2] / abs(position[2]) #stores the player`s square min and max coordinates if a > 0: self.max[0] = a else: self.min[0] = a if b > 0: self.max[1] = b else: self.min[1] = b self.camera = None self.setAngle()
def __init__(self, world, space, name="", position=(0., 0., 0.), velocity=(0., 0., 0.), radius=0.5, mass=1.0, color=(100., 0., 100.), fixed=False): radius *= 2 self.name = name self.radius = radius self.mass = mass self.color = color self.joints = [] self.body = ode.Body(world) M = ode.Mass() M.setSphereTotal(mass, radius) self.body.name = name self.body.shape = "sphere" self.body.radius = radius self.body.setMass(M) self.body.setPosition(position) self.body.setLinearVel(velocity) #self.geom = ode.GeomSphere(space, radius=self.body.radius) #self.geom = ode.GeomBox(space, lengths=(1, 1, 1)) #self.geom.setBody(self.body) if fixed: joint = ode.FixedJoint(world) joint.attach(self.body, ode.environment) joint.setFixed() self.joints.append(joint)
def _createODEjoint(self): # Create the ODE joint self.odejoint = ode.FixedJoint(self.odedynamics.world)
def _loadObjects(self): """ Spawns the Differential Drive robot based on the input pose. """ # Create a Box box_pos = [ self.basepos[0], self.basepos[1] + self.cubesize, self.basepos[2] ] rot = self.baserot boxbody = ode.Body(self.world) boxgeom = ode.GeomBox(space=self.space, lengths=(self.cubesize, self.cubesize * 0.75, self.cubesize)) boxgeom.setBody(boxbody) boxgeom.setPosition(box_pos) boxgeom.setRotation(rot) boxM = ode.Mass() boxM.setBox(self.cubemass, self.cubesize, self.cubesize * 0.75, self.cubesize) boxbody.setMass(boxM) # Create two Cylindrical Wheels rot = self.multmatrix(self.genmatrix(math.pi / 2, 2), rot) leftwheel_pos = [ box_pos[0] + 0.5 * self.track, box_pos[1] - self.wheelradius * 0.5, box_pos[2] + self.cubesize * 0.2 ] rightwheel_pos = [ box_pos[0] - 0.5 * self.track, box_pos[1] - self.wheelradius * 0.5, box_pos[2] + self.cubesize * 0.2 ] leftwheelbody = ode.Body(self.world) leftwheelgeom = ode.GeomCylinder(space=self.space, radius=self.wheelradius, length=self.wheelradius / 3.0) leftwheelgeom.setBody(leftwheelbody) leftwheelgeom.setPosition(leftwheel_pos) leftwheelgeom.setRotation(rot) leftwheelM = ode.Mass() leftwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius, self.wheelradius / 3.0) leftwheelbody.setMass(leftwheelM) lefthinge = ode.HingeJoint(self.world) lefthinge.attach(leftwheelbody, boxbody) lefthinge.setAnchor(leftwheel_pos) lefthinge.setAxis(self.rotate((0, 0, 1), rot)) lefthinge.setParam(ode.ParamFMax, self.hingemaxforce) rightwheelbody = ode.Body(self.world) rightwheelgeom = ode.GeomCylinder(space=self.space, radius=self.wheelradius, length=self.wheelradius) rightwheelgeom.setBody(rightwheelbody) rightwheelgeom.setPosition(rightwheel_pos) rightwheelgeom.setRotation(rot) rightwheelM = ode.Mass() rightwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius, self.cubemass / 3.0) rightwheelbody.setMass(rightwheelM) righthinge = ode.HingeJoint(self.world) righthinge.attach(rightwheelbody, boxbody) righthinge.setAnchor(rightwheel_pos) righthinge.setAxis(self.rotate((0, 0, 1), rot)) righthinge.setParam(ode.ParamFMax, self.hingemaxforce) # Create a spherical wheel at the back for support caster_pos = [ box_pos[0], box_pos[1] - self.cubesize * 0.5, box_pos[2] - self.cubesize * 0.5 ] casterbody = ode.Body(self.world) castergeom = ode.GeomSphere(space=self.space, radius=self.cubesize / 5.0) castergeom.setBody(casterbody) castergeom.setPosition(caster_pos) castergeom.setRotation(rot) casterM = ode.Mass() casterM.setSphere(self.cubemass * 100.0, self.cubesize / 5.0) casterbody.setMass(casterM) self.fixed = ode.FixedJoint(self.world) self.fixed.attach(casterbody, boxbody) self.fixed.setFixed() # WHEW, THE END OF ALL THAT FINALLY! # Build the Geoms and Joints arrays for rendering. self.boxgeom = boxgeom self._geoms = [ boxgeom, leftwheelgeom, rightwheelgeom, castergeom, self.ground ] self.lefthinge = lefthinge self.righthinge = righthinge self._joints = [self.lefthinge, self.righthinge, self.fixed] def loadObstacles(self, obstaclefile): """ Loads obstacles from the obstacle text file. """ # Initiate data structures. data = open(obstaclefile, "r") obs_sizes = [] obs_positions = [] obs_masses = [] reading = "None" # Parse the obstacle text file. for line in data: linesplit = line.split() if linesplit != []: if linesplit[0] != "#": obs_sizes.append([ float(linesplit[0]), float(linesplit[1]), float(linesplit[2]) ]) obs_positions.append([ float(linesplit[3]), float(linesplit[4]), float(linesplit[5]) ]) obs_masses.append(float(linesplit[6])) # Go through all the obstacles in the list and spawn them. for i in range(len(obs_sizes)): obs_size = obs_sizes[i] obs_pos = obs_positions[i] obs_mass = obs_masses[i] # Create the obstacle. body = ode.Body(self.world) geom = ode.GeomBox(space=self.space, lengths=obs_size) geom.setBody(body) geom.setPosition(obs_pos) M = ode.Mass() M.setBox(obs_mass, obs_size[0], obs_size[1], obs_size[2]) body.setMass(M) # Append all these new pointers to the simulator class. self._geoms.append(geom)
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()
def fixRootBody(self): self.fixedJoint = ode.FixedJoint(self.world) self.fixedJoint.attach(self.rootNode.body, ode.environment) self.fixedJoint.setFixed()
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 SetFixed(self): """Fix this body to the ode world using a FixedJoint.""" self._fixedJoint = ode.FixedJoint(self._world) self._fixedJoint.attach(self, ode.environment) self._fixedJoint.setFixed()
def __init__(self): self.dt = .005 self.viewer = None self.viewerSize = 800 self.spaceSize = 8.4 self.resolution = self.viewerSize / self.spaceSize self.perspective_transform_on = False self.init_rod_template() self._seed() self.world = ode.World() self.world.setGravity((0, 0, 0)) self.body1 = ode.Body(self.world) self.body2 = ode.Body(self.world) self.body3 = ode.Body(self.world) self.body4 = ode.Body(self.world) self.body5 = ode.Body(self.world) self.body6 = ode.Body(self.world) self.body7 = ode.Body(self.world) self.body8 = ode.Body(self.world) self.create_link(self.body1, (0.3, 0, 0)) self.create_link(self.body2, (.9, 0, 0)) self.create_link(self.body3, (1.5, 0, 0)) self.create_link(self.body4, (2.1, 0, 0)) self.create_link(self.body5, (2.7, 0, 0)) self.create_link(self.body6, (3.3, 0, 0)) self.create_link(self.body7, (3.9, 0, 0)) self.space = ode.Space() self.body8_col = ode.GeomSphere(self.space, radius=0.1) self.create_ee(self.body8, (4.2, 0, 0), self.body8_col) self.colours = [] self.dir = 1.01 # Connect body1 with the static environment self.j1 = ode.HingeJoint(self.world) self.j1.attach(self.body1, ode.environment) self.j1.setAnchor((0, 0, 0)) self.j1.setAxis((0, -1, 0)) self.j1.setFeedback(1) # Connect body2 with body1 self.j2 = ode.HingeJoint(self.world) self.j2.attach(self.body1, self.body2) self.j2.setAnchor((0.6, 0, 0)) self.j2.setAxis((0, 0, -1)) self.j2.setFeedback(1) # connect body3 with body2 self.j3 = ode.HingeJoint(self.world) self.j3.attach(self.body2, self.body3) self.j3.setAnchor((1.2, 0, 0)) self.j3.setAxis((0, 0, -1)) self.j3.setFeedback(1) # connect body4 with body3 self.j4 = ode.HingeJoint(self.world) self.j4.attach(self.body3, self.body4) self.j4.setAnchor((1.8, 0, 0)) # self.j4.setAxis( (0,0,-1) ) self.j4.setAxis((0, 1, 0)) self.j4.setFeedback(1) # connect body5 with body4 self.j5 = ode.HingeJoint(self.world) self.j5.attach(self.body4, self.body5) self.j5.setAnchor((2.4, 0, 0)) # self.j4.setAxis( (0,0,-1) ) self.j5.setAxis((0, 1, 0)) self.j5.setFeedback(1) # connect body6 with body5 self.j6 = ode.HingeJoint(self.world) self.j6.attach(self.body5, self.body6) self.j6.setAnchor((3.0, 0, 0)) # self.j4.setAxis( (0,0,-1) ) self.j6.setAxis((0, 0, -1)) self.j6.setFeedback(1) # connect body4 with body3 self.j7 = ode.HingeJoint(self.world) self.j7.attach(self.body6, self.body7) self.j7.setAnchor((3.6, 0, 0)) # self.j4.setAxis( (0,0,-1) ) self.j7.setAxis((0, 1, 0)) self.j7.setFeedback(1) # connect end effector self.j8 = ode.FixedJoint(self.world) self.j8.attach(self.body7, self.body8) self.j8.setFixed() self.controlMode = "POS" self.targetPos = self.rand_target() self.targetTime = 0 self.targetTime2 = 0 self.success = 0 self.fail = 0 self.P_gains = np.array([1000, 1000, 1000, 1000, 1000, 1000, 1000]) self.D_gains = np.array([70, 50, 40, 20, 15, 10, 5])
def set_fixed(self): sim = self.sim joint = ode.FixedJoint(sim.world) joint.attach(self.body, None) joint.setFixed() self._fixed_joint = joint
def loadConfig(self, filename, reload=False): # parameters are given in (our own brand of) config-file syntax self.config = ConfigGrabber(filename, sectionId="<!--odeenvironment parameters", delim=("<", ">")) # <passpairs> self.passpairs = [] for passpairstring in self.config.getValue("passpairs")[:]: self.passpairs.append(eval(passpairstring)) if self.verbosity > 0: print("-------[pass tuples]--------") print((self.passpairs)) print("----------------------------") # <centerOn> # set focus of camera to the first object specified in the section, if any if self.render: try: self.centerOn(self.config.getValue("centerOn")[0]) except IndexError: pass # <affixToEnvironment> for jointName in self.config.getValue("affixToEnvironment")[:]: try: # find first object with that name obj = self.root.namedChild(jointName).getODEObject() except IndexError: print(("ERROR: Could not affix object '" + jointName + "' to environment!")) sys.exit(1) if isinstance(obj, ode.Joint): # if it is a joint, use this joint to fix to environment obj.attach(obj.getBody(0), ode.environment) elif isinstance(obj, ode.Body): # if it is a body, create new joint and fix body to environment j = ode.FixedJoint(self.world) j.attach(obj, ode.environment) j.setFixed() # <colors> for coldefstring in self.config.getValue("colors")[:]: # ('name', (0.3,0.4,0.5)) objname, coldef = eval(coldefstring) for (body, _) in self.body_geom: if hasattr(body, 'name'): if objname == body.name: body.color = coldef break if not reload: # add the JointSensor as default self.sensors = [] ## self.addSensor(self._jointSensor) # <sensors> # expects a list of strings, each of which is the executable command to create a sensor object # example: DistToPointSensor('legSensor', (0.0, 0.0, 5.0)) sens = self.config.getValue("sensors")[:] for s in sens: try: self.addSensor(eval('sensors.' + s)) except AttributeError: print((dir(sensors))) warnings.warn("Sensor name with name " + s + " not found. skipped.") else: for s in self.sensors: s._connect(self) for a in self.actuators: a._connect(self)
def __init__(self, **kwargs): self.gui = None self.boat = { 'x': 0.0, 'y': 0.0, 'speed': 0.0, 'direction': 0.0, 'tiler': 0.0 } self.odeBody = ob() self.odeWorld = ode.World() self.odeWorld.setGravity((0, -9.81, 0)) self.odeBoat = ode.Body(self.odeWorld) mas = ode.Mass() mas.setBox(10, 10, 10, 10) mas.mass = 1.0 self.odeBoat.setMass(mas) self.odeBoat.setPosition((self.boat['x'], 0, self.boat['y'])) self.odeR = ode.Body(self.odeWorld) M = ode.Mass() M.setBox(10, 10, 10, 10) M.mass = 1.0 self.odeR.setMass(M) self.odeR.setPosition((self.boat['x'] + 2, 0, self.boat['y'])) self.odeJR = ode.FixedJoint(self.odeWorld) self.odeJR.setFeedback(True) self.odeJR.attach(self.odeBoat, self.odeR) self.odeJR.setFixed() #j.setAnchor( (2.0,0,0) ) self.dirAvg = self.boat['direction'] self.heewCounter = 0.0 self.pitchCounter = 0.0 self.k = {'up': False, 'down': False, 'left': False, 'right': False} self.canvas = RenderContext(compute_normal_mat=True) self.canvas.shader.source = resource_find('simple.glsl') self.d3h = D3Helper() self.d3h.loadObj([ [ 'boat', "./3dModels/boat2_3dex_boat.obj", "./3dModels/oiysh_profile2.jpeg" ], ['roseta', './3dModels/3d_roseta.obj', ''], [ 'genoa', "./3dModels/boat2_3dex_sailGenoa_onPower.obj", "./3dModels/3d_texture_genoa.jpg" ], [ 'main', "./3dModels/boat2_3dex_sailMain_sailup_onPower.obj", "./3dModels/IMG_5410.jpg" ], ['boom', "./3dModels/boat2_3dex_boom.obj", ""], ['ruder', "./3dModels/boat2_3dex_ruder.obj", ""], ['tiler', "./3dModels/boat2_3dex_tiler.obj", ""], #['hdr1','./3dModels/hdr1.obj','./3dModels/hdr1.png'], ['xAxis', './3dModels/x-axis.obj', './3dModels/red.png'], ['yAxis', './3dModels/y-axis.obj', './3dModels/green.png'], ['zAxis', './3dModels/z-axis.obj', './3dModels/blue.png'], ['box', './3dModels/3d_box.obj', ''], [ 'waterTile', './3dModels/waterTile.obj', "./3dModels/IMG_5410.jpg" ], [ 'hdr2', './3dModels/hdr2.obj', "./3dModels/PANO_20160311_160253.jpg" ], ['kap2', './3dModels/tile.obj', "./3dModels/kap2.png"], ["infWat", "./3dModels/infinityWater.obj", ""] ]) super(d3tex2, self).__init__(**kwargs) with self.canvas: self.cb = Callback(self.setup_gl_context) PushMatrix() self.setup_scene() PopMatrix() self.cb = Callback(self.reset_gl_context) if d3tex2RunAlone: self.on_displayNow() Window.bind(on_key_down=self.on_key_down) Window.bind(on_key_up=self.on_key_up) Window.bind(on_touch_down=self.on_touchDown) Window.bind(on_touch_move=self.on_touchMove) Window.bind(on_touch_up=self.on_touchUp)
def makeFJ(self, o0, o1): print("makeFJ") odeJR = ode.FixedJoint(self.odeWorld) odeJR.setFeedback(True) odeJR.attach(o0, o1) return odeJR
def _loadObjects(self): """ Spawns the Differential Drive robot based on the input pose. """ # Create a Box box_pos = [self.basepos[0], self.basepos[1] + self.cubesize, self.basepos[2]] rot = self.baserot boxbody = ode.Body(self.world) boxgeom = ode.GeomBox(space=self.space, lengths=(self.cubesize, self.cubesize*0.75, self.cubesize) ) boxgeom.setBody(boxbody) boxgeom.setPosition(box_pos) boxgeom.setRotation(rot) boxM = ode.Mass() boxM.setBox(self.cubemass,self.cubesize,self.cubesize*0.75,self.cubesize) boxbody.setMass(boxM) # Create two Cylindrical Wheels rot = self.multmatrix(self.genmatrix(math.pi/2,2),rot) leftwheel_pos = [box_pos[0] + 0.5*self.track, box_pos[1] - self.wheelradius*0.5, box_pos[2] + self.cubesize*0.2] rightwheel_pos = [box_pos[0] - 0.5*self.track, box_pos[1] - self.wheelradius*0.5, box_pos[2] + self.cubesize*0.2] leftwheelbody = ode.Body(self.world) leftwheelgeom = ode.GeomCylinder(space=self.space, radius = self.wheelradius, length = self.wheelradius/3.0 ) leftwheelgeom.setBody(leftwheelbody) leftwheelgeom.setPosition(leftwheel_pos) leftwheelgeom.setRotation(rot) leftwheelM = ode.Mass() leftwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius, self.wheelradius/3.0) leftwheelbody.setMass(leftwheelM) lefthinge = ode.HingeJoint(self.world) lefthinge.attach(leftwheelbody,boxbody) lefthinge.setAnchor(leftwheel_pos) lefthinge.setAxis(self.rotate((0,0,1),rot)) lefthinge.setParam(ode.ParamFMax,self.hingemaxforce) rightwheelbody = ode.Body(self.world) rightwheelgeom = ode.GeomCylinder(space=self.space, radius = self.wheelradius, length = self.wheelradius ) rightwheelgeom.setBody(rightwheelbody) rightwheelgeom.setPosition(rightwheel_pos) rightwheelgeom.setRotation(rot) rightwheelM = ode.Mass() rightwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius, self.cubemass/3.0) rightwheelbody.setMass(rightwheelM) righthinge = ode.HingeJoint(self.world) righthinge.attach(rightwheelbody,boxbody) righthinge.setAnchor(rightwheel_pos) righthinge.setAxis(self.rotate((0,0,1),rot)) righthinge.setParam(ode.ParamFMax,self.hingemaxforce) # Create a spherical wheel at the back for support caster_pos = [box_pos[0], box_pos[1] - self.cubesize*0.5, box_pos[2] - self.cubesize*0.5] casterbody = ode.Body(self.world) castergeom = ode.GeomSphere(space=self.space, radius = self.cubesize/5.0) castergeom.setBody(casterbody) castergeom.setPosition(caster_pos) castergeom.setRotation(rot) casterM = ode.Mass() casterM.setSphere(self.cubemass*100.0, self.cubesize/5.0) casterbody.setMass(casterM) self.fixed = ode.FixedJoint(self.world) self.fixed.attach(casterbody,boxbody) self.fixed.setFixed() # WHEW, THE END OF ALL THAT FINALLY! # Build the Geoms and Joints arrays for rendering. self.boxgeom = boxgeom self._geoms = [boxgeom, leftwheelgeom, rightwheelgeom, castergeom, self.ground] self.lefthinge = lefthinge self.righthinge = righthinge self._joints = [self.lefthinge, self.righthinge, self.fixed]
# And a rod without any weight rod = ode.Body(world) M = ode.Mass() M.setCylinder(0.01, 2, 0.01, 1) M.mass = 1e-3 rod.setMass(M) rod.setPosition((0, 0.5, 0)) # Connect the rod with the world through a Hinge joint. world_joint = ode.HingeJoint(world) world_joint.attach(rod, ode.environment) world_joint.setAnchor((0, 0, 0)) world_joint.setAxis((0, 0, 1)) # Connect rod with ball with a fixed joint rod_ball_joint = ode.FixedJoint(world) rod_ball_joint.attach(rod, ball) rod_ball_joint.setFixed() # Create the viewer window viewer = Euv.Viewer(size=(600, 600), view_port_center=(0, 0), view_port_width=2.5, flip_y=True) # Do the simulation total_time = 0.0 dt = 0.02 Kf = 0.5 # Friction force while total_time < 30:
def world_tick(self): #if random.randrange(10) == 0: #boat_lidar.get_lidar_range() # Use and publish whoa! water_vel = self.get_water_vel(V(body.getPosition())) body.addForceAtRelPos((0, 0, self.buoyancy_force(-body.getPosition()[2], 0.5)), (0, 0, .1)) # the following frictional forces are from darsen's models #frictional force opposite to the velocity of the boat body_velocity=body.vectorFromWorld(body.getLinearVel()) # adds a resistance force for the water proportional to the velocity (where [0] is x, [1] is y, and [2] is z). units in N/(m/s) friction_force_forward=self.friction_coefficient_forward*body_velocity[0]-self.friction_coefficient_forward_reduction*(-1.0*pow(body_velocity[0],2) if body_velocity[0]<0.0 else pow(body_velocity[0],2)) friction_force_lateral=self.friction_coefficient_lateral*body_velocity[1]-self.friction_coefficient_lateral_reduction*(-1.0*pow(body_velocity[1],2) if body_velocity[1]<0.0 else pow(body_velocity[1],2)) body.addRelForce([-friction_force_forward,-friction_force_lateral,body_velocity[2]*-450]) # adds a angular resistance force for the water proportional to the velocity # angular_velocity is a 3-tuple ([0]=roll,[1]=pitch,[2]=yaw) angular_velocity=body.vectorFromWorld(body.getAngularVel()) friction_force_roll=self.friction_coefficient_rotational*angular_velocity[0]-self.friction_coefficient_rotational_reduction*(-1.0*pow(angular_velocity[0],2) if angular_velocity[0]<0.0 else pow(angular_velocity[0],2)) friction_force_pitch=self.friction_coefficient_rotational*angular_velocity[1]-self.friction_coefficient_rotational_reduction*(-1.0*pow(angular_velocity[1],2) if angular_velocity[1]<0.0 else pow(angular_velocity[1],2)) friction_force_yaw=self.friction_coefficient_rotational*angular_velocity[2]-self.friction_coefficient_rotational_reduction*(-1.0*pow(angular_velocity[2],2) if angular_velocity[2]<0.0 else pow(angular_velocity[2],2)) body.addRelTorque([-friction_force_roll,-friction_force_pitch,-friction_force_yaw]) #print thrusters # map the thrusters position on the boat self.boat_model.vectors = [] # id2=starboard, id3=port if not self.killed: for thruster_id in [0, 1, 2, 3]: # set the thrusters position on the boat if thruster_id == 0: relpos = v(self.BL_offset[0], self.BL_offset[1], 0) reldir = v(1, 1, 0) if thruster_id == 1: relpos = v(self.BR_offset[0], self.BR_offset[1], 0) reldir = v(1, -1, 0) if thruster_id == 2: relpos = v(self.FL_offset[0], self.FL_offset[1], 0) reldir = v(1, -1, 0) if thruster_id == 3: relpos = v(self.FR_offset[0], self.FR_offset[1], 0) reldir = v(1, 1, 0) force = self.thrusts[thruster_id] body.addRelForceAtRelPos(reldir*force, relpos) self.boat_model.vectors.append((relpos, relpos + 0.05*reldir*force)) keys = pygame.key.get_pressed() for keycode, force in [ (pygame.K_k, v(-50, 0, 0)), (pygame.K_i, v(+50, 0, 0)), (pygame.K_j, v(0, +50, 0)), (pygame.K_l, v(0, -50, 0)), (pygame.K_o, v(0, 0, +50)), (pygame.K_m, v(0, 0, -50)), ]: if keys[keycode]: body.addRelForce(force*(10 if keys[pygame.K_RSHIFT] else 1)*(.1 if keys[pygame.K_RCTRL] else 1)) for keycode, torque in [ (pygame.K_COMMA, v(-20, 0, 0)), (pygame.K_u, v(+20, 0, 0)), (pygame.K_h, v(0, +20, 0)), (pygame.K_SEMICOLON, v(0, -20, 0)), (pygame.K_0, v(0, 0, +20)), (pygame.K_n, v(0, 0, -20)), ]: if keys[keycode]: body.addRelTorque(torque*(10 if keys[pygame.K_RSHIFT] else 1)*(.1 if keys[pygame.K_RCTRL] else 1)) if keys[pygame.K_1]: self.killed = True if keys[pygame.K_2]: self.killed = False global locked if keys[pygame.K_3]: locked = True if keys[pygame.K_4]: locked = False contactgroup = ode.JointGroup() if self.locked: j = ode.FixedJoint(world, contactgroup) j.attach(body, None) j.setFixed() near_pairs = [] space.collide(None, lambda _, geom1, geom2: near_pairs.append((geom1, geom2))) for geom1, geom2 in near_pairs: for contact in ode.collide(geom1, geom2): contact.setBounce(0.2) contact.setMu(5000) j = ode.ContactJoint(world, contactgroup, contact) j.attach(geom1.getBody(), geom2.getBody()) dt = 1/50 self.world_time += dt world.step(dt) contactgroup.empty() pos = body.getPosition() q = V(body.getQuaternion()) # Avoid stupid [0, 0, 0, 0] quaternion initialization if not numpy.any(q): q = [1, 0, 0, 0] # Publish tf /enu self.enu_tf_br.sendTransform( translation = (pos[0], pos[1], pos[2]), rotation = (q[1], q[2], q[3], q[0]), time = rospy.Time(self.world_time), child = '/base_link', parent = '/enu') # Publish odom msg = Odometry() msg.header.stamp = rospy.Time(self.world_time) msg.header.frame_id = '/enu' msg.child_frame_id = '/base_link' msg.pose.pose.position = Point(*pos) msg.pose.pose.orientation = Quaternion(q[1], q[2], q[3], q[0]) msg.twist.twist.linear = Vector3(*q.conj().quat_rot(body.getLinearVel())) msg.twist.twist.angular = Vector3(*q.conj().quat_rot(body.getAngularVel())) self.odom_pub.publish(msg) # XXX msg = Odometry() msg.header.stamp = rospy.Time(self.world_time) msg.header.frame_id = '/ecef' msg.child_frame_id = '/base_link' # ecef_loc = gps.ecef_from_latlongheight(math.radians(36.802002), math.radians(-76.191019), 7) enu_loc = numpy.array([53.6686215007, -20.8502282916, -0.0733864689281]) # msg.pose.pose.position = Point(*gps.ecef_from_enu(enu_v=body.getPosition() - enu_loc, ecef_pos=ecef_loc) + ecef_loc) self.abs_odom_pub.publish(msg) reactor.callLater(max(0, self.world_time + dt - reactor.seconds()), self.world_tick)