def createDoublePendulumModel(): model = osim.Model() model.setName("double_pendulum") # Create two links, each with a mass of 1 kg, center of mass at the body's # origin, and moments and products of inertia of zero. b0 = osim.Body("b0", 1, osim.Vec3(0), osim.Inertia(1)) model.addBody(b0) b1 = osim.Body("b1", 1, osim.Vec3(0), osim.Inertia(1)) model.addBody(b1) # Add markers to body origin locations. m0 = osim.Marker("m0", b0, osim.Vec3(0)) m1 = osim.Marker("m1", b1, osim.Vec3(0)) model.addMarker(m0) model.addMarker(m1) # Connect the bodies with pin joints. Assume each body is 1 m long. j0 = osim.PinJoint("j0", model.getGround(), osim.Vec3(0), osim.Vec3(0), b0, osim.Vec3(-1, 0, 0), osim.Vec3(0)) q0 = j0.updCoordinate() q0.setName("q0") j1 = osim.PinJoint("j1", b0, osim.Vec3(0), osim.Vec3(0), b1, osim.Vec3(-1, 0, 0), osim.Vec3(0)) q1 = j1.updCoordinate() q1.setName("q1") model.addJoint(j0) model.addJoint(j1) tau0 = osim.CoordinateActuator() tau0.setCoordinate(j0.updCoordinate()) tau0.setName("tau0") tau0.setOptimalForce(1) model.addComponent(tau0) tau1 = osim.CoordinateActuator() tau1.setCoordinate(j1.updCoordinate()) tau1.setName("tau1") tau1.setOptimalForce(1) model.addComponent(tau1) # Add display geometry. bodyGeometry = osim.Ellipsoid(0.5, 0.1, 0.1) transform = osim.Transform(osim.Vec3(-0.5, 0, 0)) b0Center = osim.PhysicalOffsetFrame("b0_center", b0, transform) b0.addComponent(b0Center) b0Center.attachGeometry(bodyGeometry.clone()) b1Center = osim.PhysicalOffsetFrame("b1_center", b1, transform) b1.addComponent(b1Center) b1Center.attachGeometry(bodyGeometry.clone()) model.finalizeConnections() model.printToXML("double_pendulum.osim") return model
def generate_results(self, root_dir, args): model = osim.Model() body = osim.Body("b", 1, osim.Vec3(0), osim.Inertia(0)) model.addBody(body) joint = osim.SliderJoint("j", model.getGround(), body) joint.updCoordinate().setName("coord") model.addJoint(joint) damper = osim.SpringGeneralizedForce("coord") damper.setViscosity(-1.0) model.addForce(damper) actu = osim.CoordinateActuator("coord") model.addForce(actu) model.finalizeConnections() moco = osim.MocoStudy() problem = moco.updProblem() problem.setModel(model) problem.setTimeBounds(0, 2) problem.setStateInfo("/jointset/j/coord/value", [-10, 10], 0, 5) problem.setStateInfo("/jointset/j/coord/speed", [-10, 10], 0, 2) problem.setControlInfo("/forceset/coordinateactuator", [-50, 50]) problem.addGoal(osim.MocoControlGoal("effort", 0.5)) solver = moco.initCasADiSolver() solver.set_num_mesh_intervals(50) solution = moco.solve() solution.write(self.solution_file % root_dir)
def test_markAdopted2(self): a = osim.Model() # We just need the following not to cause a segfault. # Model add* pa = osim.PathActuator() pa.setName('pa') a.addForce(pa) probe = osim.Umberger2010MuscleMetabolicsProbe() probe.setName('probe') a.addProbe(probe) ma = osim.MuscleAnalysis() ma.setName('ma') a.addAnalysis(ma) pc = osim.PrescribedController() pc.setName('pc') a.addController(pc) body = osim.Body('body1', 1.0, osim.Vec3(0, 0, 0), osim.Inertia(0, 0, 0)) loc_in_parent = osim.Vec3(0, 0, 0) orient_in_parent = osim.Vec3(0, 0, 0) loc_in_body = osim.Vec3(0, 0, 0) orient_in_body = osim.Vec3(0, 0, 0) print "creating Weld Joint.." joint = osim.WeldJoint("weld_joint", a.getGround(), loc_in_parent, orient_in_parent, body, loc_in_body, orient_in_parent) print "adding a body .." a.addBody(body) print "adding a joint .." a.addJoint(joint) print "Creating a ConstantDistanceConstraint.." constr = osim.ConstantDistanceConstraint() constr.setBody1ByName("ground") constr.setBody1PointLocation(osim.Vec3(0, 0, 0)) constr.setBody2ByName("body") constr.setBody2PointLocation(osim.Vec3(1, 0, 0)) constr.setConstantDistance(1) a.addConstraint(constr) f = osim.BushingForce("bushing", "ground", "body", osim.Vec3(2, 2, 2), osim.Vec3(1, 1, 1), osim.Vec3(0, 0, 0), osim.Vec3(0, 0, 0)) a.addForce(f) f2 = osim.BushingForce() a.addForce(f2) f3 = osim.SpringGeneralizedForce() a.addForce(f3) model = osim.Model(os.path.join(test_dir, "arm26.osim")) g = osim.CoordinateActuator('r_shoulder_elev') model.addForce(g)
def addCoordinateActuator(model, coordName, optForce): coordSet = model.updCoordinateSet() actu = osim.CoordinateActuator() actu.setName('tau_' + coordName) actu.setCoordinate(coordSet.get(coordName)) actu.setOptimalForce(optForce) actu.setMinControl(-1) actu.setMaxControl(1) model.addComponent(actu)
def add_reserve(model, coord, max_control): actu = osim.CoordinateActuator(coord) if coord.startswith('lumbar'): prefix = 'torque_' elif coord.startswith('pelvis'): prefix = 'residual_' else: prefix = 'reserve_' actu.setName(prefix + coord) actu.setMinControl(-1) actu.setMaxControl(1) actu.setOptimalForce(max_control) model.addForce(actu)
def addCoordinateActuator(model, coordinateName, optForce): coordSet = model.getCoordinateSet(); actu = osim.CoordinateActuator(); actu.setName('torque_' + coordinateName); actu.setCoordinate(coordSet.get(coordinateName)); actu.setOptimalForce(optForce); actu.setMinControl(-1); actu.setMaxControl(1); # Add to ForceSet model.addForce(actu);
def test_markAdopted(): a = osim.Model() # We just need the following not to not cause a segfault. # Model add* a.addComponent(osim.PathActuator()) a.addProbe(osim.Umberger2010MuscleMetabolicsProbe()) a.addAnalysis(osim.MuscleAnalysis()) a.addController(osim.PrescribedController()) body = osim.Body('body', 1.0, osim.Vec3(0, 0, 0), osim.Inertia(0, 0, 0)) loc_in_parent = osim.Vec3(0, -0, 0) orient_in_parent = osim.Vec3(0, 0, 0) loc_in_body = osim.Vec3(0, 0, 0) orient_in_body = osim.Vec3(0, 0, 0) joint = osim.WeldJoint("weld_joint", a.getGroundBody(), loc_in_parent, orient_in_parent, body, loc_in_body, orient_in_parent) a.addBody(body) constr = osim.ConstantDistanceConstraint() constr.setBody1ByName("ground") constr.setBody1PointLocation(osim.Vec3(0, 0, 0)) constr.setBody2ByName("body") constr.setBody2PointLocation(osim.Vec3(1, 0, 0)) constr.setConstantDistance(1) a.addConstraint(constr) # Force requires body names. If not provided, you get a segfault. f = osim.BushingForce() f.setBody1ByName("ground") f.setBody2ByName("body") a.addForce(f) model = osim.Model(os.environ['OPENSIM_HOME'] + "/Models/Arm26/arm26.osim") g = osim.CoordinateActuator('r_shoulder_elev') model.addForce(g)
def createSlidingMassModel(): model = osim.Model() model.setName("sliding_mass") model.set_gravity(osim.Vec3(0, 0, 0)) body = osim.Body("body", 10.0, osim.Vec3(0), osim.Inertia(0)) model.addComponent(body) # Allows translation along x. joint = osim.SliderJoint("slider", model.getGround(), body) coord = joint.updCoordinate(osim.SliderJoint.Coord_TranslationX) coord.setName("position") model.addComponent(joint) actu = osim.CoordinateActuator() actu.setCoordinate(coord) actu.setName("actuator") actu.setOptimalForce(1) actu.setMinControl(-10) actu.setMaxControl(10) model.addComponent(actu) return model
def test_PrescribedController_prescribeControlForActuator(self): # Test memory management for # PrescribedController::prescribeControlForActuator(). model = osim.Model() # Body. body = osim.Body('b1', 1.0, osim.Vec3(0, 0, 0), osim.Inertia(0, 0, 0)) model.addBody(body) # Joint. joint = osim.PinJoint('j1', model.getGround(), body) model.addJoint(joint) # Actuator. actu = osim.CoordinateActuator() actu.setName('actu') actu.setCoordinate(joint.get_coordinates(0)) model.addForce(actu) # Controller. contr = osim.PrescribedController() contr.addActuator(actu) self.assertRaises(RuntimeError, contr.prescribeControlForActuator, 1, osim.Constant(3)) # The following calls should not cause a memory leak: contr.prescribeControlForActuator(0, osim.Constant(2)) contr.prescribeControlForActuator('actu', osim.Constant(4))
def addCoordinateActuator(modelObject = None, coordinate = None, optForce = 1.0, controlLevel = [float('inf'),float('-inf')], appendStr = '_torque'): # Convenience function for adding a coordinate actuator to model # # Input: modelObject - Opensim model object to add actuator to # coordinate - string of coordinate name for actuator # optForce - value for actuators optimal force # controlLevel - [x,y] values for max and min control # appendStr - string to append to coordinate name in setting actuator name #Check for model object if modelObject is None: raise ValueError('A model object must be included') #Check for coordinate if coordinate is None: raise ValueError('A coordinate string must be specified') #Create actuator actu = osim.CoordinateActuator() #Set name actu.setName(coordinate+appendStr) #Set coordinate for actuator actu.setCoordinate(modelObject.getCoordinateSet().get(coordinate)) #Set optimal force actu.setOptimalForce(optForce) #Set control levels actu.setMaxControl(controlLevel[0]) actu.setMinControl(controlLevel[1]) #Add actuator to models forceset modelObject.updForceSet().append(actu)
import os import opensim as osim model = osim.Model() model.setName('sliding_mass') model.set_gravity(osim.Vec3(0, 0, 0)) body = osim.Body('body', 2.0, osim.Vec3(0), osim.Inertia(0)) model.addComponent(body) # Allows translation along x. joint = osim.SliderJoint('slider', model.getGround(), body) coord = joint.updCoordinate() coord.setName('position') model.addComponent(joint) actu = osim.CoordinateActuator() actu.setCoordinate(coord) actu.setName('actuator') actu.setOptimalForce(1) model.addComponent(actu) body.attachGeometry(osim.Sphere(0.05)) model.finalizeConnections() # Create MucoTool. # ================ muco = osim.MucoTool() muco.setName('sliding_mass') # Define the optimal control problem.
masterOffsetFrame = opensim.PhysicalOffsetFrame() masterOffsetFrame.setParentFrame(master) masterOffsetFrame.setOffsetTransform( opensim.Transform(opensim.Vec3(0, 0, 0.5))) master.addComponent(masterOffsetFrame) masterOffsetFrame.attachGeometry(blockMesh.clone()) blockMesh.setColor(opensim.Blue) slaveOffsetFrame = opensim.PhysicalOffsetFrame() slaveOffsetFrame.setParentFrame(slave) slaveOffsetFrame.setOffsetTransform( opensim.Transform(opensim.Vec3(0, 0, -0.5))) slave.addComponent(slaveOffsetFrame) slaveOffsetFrame.attachGeometry(blockMesh.clone()) masterCoordinate = masterToGround.updCoordinate() masterInputActuator = opensim.CoordinateActuator('masterInput') masterInputActuator.setCoordinate(masterCoordinate) model.addForce(masterInputActuator) masterFeedbackActuator = opensim.CoordinateActuator('masterFeedback') masterFeedbackActuator.setCoordinate(masterCoordinate) model.addForce(masterFeedbackActuator) slaveCoordinate = slaveToGround.updCoordinate() slaveInputActuator = opensim.CoordinateActuator('slaveInput') slaveInputActuator.setCoordinate(slaveCoordinate) model.addForce(slaveInputActuator) slaveFeedbackActuator = opensim.CoordinateActuator('slaveFeedback') slaveFeedbackActuator.setCoordinate(slaveCoordinate) model.addForce(slaveFeedbackActuator) #model.finalizeFromProperties()
def test_bounds(self): model = osim.Model() model.setName('sliding_mass') model.set_gravity(osim.Vec3(0, 0, 0)) body = osim.Body('body', 2.0, osim.Vec3(0), osim.Inertia(0)) model.addComponent(body) joint = osim.SliderJoint('slider', model.getGround(), body) coord = joint.updCoordinate() coord.setName('position') model.addComponent(joint) actu = osim.CoordinateActuator() actu.setCoordinate(coord) actu.setName('actuator') actu.setOptimalForce(1) model.addComponent(actu) study = osim.MocoStudy() study.setName('sliding_mass') mp = study.updProblem() mp.setModel(model) ph0 = mp.getPhase() mp.setTimeBounds(osim.MocoInitialBounds(0.), osim.MocoFinalBounds(0.1, 5.)) assert ph0.getTimeInitialBounds().getLower() == 0 assert ph0.getTimeInitialBounds().getUpper() == 0 self.assertAlmostEqual(ph0.getTimeFinalBounds().getLower(), 0.1) self.assertAlmostEqual(ph0.getTimeFinalBounds().getUpper(), 5.0) mp.setTimeBounds([0.2, 0.3], [3.5]) self.assertAlmostEqual(ph0.getTimeInitialBounds().getLower(), 0.2) self.assertAlmostEqual(ph0.getTimeInitialBounds().getUpper(), 0.3) self.assertAlmostEqual(ph0.getTimeFinalBounds().getLower(), 3.5) self.assertAlmostEqual(ph0.getTimeFinalBounds().getUpper(), 3.5) # Use setter on MocoPhase. ph0.setTimeBounds([2.2, 2.3], [4.5]) self.assertAlmostEqual(ph0.getTimeInitialBounds().getLower(), 2.2) self.assertAlmostEqual(ph0.getTimeInitialBounds().getUpper(), 2.3) self.assertAlmostEqual(ph0.getTimeFinalBounds().getLower(), 4.5) self.assertAlmostEqual(ph0.getTimeFinalBounds().getUpper(), 4.5) mp.setStateInfo('slider/position/value', osim.MocoBounds(-5, 5), osim.MocoInitialBounds(0)) assert -5 == ph0.getStateInfo( 'slider/position/value').getBounds().getLower() assert 5 == ph0.getStateInfo( 'slider/position/value').getBounds().getUpper() assert isnan( ph0.getStateInfo( 'slider/position/value').getFinalBounds().getLower()) assert isnan( ph0.getStateInfo( 'slider/position/value').getFinalBounds().getUpper()) mp.setStateInfo('slider/position/speed', [-50, 50], [-3], 1.5) assert -50 == ph0.getStateInfo( 'slider/position/speed').getBounds().getLower() assert 50 == ph0.getStateInfo( 'slider/position/speed').getBounds().getUpper() assert -3 == ph0.getStateInfo( 'slider/position/speed').getInitialBounds().getLower() assert -3 == ph0.getStateInfo( 'slider/position/speed').getInitialBounds().getUpper() self.assertAlmostEqual( 1.5, ph0.getStateInfo( 'slider/position/speed').getFinalBounds().getLower()) self.assertAlmostEqual( 1.5, ph0.getStateInfo( 'slider/position/speed').getFinalBounds().getUpper()) # Use setter on MocoPhase. ph0.setStateInfo('slider/position/speed', [-6, 10], [-4, 3], [0]) assert -6 == ph0.getStateInfo( 'slider/position/speed').getBounds().getLower() assert 10 == ph0.getStateInfo( 'slider/position/speed').getBounds().getUpper() assert -4 == ph0.getStateInfo( 'slider/position/speed').getInitialBounds().getLower() assert 3 == ph0.getStateInfo( 'slider/position/speed').getInitialBounds().getUpper() assert 0 == ph0.getStateInfo( 'slider/position/speed').getFinalBounds().getLower() assert 0 == ph0.getStateInfo( 'slider/position/speed').getFinalBounds().getUpper() # Controls. mp.setControlInfo('actuator', osim.MocoBounds(-50, 50)) assert -50 == ph0.getControlInfo('actuator').getBounds().getLower() assert 50 == ph0.getControlInfo('actuator').getBounds().getUpper() mp.setControlInfo('actuator', [18]) assert 18 == ph0.getControlInfo('actuator').getBounds().getLower() assert 18 == ph0.getControlInfo('actuator').getBounds().getUpper()
opensim.Vec3(opensim.SimTK_PI / 2, 0.0, 0.0)) bodyMesh = opensim.Cylinder(BODY_SIZE / 2, BODY_SIZE) bodyMesh.setColor(BLOCK_COLORS[index]) offsetFrame.attachGeometry(bodyMesh) body.addComponent(offsetFrame) if index == 1: markerX = opensim.Marker('effector_x', body, opensim.Vec3(0, BODY_DISTANCE, 0)) model.addMarker(markerX) markerY = opensim.Marker('effector_y', body, opensim.Vec3(0, BODY_DISTANCE, 0)) model.addMarker(markerY) coordinate = pinJoint.getCoordinate() userActuator = opensim.CoordinateActuator(coordinate.getName()) model.addForce(userActuator) controlActuator = opensim.CoordinateActuator(coordinate.getName()) model.addForce(controlActuator) systemState = model.initSystem() file = open('robot_model.osim', 'w') file.write(model.dump()) file.close() manager = opensim.Manager(model) systemState.setTime(0) manager.initialize(systemState) systemState = manager.integrate(0.001)