Example #1
0
    def __init__(self, filename, bounding_box=None):
        self.filename = filename

        # If bounding box is not passed in, calculate it
        if bounding_box == None:
            self.bounding_box = CalcBoundingBox()
        else:
            self.bounding_box = bounding_box

        self.mesh = chrono.ChTriangleMeshConnected()
        self.mesh.LoadWavefrontMesh(chrono.GetChronoDataFile(filename), False,
                                    True)
        self.shape = chrono.ChTriangleMeshShape()
        self.shape.SetMesh(self.mesh)
        self.shape.SetStatic(True)
        self.body = chrono.ChBody()
        self.body.AddAsset(self.shape)
        self.body.SetCollide(False)
        self.body.SetBodyFixed(True)

        self.scaled = False

        self.pos = chrono.ChVectorD()
        self.scale = 1
        self.ang = 0
Example #2
0
    def __init__(self, filename, scale_range):
        self.filename = filename
        self.scale_range = scale_range
        self.ready = False

        self.mesh = chrono.ChTriangleMeshConnected()
        self.mesh.LoadWavefrontMesh(chrono.GetChronoDataFile(self.filename), True, True)

        self.shape = chrono.ChTriangleMeshShape()
        self.shape.SetMesh(self.mesh)
        self.shape.SetStatic(True)

        self.body = chrono.ChBody()
        self.body.AddAsset(self.shape)
        self.body.SetCollide(True)
        self.body.SetBodyFixed(True)

        surface_mat = chrono.ChMaterialSurfaceNSC()
        surface_mat.SetFriction(0.9)
        surface_mat.SetRestitution(0.01)

        self.body.GetCollisionModel().ClearModel()
        self.body.GetCollisionModel().AddTriangleMesh(surface_mat, self.mesh, False, False)
        self.body.GetCollisionModel().BuildModel()

        self.scale = 1
Example #3
0
    def DrawCones(self, points, color, z=.3, n=10):
        for p in points[::n]:
            p.z += z
            cmesh = chrono.ChTriangleMeshConnected()
            if color == 'red':
                cmesh.LoadWavefrontMesh(
                    chrono.GetChronoDataFile("sensor/cones/red_cone.obj"),
                    False, True)
            elif color == 'green':
                cmesh.LoadWavefrontMesh(
                    chrono.GetChronoDataFile("sensor/cones/green_cone.obj"),
                    False, True)

            cshape = chrono.ChTriangleMeshShape()
            cshape.SetMesh(cmesh)
            cshape.SetName("Cone")
            cshape.SetStatic(True)

            cbody = chrono.ChBody()
            cbody.SetPos(p)
            cbody.AddAsset(cshape)
            cbody.SetBodyFixed(True)
            cbody.SetCollide(False)
            if color == 'red':
                cbody.AddAsset(chrono.ChColorAsset(1, 0, 0))
            elif color == 'green':
                cbody.AddAsset(chrono.ChColorAsset(0, 1, 0))

            self.system.Add(cbody)
Example #4
0
def load_shape(filepath, contact_method, texture):
    """
    Import the mesh stored in filepath as a shape
    :param filepath: Path to the mesh
    :param contact_method: SMC or NSC
    :param texture: Path to the texture .jpg
    :return: A ChBody shape
    """
    shape = chrono.ChBody(contact_method)
    shape.SetBodyFixed(True)
    shape_mesh = chrono.ChObjShapeFile()
    shape_mesh.SetFilename(filepath)
    shape.AddAsset(shape_mesh)
    tmc = chrono.ChTriangleMeshConnected()
    tmc.LoadWavefrontMesh(filepath)

    # Add a collision mesh to the shape
    shape.GetCollisionModel().ClearModel()
    shape.GetCollisionModel().AddTriangleMesh(tmc, True, True)
    shape.GetCollisionModel().BuildModel()
    shape.SetShowCollisionMesh(True)
    shape.SetCollide(False)

    # Add a skin texture
    skin_texture = chrono.ChTexture()
    skin_texture.SetTextureFilename(chrono.GetChronoDataPath() + texture)
    shape.GetAssets().push_back(skin_texture)

    return shape
    def __init__(self,
                 system: 'WAChronoSystem',
                 filename: str,
                 vehicle: 'WAChronoVehicle' = None,
                 body: 'WABody' = None):
        if missing_chrono_sensor:
            sens_import_error('WAChronoSensor.__init__')

        super().__init__(vehicle, body)

        j = _load_json(filename)

        # Validate the json file
        _check_field(j, 'Type', value='Sensor')
        _check_field(j,
                     'Template',
                     allowed_values=['Camera', 'Lidar', 'IMU', 'GPS'])  # noqa
        _check_field(j, 'Offset Pose', field_type=dict)

        offset_pose = ChFrame_from_json(j['Offset Pose'])

        if vehicle is not None:
            body = vehicle._vehicle.GetChassisBody()
        else:
            asset = body
            body = chrono.ChBody()
            body.SetPos(WAVector_to_ChVector(asset.position))
            body.SetBodyFixed(True)
            system._system.AddBody(body)

            # Create the chrono sensor through the Sensor chrono class
        self._sensor = sens.Sensor.CreateFromJSON(filename, body,
                                                  offset_pose)  # noqa
Example #6
0
 def addLeaders(self, system, path):
     self.leaders = []
     self.path = path
     for i in range(self.numlead):
         leader = chrono.ChBody()
         leader.AddAsset(self.trimesh_shape)
         system.Add(leader)
         self.leaders.append(leader)
     self.Update()
Example #7
0
 def placeObstacle(self, numob):
     for i in range(numob):
         side = randint(1, 2)
         path = randint(0, len(self.obst_paths) - 1)
         obst = chrono.ChBody()
         #vis_mesh = self.vis_meshes[path]
         obst.AddAsset(self.trimeshes[path])
         x, y, z = self.obst_bound[path]
         obst.GetCollisionModel().ClearModel()
         obst.GetCollisionModel().AddBox(self.surf_material, x / 2, y / 2,
                                         z / 2)  # must set half sizes
         obst.GetCollisionModel().BuildModel()
         obst.SetCollide(True)
         p0, q = self.path.getPosRot((i + 1) / (numob + 1))
         dist = np.max([x, y]) + self.leader_box[1]
         pos = p0 + q.Rotate(chrono.VECT_Y) * (
             dist * pow(-1, side)) - chrono.ChVectorD(0, 0, p0.z)
         obst.SetPos(pos)
         obst.SetBodyFixed(True)
         self.obstacles.append(obst)
         self.system.Add(obst)
Example #8
0
    def __init__(self, system, mesh):
        # Create a section, i.e. thickness and material properties
        # for beams. This will be shared among some beams.

        msection_cable2 = fea.ChBeamSectionCable()
        msection_cable2.SetDiameter(0.015)
        msection_cable2.SetYoungModulus(0.01e9)
        msection_cable2.SetBeamRaleyghDamping(0.000)

        # This ChBuilderCableANCF helper object is very useful because it will
        # subdivide 'beams' into sequences of finite elements of beam type, ex.
        # one 'beam' could be made of 5 FEM elements of ChElementBeamANCF class.
        # If new nodes are needed, it will create them for you.
        builder = fea.ChBuilderCableANCF()

        # Now, simply use BuildBeam to create a beam from a poto another:
        builder.BuildBeam(
            mesh,  # the mesh where to put the created nodes and elements
            msection_cable2,  # the ChBeamSectionCable to use for the ChElementBeamANCF elements
            10,  # the number of ChElementBeamANCF to create
            chrono.ChVectorD(0, 0,
                             -0.1),  # the 'A' poin space (beginning of beam)
            chrono.ChVectorD(0.5, 0, -0.1))  # the 'B' poin space (end of beam)

        # After having used BuildBeam(), you can retrieve the nodes used for the beam,
        # For example say you want to fix both pos and dir of A end and apply a force to the B end:
        # builder.GetLastBeamNodes().back().SetFixed(True)
        builder.GetLastBeamNodes().front().SetForce(
            chrono.ChVectorD(0, -0.2, 0))

        # For instance, now retrieve the A end and add a constrato
        # block the position only of that node:
        mtruss = chrono.ChBody()
        mtruss.SetBodyFixed(True)

        constraint_hinge = fea.ChLinkPointFrame()
        constraint_hinge.Initialize(builder.GetLastBeamNodes().back(), mtruss)
        system.Add(constraint_hinge)
Example #9
0
def make_box(size,
             pos,
             fixed=False,
             collide=True,
             euler_angles=(0, 0, 0),
             color=(1., 0, 1.)):
    body = chrono.ChBody()
    body.SetBodyFixed(fixed)
    body.SetPos(chrono.ChVectorD(*(swap_yz(pos))))
    body.SetRot(chrono.Q_from_Euler123(chrono.ChVectorD(*euler_angles)))

    body.GetCollisionModel().ClearModel()
    # swap_yz(size)
    body.GetCollisionModel().AddBox(size[0], size[2],
                                    size[1])  # must set half sizes
    body.GetCollisionModel().BuildModel()
    body.SetCollide(collide)

    mboxasset = chrono.ChBoxShape()
    mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(*(swap_yz(size)))
    mboxasset.SetColor(chrono.ChColor(color[0], color[1], color[2]))
    body.AddAsset(mboxasset)
    # system.Add(body)
    return body
Example #10
0
    def reset(self):

        self.isdone = False
        self.hexapod_sys.Clear()
        self.exported_items = chrono.ImportSolidWorksSystem(self.fpath)
        self.csys = []
        self.frames = []
        self.revs = []
        self.motors = []
        self.limits = []

        for con, coi in zip(self.con_link, self.coi_link):
            indices = []
            for i in range(len(self.exported_items)):
                if con == self.exported_items[i].GetName(
                ) or coi == self.exported_items[i].GetName():
                    indices.append(i)
            rev = self.exported_items[indices[0]]
            af0 = rev.GetAssetsFrame()
            # Revolute joints and ChLinkMotorRotation are z oriented, while parallel is x oriented.
            # Event though this Frame won't be used anymore is good practice to create a copy before editing its value.
            af = chrono.ChFrameD(af0)
            af.SetRot(af0.GetRot() % chrono.Q_ROTATE_X_TO_Z)
            self.frames.append(af)
            for i in reversed(indices):
                del self.exported_items[i]

        # ADD IMPORTED ITEMS TO THE SYSTEM
        for my_item in self.exported_items:
            self.hexapod_sys.Add(my_item)
        """
              $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR MARKERS AND GET RID OF EM $$$$$$$$ 
              """
        self.hips = [
            self.hexapod_sys.SearchBody(name) for name in self.hip_names
        ]
        self.femurs = [
            self.hexapod_sys.SearchBody(name) for name in self.femur_names
        ]
        self.tibias = [
            self.hexapod_sys.SearchBody(name) for name in self.tibia_names
        ]
        self.feet = [
            self.hexapod_sys.SearchBody(name) for name in self.feet_names
        ]
        self.centralbody = self.hexapod_sys.SearchBody('Body-1')
        # Bodies are used to replace constraints and detect unwanted collision, so feet are excluded
        self.bodies = [self.centralbody
                       ] + self.hips + self.femurs + self.tibias
        self.centralbody.SetBodyFixed(False)
        self.y0 = self.centralbody.GetPos().y
        """
              # SNIPPET FOR COLOR
              orange = chrono.ChColorAsset()
              orange.SetColor(chrono.ChColor(255/255,77/255,6/255))
              black = chrono.ChColorAsset()
              black.SetColor(chrono.ChColor(0,0,0))
              for body in self.bodies[:-1]:
                  assets = body.GetAssets()
                  for ast in assets:
                      ass_lev = chrono.CastToChAssetLevel(ast)
                      ass_lev.GetAssets().push_back(orange) 
                      
              assets = self.hand.GetAssets()
              for ast in assets:
                      ass_lev = chrono.CastToChAssetLevel(ast)
                      ass_lev.GetAssets().push_back(black) 
              """

        for i in range(len(self.con_link)):
            revolute = chrono.ChLinkLockRevolute()
            cs = chrono.ChCoordsysD(self.frames[i].GetPos(),
                                    self.frames[i].GetRot())
            self.csys.append(cs)
            if i < 6:
                j = 0
            else:
                j = i - 5
            revolute.Initialize(self.bodies[j], self.bodies[i + 1],
                                self.csys[i])
            self.revs.append(revolute)
            self.hexapod_sys.Add(self.revs[i])
            lim = self.revs[i].GetLimit_Rz()
            self.limits.append(lim)
            self.limits[i].SetActive(True)
            self.limits[i].SetMin(self.minRot[i] * (math.pi / 180))
            self.limits[i].SetMax(self.maxRot[i] * (math.pi / 180))
            m = chrono.ChLinkMotorRotationTorque()
            m.SetSpindleConstraint(False, False, False, False, False)
            m.Initialize(self.bodies[j], self.bodies[i + 1], self.frames[i])
            self.motors.append(m)
            self.hexapod_sys.Add(self.motors[i])

        self.body_floor = chrono.ChBody()
        self.body_floor.SetBodyFixed(True)
        self.body_floor.SetPos(chrono.ChVectorD(0, -1 - 0.128 - 0.0045, 10))

        # Floor Collision.
        self.body_floor.GetCollisionModel().ClearModel()
        self.body_floor.GetCollisionModel().AddBox(self.my_material, 50, 1, 50,
                                                   chrono.ChVectorD(0, 0, 0))
        self.body_floor.GetCollisionModel().BuildModel()
        self.body_floor.SetCollide(True)

        # Visualization shape
        body_floor_shape = chrono.ChBoxShape()
        body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(4, 1, 15)
        body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
        self.body_floor.GetAssets().push_back(body_floor_shape)
        body_floor_texture = chrono.ChTexture()
        texpath = os.path.join(chrono.GetChronoDataPath(), 'concrete.jpg')
        body_floor_texture.SetTextureFilename(texpath)
        self.body_floor.GetAssets().push_back(body_floor_texture)
        self.hexapod_sys.Add(self.body_floor)

        self.numsteps = 0

        if (self.render_setup):
            self.myapplication.AssetBindAll()
            self.myapplication.AssetUpdateAll()
        return self.get_ob()
## 1. Create the physical system that will handle all bodies and constraints.

##    Specify the gravitational acceleration vector, consistent with the
##    global reference frame having Z up.
system = chrono.ChSystemNSC()
system.Set_G_acc(chrono.ChVectorD(0, 0, -9.81))

## 2. Create the rigid bodies of the slider-crank mechanical system.
##    For each body, specify:
##    - a unique identifier
##    - mass and moments of inertia
##    - position and orientation of the (centroidal) body frame
##    - visualization assets (defined with respect to the body frame)

## Ground
ground = chrono.ChBody()
system.AddBody(ground)
ground.SetIdentifier(-1)
ground.SetName("ground")
ground.SetBodyFixed(True)

cyl_g = chrono.ChCylinderShape()
cyl_g.GetCylinderGeometry().p1 = chrono.ChVectorD(0, 0.2, 0)
cyl_g.GetCylinderGeometry().p2 = chrono.ChVectorD(0, -0.2, 0)
cyl_g.GetCylinderGeometry().rad = 0.03
ground.AddAsset(cyl_g)

col_g = chrono.ChColorAsset()
col_g.SetColor(chrono.ChColor(0.6, 0.6, 0.2))
ground.AddAsset(col_g)
            body_particles.AddParticle(chrono.ChCoordsysD(chrono.ChVectorD(ix/100,0.1+iy/100, iz/100)))

# Visualization shape (shared by all particle clones)
body_particles_shape = chrono.ChSphereShape()
body_particles_shape.GetSphereGeometry().rad = 0.005
body_particles.GetAssets().push_back(body_particles_shape)

my_system.Add(body_particles)




# Create the floor: a simple fixed rigid body with a collision shape
# and a visualization shape

body_floor = chrono.ChBody()
body_floor.SetBodyFixed(True)

# Collision shape
body_floor.GetCollisionModel().ClearModel()
body_floor.GetCollisionModel().AddBox(0.1, 0.02, 0.1) # hemi sizes
body_floor.GetCollisionModel().BuildModel()
body_floor.SetCollide(True)

# Visualization shape
body_floor_shape = chrono.ChBoxShape()
body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(0.1, 0.02, 0.1)
body_floor_shape.SetColor(chrono.ChColor(0.5,0.5,0.5))
body_floor.GetAssets().push_back(body_floor_shape)

my_system.Add(body_floor)
Example #13
0
    def reset(self):
        #print("reset")
        self.isdone = False
        self.rev_pend_sys.Clear()
        # create it
        self.body_rod = chrono.ChBody()
        # set initial position
        self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0))
        # set mass properties
        self.body_rod.SetMass(self.mass_rod)

        self.body_rod.SetInertiaXX(
            chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y,
                             self.inertia_rod_x))
        # set collision surface properties
        self.body_rod.SetMaterialSurface(self.rod_material)

        # Visualization shape, for rendering animation

        self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0)
        self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0)

        self.body_rod_shape = chrono.ChCylinderShape()
        self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1
        self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2
        self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod

        self.body_rod.AddAsset(self.body_rod_shape)
        self.rev_pend_sys.Add(self.body_rod)

        self.body_floor = chrono.ChBody()
        self.body_floor.SetBodyFixed(True)
        self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0))
        self.body_floor.SetMaterialSurface(self.rod_material)

        if self.render:
            self.body_floor_shape = chrono.ChBoxShape()
            self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(
                3, 1, 3)
            self.body_floor.GetAssets().push_back(self.body_floor_shape)
            self.body_floor_texture = chrono.ChTexture()
            self.body_floor_texture.SetTextureFilename(
                '../../../data/concrete.jpg')
            self.body_floor.GetAssets().push_back(self.body_floor_texture)

        self.rev_pend_sys.Add(self.body_floor)

        self.body_table = chrono.ChBody()
        self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0))
        self.body_table.SetMaterialSurface(self.rod_material)

        if self.render:
            self.body_table_shape = chrono.ChBoxShape()
            self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD(
                self.size_table_x / 2, self.size_table_y / 2,
                self.size_table_z / 2)
            self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
            self.body_table.GetAssets().push_back(self.body_table_shape)

            self.body_table_texture = chrono.ChTexture()
            self.body_table_texture.SetTextureFilename(
                '../../../data/concrete.jpg')
            self.body_table.GetAssets().push_back(self.body_table_texture)
        self.body_table.SetMass(0.1)
        self.rev_pend_sys.Add(self.body_table)

        self.link_slider = chrono.ChLinkLockPrismatic()
        z2x = chrono.ChQuaternionD()
        z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0))

        self.link_slider.Initialize(
            self.body_table, self.body_floor,
            chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x))
        self.rev_pend_sys.Add(self.link_slider)

        self.act_initpos = chrono.ChVectorD(0, 0, 0)
        self.actuator = chrono.ChLinkMotorLinearForce()
        self.actuator.Initialize(self.body_table, self.body_floor,
                                 chrono.ChFrameD(self.act_initpos))
        self.rev_pend_sys.Add(self.actuator)

        self.rod_pin = chrono.ChMarker()
        self.body_rod.AddMarker(self.rod_pin)
        self.rod_pin.Impose_Abs_Coord(
            chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))

        self.table_pin = chrono.ChMarker()
        self.body_table.AddMarker(self.table_pin)
        self.table_pin.Impose_Abs_Coord(
            chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))

        self.pin_joint = chrono.ChLinkLockRevolute()
        self.pin_joint.Initialize(self.rod_pin, self.table_pin)
        self.rev_pend_sys.Add(self.pin_joint)

        if self.render:

            # ---------------------------------------------------------------------
            #
            #  Create an Irrlicht application to visualize the system
            #
            # ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items
            # in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes.
            # If you need a finer control on which item really needs a visualization proxy
            # Irrlicht, just use application.AssetBind(myitem); on a per-item basis.

            self.myapplication.AssetBindAll()

            # ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
            # that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!

            self.myapplication.AssetUpdateAll()

        self.isdone = False
        self.steps = 0
        self.step(np.array([[0]]))
        return self.get_ob()
Example #14
0
    def reset(self):
        #print("reset")
        self.isdone = False
        self.rev_pend_sys.Clear()
        # create it
        self.body_rod = chrono.ChBody()
        # set initial position
        self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0))
        # set mass properties
        self.body_rod.SetMass(self.mass_rod)

        self.body_rod.SetInertiaXX(
            chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y,
                             self.inertia_rod_x))
        # set collision surface properties
        self.body_rod.SetMaterialSurface(self.rod_material)

        self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0)
        self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0)

        self.body_rod_shape = chrono.ChCylinderShape()
        self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1
        self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2
        self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod

        self.body_rod.AddAsset(self.body_rod_shape)
        self.rev_pend_sys.Add(self.body_rod)

        self.body_floor = chrono.ChBody()
        self.body_floor.SetBodyFixed(True)
        self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0))
        self.body_floor.SetMaterialSurface(self.rod_material)
        self.body_floor_shape = chrono.ChBoxShape()
        self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(3, 1, 3)
        self.body_floor.GetAssets().push_back(self.body_floor_shape)
        self.body_floor_texture = chrono.ChTexture()
        self.body_floor_texture.SetTextureFilename(chrono.GetChronoDataPath() +
                                                   '/concrete.jpg')
        self.body_floor.GetAssets().push_back(self.body_floor_texture)

        self.rev_pend_sys.Add(self.body_floor)

        self.body_table = chrono.ChBody()
        self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0))
        self.body_table.SetMaterialSurface(self.rod_material)

        self.body_table.SetMass(0.1)
        self.body_table_shape = chrono.ChBoxShape()
        self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD(
            self.size_table_x / 2, self.size_table_y / 2,
            self.size_table_z / 2)
        self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
        self.body_table.GetAssets().push_back(self.body_table_shape)
        self.body_table_texture = chrono.ChTexture()
        self.body_table_texture.SetTextureFilename(chrono.GetChronoDataPath() +
                                                   '/concrete.jpg')
        self.body_table.GetAssets().push_back(self.body_table_texture)
        self.rev_pend_sys.Add(self.body_table)

        self.link_slider = chrono.ChLinkLockPrismatic()
        z2x = chrono.ChQuaternionD()
        z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0))

        self.link_slider.Initialize(
            self.body_table, self.body_floor,
            chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x))
        self.rev_pend_sys.Add(self.link_slider)

        self.act_initpos = chrono.ChVectorD(0, 0, 0)
        self.actuator = chrono.ChLinkMotorLinearForce()
        self.actuator.Initialize(self.body_table, self.body_floor,
                                 chrono.ChFrameD(self.act_initpos))
        self.rev_pend_sys.Add(self.actuator)

        self.rod_pin = chrono.ChMarker()
        self.body_rod.AddMarker(self.rod_pin)
        self.rod_pin.Impose_Abs_Coord(
            chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))

        self.table_pin = chrono.ChMarker()
        self.body_table.AddMarker(self.table_pin)
        self.table_pin.Impose_Abs_Coord(
            chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))

        self.pin_joint = chrono.ChLinkLockRevolute()
        self.pin_joint.Initialize(self.rod_pin, self.table_pin)
        self.rev_pend_sys.Add(self.pin_joint)

        if self.render_setup:
            self.myapplication.AssetBindAll()
            self.myapplication.AssetUpdateAll()

        self.isdone = False
        self.steps = 0
        self.step(np.array([[0]]))
        return self.get_ob()
Example #15
0
#
# Recall that Irrlicht uses a left-hand frame, so everything is rendered with
# left and right flipped.
#
# =============================================================================

import pychrono as chrono
import pychrono.irrlicht as irr
import math as m

print("Copyright (c) 2017 projectchrono.org")

system = chrono.ChSystemNSC()

# Create ground body
ground = chrono.ChBody()
system.AddBody(ground)
ground.SetIdentifier(-1)
ground.SetBodyFixed(True)
ground.SetCollide(False)

# Visualization for revolute joint
cyl_rev = chrono.ChCylinderShape()
cyl_rev.GetCylinderGeometry().p1 = chrono.ChVectorD(0, 0, 0.2)
cyl_rev.GetCylinderGeometry().p2 = chrono.ChVectorD(0, 0, -0.2)
cyl_rev.GetCylinderGeometry().rad = 0.04
ground.AddAsset(cyl_rev)

# Create a pendulum body
pend = chrono.ChBody()
system.AddBody(pend)
Example #16
0
    def __init__(self, system, mesh, n_chains=6):
        self.bodies = []  #[chrono.ChBodyEasyBox for i in range(n_chains)]
        msection_cable2 = fea.ChBeamSectionCable()
        msection_cable2.SetDiameter(0.015)
        msection_cable2.SetYoungModulus(0.01e9)
        msection_cable2.SetBeamRaleyghDamping(0.000)

        mtruss = chrono.ChBody()
        mtruss.SetBodyFixed(True)

        for j in range(n_chains):
            builder = fea.ChBuilderCableANCF()

            # Now, simply use BuildBeam to create a beam from a poto another:
            builder.BuildBeam(
                mesh,  # the mesh where to put the created nodes and elements
                msection_cable2,  # ChBeamSectionCable to use for the ChElementBeamANCF elements
                1 + j,  # number of ChElementBeamANCF to create
                chrono.ChVectorD(0, 0, -0.1 * j),  # poA (beginning of beam)
                chrono.ChVectorD(0.1 + 0.1 * j, 0,
                                 -0.1 * j)  # poB (end of beam)
            )

            builder.GetLastBeamNodes().back().SetForce(
                chrono.ChVectorD(0, -0.2, 0))

            constraint_hinge = fea.ChLinkPointFrame()
            constraint_hinge.Initialize(builder.GetLastBeamNodes().front(),
                                        mtruss)
            system.Add(constraint_hinge)

            msphere = chrono.ChSphereShape()
            msphere.GetSphereGeometry().rad = 0.02
            constraint_hinge.AddAsset(msphere)

            # make a box and connect it
            mbox = chrono.ChBodyEasyBox(0.2, 0.04, 0.04, 1000)
            mbox.SetPos(builder.GetLastBeamNodes().back().GetPos() +
                        chrono.ChVectorD(0.1, 0, 0))
            system.Add(mbox)

            constraint_pos = fea.ChLinkPointFrame()
            constraint_pos.Initialize(builder.GetLastBeamNodes().back(), mbox)
            system.Add(constraint_pos)

            constraint_dir = fea.ChLinkDirFrame()
            constraint_dir.Initialize(builder.GetLastBeamNodes().back(), mbox)
            constraint_dir.SetDirectionInAbsoluteCoords(
                chrono.ChVectorD(1, 0, 0))
            system.Add(constraint_dir)

            # make another beam
            builder.BuildBeam(
                mesh,  # mesh where to put the created nodes and elements
                msection_cable2,  # ChBeamSectionCable to use for the ChElementBeamANCF elements
                1 + (n_chains - j),  # number of ChElementBeamANCF to create
                chrono.ChVectorD(mbox.GetPos().x + 0.1, 0,
                                 -0.1 * j),  # poA (beginning of beam)
                chrono.ChVectorD(mbox.GetPos().x + 0.1 + 0.1 * (n_chains - j),
                                 0, -0.1 * j)  # poB (end of beam)
            )

            constraint_pos2 = fea.ChLinkPointFrame()
            constraint_pos2.Initialize(builder.GetLastBeamNodes().front(),
                                       mbox)
            system.Add(constraint_pos2)

            constraint_dir2 = fea.ChLinkDirFrame()
            constraint_dir2.Initialize(builder.GetLastBeamNodes().front(),
                                       mbox)
            constraint_dir2.SetDirectionInAbsoluteCoords(
                chrono.ChVectorD(1, 0, 0))
            system.Add(constraint_dir2)

            # make a box and connect it
            self.bodies.append(chrono.ChBodyEasyBox(0.2, 0.04, 0.04, 1000))
            self.bodies[j].SetPos(builder.GetLastBeamNodes().back().GetPos() +
                                  chrono.ChVectorD(0.1, 0, 0))
            system.Add(self.bodies[j])

            constraint_pos3 = fea.ChLinkPointFrame()
            constraint_pos3.Initialize(builder.GetLastBeamNodes().back(),
                                       self.bodies[j])
            system.Add(constraint_pos3)

            constraint_dir3 = fea.ChLinkDirFrame()
            constraint_dir3.Initialize(builder.GetLastBeamNodes().back(),
                                       self.bodies[j])
            constraint_dir3.SetDirectionInAbsoluteCoords(
                chrono.ChVectorD(1, 0, 0))
            system.Add(constraint_dir3)
Example #17
0
   def reset(self):
    
      self.isdone = False
      self.ant_sys.Clear()
      self.body_abdomen = chrono.ChBody()
      self.body_abdomen.SetPos(chrono.ChVectorD(0, self.abdomen_y0, 0 ))
      self.body_abdomen.SetMass(self.abdomen_mass)
      self.body_abdomen.SetInertiaXX(self.abdomen_inertia)
    # set collision surface properties
      self.body_abdomen.SetMaterialSurface(self.ant_material)
      abdomen_ellipsoid = chrono.ChEllipsoid(chrono.ChVectorD(0, 0, 0 ), chrono.ChVectorD(self.abdomen_x, self.abdomen_y, self.abdomen_z ))
      self.abdomen_shape = chrono.ChEllipsoidShape(abdomen_ellipsoid)
      self.body_abdomen.AddAsset(self.abdomen_shape)
      self.body_abdomen.SetMaterialSurface(self.ant_material)
      self.body_abdomen.SetCollide(True)
      self.body_abdomen.GetCollisionModel().ClearModel()
      self.body_abdomen.GetCollisionModel().AddEllipsoid(self.abdomen_x, self.abdomen_y, self.abdomen_z, chrono.ChVectorD(0, 0, 0 ) )
      self.body_abdomen.GetCollisionModel().BuildModel()
      self.ant_sys.Add(self.body_abdomen)
      
      
      leg_ang =  (1/4)*math.pi+(1/2)*math.pi*np.array([0,1,2,3])
      Leg_quat = [chrono.ChQuaternionD() for i in range(len(leg_ang))]
      self.leg_body = [chrono.ChBody() for i in range(len(leg_ang))]
      self.leg_pos= [chrono.ChVectorD() for i in range(len(leg_ang))]
      leg_cyl = chrono.ChCylinder(-chrono.ChVectorD( self.leg_length/2,  0  ,0),chrono.ChVectorD( self.leg_length/2,  0  ,0), self.leg_radius) 
      self.leg_shape = chrono.ChCylinderShape(leg_cyl)
      ankle_cyl = chrono.ChCylinder(-chrono.ChVectorD( self.ankle_length/2,  0  ,0),chrono.ChVectorD( self.ankle_length/2,  0  ,0), self.ankle_radius) 
      self.ankle_shape = chrono.ChCylinderShape(ankle_cyl)
      foot_sphere = chrono.ChSphere(chrono.ChVectorD(self.ankle_length/2, 0, 0 ), self.ankle_radius )
      self.foot_shape = chrono.ChSphereShape(foot_sphere)
      Leg_qa = [ chrono.ChQuaternionD()  for i in range(len(leg_ang))]
      Leg_q = [ chrono.ChQuaternionD()  for i in range(len(leg_ang))]
      z2x_leg = [ chrono.ChQuaternionD() for i in range(len(leg_ang))]
      Leg_rev_pos=[]
      Leg_chordsys = []
      self.legjoint_frame = []
      x_rel = []
      z_rel = []
      self.Leg_rev = [chrono.ChLinkLockRevolute() for i in range(len(leg_ang))]
      self.leg_motor = [chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ]
      #ankle lists
      anklejoint_chordsys = []
      self.anklejoint_frame = []
      self.ankleCOG_frame = []
      q_ankle_zrot = [ chrono.ChQuaternionD() for i in range(len(leg_ang))]
      self.ankle_body = [chrono.ChBody() for i in range(len(leg_ang))]
      self.Ankle_rev = [chrono.ChLinkLockRevolute() for i in range(len(leg_ang))]
      self.ankle_motor = [chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ]
      for i in range(len(leg_ang)):
             
             # Legs
             Leg_quat[i].Q_from_AngAxis(-leg_ang[i] , chrono.ChVectorD(0, 1, 0))
             self.leg_pos[i] = chrono.ChVectorD( (0.5*self.leg_length+self.abdomen_x)*math.cos(leg_ang[i]) ,self.abdomen_y0, (0.5*self.leg_length+self.abdomen_z)*math.sin(leg_ang[i]))
             self.leg_body[i].SetPos(self.leg_pos[i])
             self.leg_body[i].SetRot(Leg_quat[i])
             self.leg_body[i].AddAsset(self.leg_shape)
             self.leg_body[i].SetMass(self.leg_mass)
             self.leg_body[i].SetInertiaXX(self.leg_inertia)
             self.ant_sys.Add(self.leg_body[i])
             x_rel.append( Leg_quat[i].Rotate(chrono.ChVectorD(1, 0, 0)))
             z_rel.append( Leg_quat[i].Rotate(chrono.ChVectorD(0, 0, 1)))
             Leg_qa[i].Q_from_AngAxis(-leg_ang[i] , chrono.ChVectorD(0, 1, 0))
             z2x_leg[i].Q_from_AngAxis(chrono.CH_C_PI / 2 , x_rel[i])
             Leg_q[i] = z2x_leg[i] * Leg_qa[i] 
             Leg_rev_pos.append(chrono.ChVectorD(self.leg_pos[i]-chrono.ChVectorD(math.cos(leg_ang[i])*self.leg_length/2,0,math.sin(leg_ang[i])*self.leg_length/2)))
             Leg_chordsys.append(chrono.ChCoordsysD(Leg_rev_pos[i], Leg_q[i]))
             self.legjoint_frame.append(chrono.ChFrameD(Leg_chordsys[i]))
             self.Leg_rev[i].Initialize(self.body_abdomen, self.leg_body[i],Leg_chordsys[i])
             self.ant_sys.Add(self.Leg_rev[i])
             self.leg_motor[i].Initialize(self.body_abdomen, self.leg_body[i],self.legjoint_frame[i])
             self.ant_sys.Add(self.leg_motor[i])
             # Ankles
             q_ankle_zrot[i].Q_from_AngAxis(-self.ankle_angle , z_rel[i])
             anklejoint_chordsys.append(chrono.ChCoordsysD(self.leg_body[i].GetPos()+ self.leg_body[i].GetRot().Rotate(chrono.ChVectorD(self.leg_length/2, 0, 0)) , q_ankle_zrot[i] * self.leg_body[i].GetRot() ))
             self.anklejoint_frame.append(chrono.ChFrameD(anklejoint_chordsys[i]))
             self.ankle_body[i].SetPos(self.anklejoint_frame[i].GetPos() + self.anklejoint_frame[i].GetRot().Rotate(chrono.ChVectorD(self.ankle_length/2, 0, 0)))
             self.ankle_body[i].SetRot(  self.anklejoint_frame[i].GetRot() )
             self.ankle_body[i].AddAsset(self.ankle_shape)
             self.ankle_body[i].SetMass(self.ankle_mass)
             self.ankle_body[i].SetInertiaXX(self.ankle_inertia)
             self.ant_sys.Add(self.ankle_body[i])
             self.Ankle_rev[i].Initialize(self.leg_body[i], self.ankle_body[i], anklejoint_chordsys[i])
             self.ant_sys.Add(self.Ankle_rev[i])
             self.ankle_motor[i].Initialize(self.leg_body[i], self.ankle_body[i],self.anklejoint_frame[i])
             self.ant_sys.Add(self.ankle_motor[i])
             # Feet collisions
             self.ankle_body[i].SetMaterialSurface(self.ant_material)
             self.ankle_body[i].SetCollide(True)
             self.ankle_body[i].GetCollisionModel().ClearModel()
             self.ankle_body[i].GetCollisionModel().AddSphere(self.ankle_radius, chrono.ChVectorD(self.ankle_length/2, 0, 0 ) )
             self.ankle_body[i].GetCollisionModel().BuildModel()
             self.ankle_body[i].AddAsset(self.ankle_shape)
             
             self.ankle_body[i].AddAsset(self.foot_shape)
             
             self.Leg_rev[i].GetLimit_Rz().SetActive(True)
             self.Leg_rev[i].GetLimit_Rz().SetMin(-math.pi/3)
             self.Leg_rev[i].GetLimit_Rz().SetMax(math.pi/3)
             self.Ankle_rev[i].GetLimit_Rz().SetActive(True)
             self.Ankle_rev[i].GetLimit_Rz().SetMin(-math.pi/2)
             self.Ankle_rev[i].GetLimit_Rz().SetMax(math.pi/4)
             

           
    # Create the room floor: a simple fixed rigid body with a collision shape
    # and a visualization shape
      self.body_floor = chrono.ChBody()
      self.body_floor.SetBodyFixed(True)
      self.body_floor.SetPos(chrono.ChVectorD(0, -1, 0 ))
      self.body_floor.SetMaterialSurface(self.ant_material)
      
      # Floor Collision.
      self.body_floor.SetMaterialSurface(self.ant_material)
      self.body_floor.GetCollisionModel().ClearModel()
      self.body_floor.GetCollisionModel().AddBox(50, 1, 50, chrono.ChVectorD(0, 0, 0 ))
      self.body_floor.GetCollisionModel().BuildModel()
      self.body_floor.SetCollide(True)

    # Visualization shape
      body_floor_shape = chrono.ChBoxShape()
      body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5)
      body_floor_shape.SetColor(chrono.ChColor(0.4,0.4,0.5))
      self.body_floor.GetAssets().push_back(body_floor_shape)
      body_floor_texture = chrono.ChTexture()
      body_floor_texture.SetTextureFilename(chrono.GetChronoDataFile('vehicle/terrain/textures/grass.jpg'))
      self.body_floor.GetAssets().push_back(body_floor_texture)     
      self.ant_sys.Add(self.body_floor)
      #self.body_abdomen.SetBodyFixed(True)
   
      if (self.animate):
            self.myapplication.AssetBindAll()
            self.myapplication.AssetUpdateAll()

      self.numsteps= 0
      self.step(np.zeros(8))
      return self.get_ob()
Example #18
0
# Created:     1/01/2019
# Copyright:   (c) ProjectChrono 2019
#------------------------------------------------------------------------------

print ("Second tutorial: create and populate a physical system");


# Load the Chrono::Engine core module!
import pychrono as chrono


# Create a physical system,
my_system = chrono.ChSystemNSC()

# Add two bodies
my_shbodyA = chrono.ChBody()
my_shbodyA.SetMass(20)
my_shbodyA.SetName('BodyA')
my_shbodyA.SetInertiaXX( chrono.ChVectorD(10,10,10) )
print (my_shbodyA.GetInertia() )
my_shbodyA.SetPos(chrono.ChVectorD(1,-1,0))
my_shbodyA.GetCollisionModel().AddBox(10,1,10)
my_shbodyA.SetBodyFixed(True)
my_shbodyA.SetCollide(True)

my_shbodyB = chrono.ChBody()
my_shbodyB.SetName('BodyB')
my_shbodyB.SetPos(chrono.ChVectorD(0,2,0))
my_shbodyB.GetCollisionModel().AddBox(1,1,1)
my_shbodyB.SetCollide(True)
Example #19
0

# =============================================================================

print("Copyright (c) 2017 projectchrono.org")

system = chrono.ChSystemNSC()
system.Set_G_acc(chrono.ChVectorD(0, 0, 0))

# Revolute joint frame
rev_rot = chrono.Q_from_AngX(m.pi / 6.0)
rev_dir = rev_rot.GetZaxis()
rev_pos = chrono.ChVectorD(+1, 0, 0)

# Create ground body
ground = chrono.ChBody()
system.AddBody(ground)
ground.SetIdentifier(-1)
ground.SetBodyFixed(True)
ground.SetCollide(False)

# Visualization for revolute joint
cyl_rev = chrono.ChCylinderShape()
cyl_rev.GetCylinderGeometry().p1 = rev_pos + rev_dir * 0.2
cyl_rev.GetCylinderGeometry().p2 = rev_pos - rev_dir * 0.2
cyl_rev.GetCylinderGeometry().rad = 0.1
ground.AddAsset(cyl_rev)

# Offset from joint to body COM
offset = chrono.ChVectorD(1.5, 0, 0)
Example #20
0
    def reset(self):

        self.isdone = False
        self.ant_sys.Clear()
        self.body_abdomen = chrono.ChBody()
        self.body_abdomen.SetPos(chrono.ChVectorD(0, self.abdomen_y0, 0))
        self.body_abdomen.SetMass(self.abdomen_mass)
        self.body_abdomen.SetInertiaXX(self.abdomen_inertia)
        # set collision surface properties
        self.body_abdomen.SetMaterialSurface(self.ant_material)
        abdomen_ellipsoid = chrono.ChEllipsoid(
            chrono.ChVectorD(0, 0, 0),
            chrono.ChVectorD(self.abdomen_x, self.abdomen_y, self.abdomen_z))
        self.abdomen_shape = chrono.ChEllipsoidShape(abdomen_ellipsoid)
        self.body_abdomen.AddAsset(self.abdomen_shape)
        self.body_abdomen.SetMaterialSurface(self.ant_material)
        self.body_abdomen.SetCollide(True)
        self.body_abdomen.GetCollisionModel().ClearModel()
        self.body_abdomen.GetCollisionModel().AddEllipsoid(
            self.abdomen_x, self.abdomen_y, self.abdomen_z,
            chrono.ChVectorD(0, 0, 0))
        self.body_abdomen.GetCollisionModel().BuildModel()
        self.ant_sys.Add(self.body_abdomen)

        tilt_factor = 1.1  # no tilt; higher number tilts body toward positive x
        leg_ang = np.array([
            tilt_factor * math.pi / 2, tilt_factor * math.pi / 2
        ])  #(1/4)*math.pi+(1/2)*math.pi*np.array([0,1])
        Leg_quat = [chrono.ChQuaternionD() for i in range(len(leg_ang))]
        self.leg_body = [chrono.ChBody() for i in range(len(leg_ang))]
        self.leg_pos = [chrono.ChVectorD() for i in range(len(leg_ang))]
        leg_cyl = chrono.ChCylinder(
            -chrono.ChVectorD(self.leg_length / 2, 0, 0),
            chrono.ChVectorD(self.leg_length / 2, 0, 0), self.leg_radius)
        self.leg_shape = chrono.ChCylinderShape(leg_cyl)
        ankle_cyl = chrono.ChCylinder(
            -chrono.ChVectorD(self.ankle_length / 2, 0, 0),
            chrono.ChVectorD(self.ankle_length / 2, 0, 0), self.ankle_radius)
        self.ankle_shape = chrono.ChCylinderShape(ankle_cyl)
        foot_sphere = chrono.ChSphere(
            chrono.ChVectorD(self.ankle_length / 2, 0, 0), self.ankle_radius)
        self.foot_shape = chrono.ChSphereShape(foot_sphere)
        Leg_qa = [chrono.ChQuaternionD() for i in range(len(leg_ang))]
        Leg_q = [chrono.ChQuaternionD() for i in range(len(leg_ang))]
        z2x_leg = [chrono.ChQuaternionD() for i in range(len(leg_ang))]
        Leg_rev_pos = []
        Leg_chordsys = []
        self.legjoint_frame = []
        x_rel = []
        z_rel = []
        self.Leg_rev = [
            chrono.ChLinkLockRevolute() for i in range(len(leg_ang))
        ]
        self.leg_motor = [
            chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang))
        ]
        #ankle lists
        anklejoint_chordsys = []
        self.anklejoint_frame = []
        self.ankleCOG_frame = []
        q_ankle_zrot = [chrono.ChQuaternionD() for i in range(len(leg_ang))]
        self.ankle_body = [chrono.ChBody() for i in range(len(leg_ang))]
        self.Ankle_rev = [
            chrono.ChLinkLockRevolute() for i in range(len(leg_ang))
        ]
        self.ankle_motor = [
            chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang))
        ]
        for i in range(len(leg_ang)):

            # Legs
            Leg_quat[i].Q_from_AngAxis(-leg_ang[i], chrono.ChVectorD(0, 0, 1))
            # postion of leg CM from abdomen
            self.leg_pos[i] = chrono.ChVectorD(
                (0.5 * self.leg_length * math.cos(leg_ang[i])),
                (self.abdomen_y0 - self.leg_length / 2 * math.sin(leg_ang[i]) -
                 self.abdomen_y * math.cos(self.CMtohip_angle)),
                (-1)**i * self.abdomen_z * math.sin(self.CMtohip_angle))
            self.leg_body[i].SetPos(self.leg_pos[i])
            self.leg_body[i].SetRot(Leg_quat[i])
            self.leg_body[i].AddAsset(self.leg_shape)
            self.leg_body[i].SetMass(self.leg_mass)
            self.leg_body[i].SetInertiaXX(self.leg_inertia)
            self.ant_sys.Add(self.leg_body[i])
            x_rel.append(Leg_quat[i].Rotate(chrono.ChVectorD(1, 0, 0)))
            z_rel.append(Leg_quat[i].Rotate(chrono.ChVectorD(0, 0, 1)))
            Leg_qa[i].Q_from_AngAxis(-leg_ang[i], chrono.ChVectorD(0, 1, 0))
            z2x_leg[i].Q_from_AngAxis(chrono.CH_C_PI / 2, x_rel[i])
            Leg_q[i] = z2x_leg[i] * Leg_qa[i]
            Leg_rev_pos.append(chrono.ChVectorD(0, self.abdomen_y0,
                                                0))  # connection to abdomen
            Leg_chordsys.append(chrono.ChCoordsysD(Leg_rev_pos[i], Leg_q[i]))
            self.legjoint_frame.append(chrono.ChFrameD(Leg_chordsys[i]))
            self.Leg_rev[i].Initialize(self.body_abdomen, self.leg_body[i],
                                       Leg_chordsys[i])
            self.ant_sys.Add(self.Leg_rev[i])
            self.leg_motor[i].Initialize(self.body_abdomen, self.leg_body[i],
                                         self.legjoint_frame[i])
            self.ant_sys.Add(self.leg_motor[i])
            # Ankles
            q_ankle_zrot[i].Q_from_AngAxis(-self.ankle_angle, z_rel[i])
            anklejoint_chordsys.append(
                chrono.ChCoordsysD(
                    self.leg_body[i].GetPos() +
                    self.leg_body[i].GetRot().Rotate(
                        chrono.ChVectorD(self.leg_length / 2, 0, 0)),
                    q_ankle_zrot[i] * self.leg_body[i].GetRot()))
            self.anklejoint_frame.append(
                chrono.ChFrameD(anklejoint_chordsys[i]))
            self.ankle_body[i].SetPos(
                self.anklejoint_frame[i].GetPos() +
                self.anklejoint_frame[i].GetRot().Rotate(
                    chrono.ChVectorD(self.ankle_length / 2, 0, 0)))
            self.ankle_body[i].SetRot(self.anklejoint_frame[i].GetRot())
            self.ankle_body[i].AddAsset(self.ankle_shape)
            self.ankle_body[i].SetMass(self.ankle_mass)
            self.ankle_body[i].SetInertiaXX(self.ankle_inertia)
            self.ant_sys.Add(self.ankle_body[i])
            self.Ankle_rev[i].Initialize(self.leg_body[i], self.ankle_body[i],
                                         anklejoint_chordsys[i])
            self.ant_sys.Add(self.Ankle_rev[i])
            self.ankle_motor[i].Initialize(self.leg_body[i],
                                           self.ankle_body[i],
                                           self.anklejoint_frame[i])
            self.ant_sys.Add(self.ankle_motor[i])
            # Feet collisions
            self.ankle_body[i].SetMaterialSurface(self.ant_material)
            self.ankle_body[i].SetCollide(True)
            self.ankle_body[i].GetCollisionModel().ClearModel()
            self.ankle_body[i].GetCollisionModel().AddSphere(
                self.ankle_radius, chrono.ChVectorD(self.ankle_length / 2, 0,
                                                    0))
            self.ankle_body[i].GetCollisionModel().BuildModel()
            self.ankle_body[i].AddAsset(self.ankle_shape)
            self.ankle_body[i].AddAsset(self.foot_shape)

            # Limits need to turn back to true
            self.Leg_rev[i].GetLimit_Rz().SetActive(True)
            self.Leg_rev[i].GetLimit_Rz().SetMin(-math.pi / 3)
            self.Leg_rev[i].GetLimit_Rz().SetMax(math.pi / 3)
            self.Ankle_rev[i].GetLimit_Rz().SetActive(True)
            self.Ankle_rev[i].GetLimit_Rz().SetMin(-math.pi / 3)
            self.Ankle_rev[i].GetLimit_Rz().SetMax(math.pi / 3)

    # Create the room floor: a simple fixed rigid body with a collision shape
    # and a visualization shape
        self.body_floor = chrono.ChBody()
        self.body_floor.SetBodyFixed(True)
        self.body_floor.SetPos(
            chrono.ChVectorD(
                0, -self.abdomen_y0 -
                self.abdomen_y * math.cos(self.CMtohip_angle) -
                self.ankle_radius, 0))
        self.body_floor.SetMaterialSurface(self.ant_material)

        # Floor Collision.
        self.body_floor.SetMaterialSurface(self.ant_material)
        self.body_floor.GetCollisionModel().ClearModel()
        self.body_floor.GetCollisionModel().AddBox(50, 1, 50,
                                                   chrono.ChVectorD(0, 0, 0))
        self.body_floor.GetCollisionModel().BuildModel()
        self.body_floor.SetCollide(True)

        # Visualization shape
        body_floor_shape = chrono.ChBoxShape()
        body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5)
        body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
        self.body_floor.GetAssets().push_back(body_floor_shape)
        body_floor_texture = chrono.ChTexture()
        body_floor_texture.SetTextureFilename('../data/grass.jpg')
        self.body_floor.GetAssets().push_back(body_floor_texture)
        self.ant_sys.Add(self.body_floor)

        # this was commented out to not fix the segments
        # this is helpful to check geometry of humanoid
        #self.body_abdomen.SetBodyFixed(True)
        #self.leg_body[0].SetBodyFixed(True)
        #self.leg_body[1].SetBodyFixed(True)
        #self.ankle_body[0].SetBodyFixed(True)
        #self.ankle_body[1].SetBodyFixed(True)

        if (self.animate):
            self.myapplication.AssetBindAll()
            self.myapplication.AssetUpdateAll()

        self.numsteps = 0
        self.step(np.zeros(4))
        return self.get_ob()
Example #21
0
body_belt.SetCoord(my_marker.GetAbsCoord())

# note: the CAD reference was on surface, so we must move down COG that is at middle of surface:
#body_belt.ConcatenatePreTransformation(chrono.ChFrameMovingD(chrono.ChVectorD(0, -0.005-conv_thick/2., 0)))


my_system.Add(body_belt)





# Create the room floor: a simple fixed rigid body with a collision shape
# and a visualization shape

body_floor = chrono.ChBody()
body_floor.SetBodyFixed(True)
body_floor.SetPos(chrono.ChVectorD(0, -1, 0 ))
body_floor.SetMaterialSurface(pebble_material)

# Collision shape (shared by all particle clones)
body_floor.GetCollisionModel().ClearModel()
body_floor.GetCollisionModel().AddBox(3, 1, 3) # hemi sizes
body_floor.GetCollisionModel().BuildModel()
body_floor.SetCollide(True)

# Visualization shape (shared by all particle clones)
body_floor_shape = chrono.ChBoxShape()
body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(3, 1, 3)
body_floor.GetAssets().push_back(body_floor_shape)
body_floor_texture = chrono.ChTexture()
Example #22
0
        force = -spring_coef * (length - rest_length) - damping_coef * vel
        return force


# =============================================================================

print("Copyright (c) 2017 projectchrono.org")

system = chrono.ChSystemNSC()
system.Set_G_acc(chrono.ChVectorD(0, 0, 0))

# Create the ground body with two visualization spheres
# -----------------------------------------------------

ground = chrono.ChBody()
system.AddBody(ground)
ground.SetIdentifier(-1)
ground.SetBodyFixed(True)
ground.SetCollide(False)
sph_1 = chrono.ChSphereShape()
sph_1.GetSphereGeometry().rad = 0.1
sph_1.Pos = chrono.ChVectorD(-1, 0, 0)
ground.AddAsset(sph_1)

sph_2 = chrono.ChSphereShape()
sph_2.GetSphereGeometry().rad = 0.1
sph_2.Pos = chrono.ChVectorD(1, 0, 0)
ground.AddAsset(sph_2)

# Create a body suspended through a ChLinkTSDA (default linear)
Example #23
0
    def reset(self):
        #print("reset")
        self.isdone = False
        self.rev_pend_sys.Clear()
        # create it
        self.body_rod = chrono.ChBody()
        # set initial position
        self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0))
        # set mass properties
        self.body_rod.SetMass(self.mass_rod)
        # una volta che le hao calcolate inserisci inerzie diverse
        self.body_rod.SetInertiaXX(
            chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y,
                             self.inertia_rod_x))
        # set collision surface properties
        self.body_rod.SetMaterialSurface(self.rod_material)

        # Visualization shape, for rendering animation
        # vettori visual cilindro
        self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0)
        self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0)
        #body_rod_shape = chrono.ChCylinder(cyl_base1, cyl_base2, radius_rod)
        self.body_rod_shape = chrono.ChCylinderShape()
        self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1
        self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2
        self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod
        #body_rod.GetAssets().push_back(body_rod_shape)
        self.body_rod.AddAsset(self.body_rod_shape)
        self.rev_pend_sys.Add(self.body_rod)

        self.body_floor = chrono.ChBody()
        self.body_floor.SetBodyFixed(True)
        self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0))
        self.body_floor.SetMaterialSurface(self.rod_material)

        # Visualization shape
        if self.render:
            self.body_floor_shape = chrono.ChBoxShape()
            self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(
                3, 1, 3)
            self.body_floor.GetAssets().push_back(self.body_floor_shape)
            self.body_floor_texture = chrono.ChTexture()
            self.body_floor_texture.SetTextureFilename(
                '../../../data/concrete.jpg')
            self.body_floor.GetAssets().push_back(self.body_floor_texture)

        self.rev_pend_sys.Add(self.body_floor)

        self.body_table = chrono.ChBody()
        self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0))
        self.body_table.SetMaterialSurface(self.rod_material)

        # Visualization shape
        if self.render:
            self.body_table_shape = chrono.ChBoxShape()
            self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD(
                self.size_table_x / 2, self.size_table_y / 2,
                self.size_table_z / 2)
            self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
            self.body_table.GetAssets().push_back(self.body_table_shape)

            self.body_table_texture = chrono.ChTexture()
            self.body_table_texture.SetTextureFilename(
                '../../../data/concrete.jpg')
        self.body_table.GetAssets().push_back(self.body_table_texture)
        self.body_table.SetMass(0.1)
        self.rev_pend_sys.Add(self.body_table)

        # Create a constraint that blocks free 3 x y z translations and 3 rx ry rz rotations
        # of the table respect to the floor, and impose that the relative imposed position
        # depends on a specified motion law.

        self.link_slider = chrono.ChLinkLockPrismatic()
        z2x = chrono.ChQuaternionD()
        z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0))

        self.link_slider.Initialize(
            self.body_table, self.body_floor,
            chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x))
        self.rev_pend_sys.Add(self.link_slider)
        self.link_slider.SetMotion_axis(chrono.ChVectorD(1, 0, 0))
        #attuatore lineare
        self.act_initpos = chrono.ChVectorD(0, 0, 0)
        self.actuator = chrono.ChLinkMotorLinearForce()
        self.actuator.Initialize(self.body_table, self.body_floor,
                                 chrono.ChFrameD(self.act_initpos))
        self.rev_pend_sys.Add(self.actuator)

        # ..create the function for imposed y vertical motion, etc.
        # tolta: solo traslazione asse z
        #mfunY = chrono.ChFunction_Sine(0,1.5,0.001)  # phase, frequency, amplitude
        #link_slider.SetMotion_Y(mfunY)

        # ..create the function for imposed z horizontal motion, etc.
        #self.mfunX = chrono.ChFunction_Sine(0,1.5,0.4)  # phase, frequency, amplitude
        #self.link_slider.SetMotion_X(self.action)

        # Note that you could use other types of ChFunction_ objects, or create
        # your custom function by class inheritance (see demo_python.py), or also
        # set a function for table rotation , etc.

        # REVLOLUTE JOINT:
        # create frames for the joint
        self.rod_pin = chrono.ChMarker()
        self.body_rod.AddMarker(self.rod_pin)
        self.rod_pin.Impose_Abs_Coord(
            chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))

        self.table_pin = chrono.ChMarker()
        self.body_table.AddMarker(self.table_pin)
        self.table_pin.Impose_Abs_Coord(
            chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))

        self.pin_joint = chrono.ChLinkLockRevolute()
        self.pin_joint.Initialize(self.rod_pin, self.table_pin)
        self.rev_pend_sys.Add(self.pin_joint)

        if self.render:

            # ---------------------------------------------------------------------
            #
            #  Create an Irrlicht application to visualize the system
            #
            # ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items
            # in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes.
            # If you need a finer control on which item really needs a visualization proxy
            # Irrlicht, just use application.AssetBind(myitem); on a per-item basis.

            self.myapplication.AssetBindAll()

            # ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
            # that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!

            self.myapplication.AssetUpdateAll()

            # If you want to show shadows because you used "AddLightWithShadow()'
            # you must remember this:

        self.isdone = False
        self.steps = 0
        self.step(np.array([[0]]))
        return self.get_ob()
Example #24
0
# Set solver settings
system.SetSolverMaxIterations(100)
system.SetSolverForceTolerance(0)

# --------------------------------------------------
# Create a contact material, shared among all bodies
# --------------------------------------------------

material = chrono.ChMaterialSurfaceSMC()
material.SetFriction(friction)

# ----------
# Add bodies
# ----------

container = chrono.ChBody()
system.Add(container)
container.SetPos(chrono.ChVectorD(0, 0, 0))
container.SetBodyFixed(True)
container.SetIdentifier(-1)

container.SetCollide(True)
container.GetCollisionModel().ClearModel()
chrono.AddBoxGeometry(container, material, chrono.ChVectorD(4, 0.5, 4), chrono.ChVectorD(0, -0.5, 0))
container.GetCollisionModel().BuildModel()

container.AddAsset(chrono.ChColorAsset(chrono.ChColor(0.4, 0.4, 0.4)))

box1 = chrono.ChBody()
box1.SetMass(10)
box1.SetInertiaXX(chrono.ChVectorD(1, 1, 1))
import pychrono.irrlicht as irr
import math as m

try:
    import numpy as np
    from numpy import linalg as LA
except ImportError:
    print("You need NumPy to run this demo!")

print("Copyright (c) 2017 projectchrono.org")

system = chrono.ChSystemNSC()
system.Set_G_acc(chrono.ChVectorD(0, 0, 0))

# Create the ground body
ground = chrono.ChBody()
system.AddBody(ground)
ground.SetIdentifier(-1)
ground.SetBodyFixed(True)
ground.SetCollide(False)

rail1 = chrono.ChBoxShape()
rail1.GetBoxGeometry().SetLengths(chrono.ChVectorD(8, 0.1, 0.1))
rail1.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, -1)
ground.AddAsset(rail1)

rail2 = chrono.ChBoxShape()
rail2.GetBoxGeometry().SetLengths(chrono.ChVectorD(8, 0.1, 0.1))
rail2.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, 1)
ground.AddAsset(rail2)
Example #26
0
    def reset(self):

        self.isdone = False
        self.robosystem.Clear()
        #action = (np.random.rand(6,)-0.5)*2
        #torques = np.multiply(action, self.maxT)
        self.exported_items = chrono.ImportSolidWorksSystem(self.fpath)
        self.csys = []
        self.frames = []
        self.revs = []
        self.motors = []
        self.limits = []

        for con, coi in zip(self.con_link, self.coi_link):
            indices = []
            for i in range(len(self.exported_items)):
                if con == self.exported_items[i].GetName(
                ) or coi == self.exported_items[i].GetName():
                    indices.append(i)
            rev = self.exported_items[indices[0]]
            af0 = rev.GetAssetsFrame()
            # Revolute joints and ChLinkMotorRotation are z oriented, while parallel is x oriented.
            # Event though this Frame won't be used anymore is good practice to create a copy before editing its value.
            af = chrono.ChFrameD(af0)
            af.SetRot(af0.GetRot() % chrono.Q_ROTATE_X_TO_Z)
            self.frames.append(af)
            for i in indices:
                del self.exported_items[i]

        # ADD IMPORTED ITEMS TO THE SYSTEM
        for my_item in self.exported_items:
            self.robosystem.Add(my_item)
        """
              $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR MARKERS AND GET RID OF EM $$$$$$$$ 
              """
        self.bodies = [
            self.robosystem.SearchBody(name) for name in self.bodiesNames
        ]
        self.hand = self.robosystem.SearchBody('Hand_base_and_p07-2')
        self.base = self.robosystem.SearchBody('Racer3_p01-3')
        self.biceps = self.robosystem.SearchBody('Racer3_p03-1')
        self.forearm = self.robosystem.SearchBody('Racer3_p05-1')
        self.finger1 = self.robosystem.SearchBody('HAND_e_finger-1')
        self.finger2 = self.robosystem.SearchBody('HAND_e_finger-2')

        for i in range(len(self.con_link)):
            revolute = chrono.ChLinkLockRevolute()
            cs = chrono.ChCoordsysD(self.frames[i].GetPos(),
                                    self.frames[i].GetRot())
            self.csys.append(cs)
            revolute.Initialize(self.bodies[i], self.bodies[i + 1],
                                self.csys[i])
            self.revs.append(revolute)
            self.robosystem.Add(self.revs[i])
            lim = self.revs[i].GetLimit_Rz()
            self.limits.append(lim)
            self.limits[i].SetActive(True)
            self.limits[i].SetMin(self.minRot[i] * (math.pi / 180))
            self.limits[i].SetMax(self.maxRot[i] * (math.pi / 180))

            m = chrono.ChLinkMotorRotationTorque()
            m.Initialize(self.bodies[i], self.bodies[i + 1], self.frames[i])
            #self.robosystem.Add(m2)
            #m2.SetTorqueFunction(chrono.ChFunction_Const(5))
            self.motors.append(m)
            #self.motors[i].SetTorqueFunction(chrono.ChFunction_Const(float(torques[i])))
            self.robosystem.Add(self.motors[i])

        self.body_floor = chrono.ChBody()
        self.body_floor.SetBodyFixed(True)
        self.body_floor.SetPos(chrono.ChVectorD(0, -1, 0))

        # Floor Collision.
        self.body_floor.GetCollisionModel().ClearModel()
        self.body_floor.GetCollisionModel().AddBox(self.my_material, 5, 1, 5,
                                                   chrono.ChVectorD(0, 0, 0))
        self.body_floor.GetCollisionModel().BuildModel()
        self.body_floor.SetCollide(True)

        # Visualization shape
        body_floor_shape = chrono.ChBoxShape()
        body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5)
        body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
        self.body_floor.GetAssets().push_back(body_floor_shape)
        body_floor_texture = chrono.ChTexture()
        texpath = os.path.join(chrono.GetChronoDataPath(), 'concrete.jpg')
        body_floor_texture.SetTextureFilename(texpath)
        self.body_floor.GetAssets().push_back(body_floor_texture)
        self.robosystem.Add(self.body_floor)
        r = np.random.rand(2, ) - np.asarray([0.5, 0.5])
        self.targ_init_pos = [
            -0.52 + 2 * (r[0] * 0.05), 0.015, 2 * r[1] * 0.05
        ]
        self.targ_box = chrono.ChBody()
        # UNset to grasp
        self.targ_box.SetBodyFixed(True)
        self.targ_box.SetPos(
            chrono.ChVectorD(self.targ_init_pos[0], self.targ_init_pos[1],
                             self.targ_init_pos[2]))
        # Floor Collision.
        self.targ_box.GetCollisionModel().ClearModel()
        self.targ_box.GetCollisionModel().AddBox(self.my_material, 0.015,
                                                 0.015, 0.015,
                                                 chrono.ChVectorD(0, 0, 0))
        self.targ_box.GetCollisionModel().BuildModel()
        self.targ_box.SetCollide(True)
        # Visualization shape
        targ_box_shape = chrono.ChBoxShape()
        targ_box_shape.GetBoxGeometry().Size = chrono.ChVectorD(
            0.015, 0.015, 0.015)
        col = chrono.ChColorAsset()
        col.SetColor(chrono.ChColor(1.0, 0, 0))
        self.targ_box.GetAssets().push_back(targ_box_shape)
        self.targ_box.GetAssets().push_back(col)
        self.robosystem.Add(self.targ_box)

        self.numsteps = 0

        if (self.render_setup):
            self.myapplication.AssetBindAll()
            self.myapplication.AssetUpdateAll()
        return self.get_ob()
Example #27
0
mbodyflywheel = chrono.ChBodyEasyCylinder(0.24, 0.05, 7800)  # R, h, density
mbodyflywheel.SetCoord(
    chrono.ChCoordsysD(
        node_mid.GetPos() + chrono.ChVectorD(
            0, 0.05, 0),  # flywheel initial center (plus Y offset)
        chrono.Q_from_AngAxis(CH_C_PI / 2.0, chrono.VECT_Z)
    )  # flywheel initial alignment (rotate 90° so cylinder axis is on X)
)
my_system.Add(mbodyflywheel)

myjoint = chrono.ChLinkMateFix()
myjoint.Initialize(node_mid, mbodyflywheel)
my_system.Add(myjoint)

# Create the truss
truss = chrono.ChBody()
truss.SetBodyFixed(True)
my_system.Add(truss)

# Create the end bearing
bearing = chrono.ChLinkMateGeneric(False, True, True, False, True, True)
bearing.Initialize(builder.GetLastBeamNodes().back(), truss,
                   chrono.ChFrameD(builder.GetLastBeamNodes().back().GetPos()))
my_system.Add(bearing)

# Create the motor that rotates the beam
rotmotor1 = chrono.ChLinkMotorRotationSpeed()

# Connect the rotor and the stator and add the motor to the system:
rotmotor1.Initialize(
    builder.GetLastBeamNodes().front(),  # body A (slave)
Example #28
0
print("Second tutorial: create and populate a physical system")

# Load the Chrono::Engine core module!
import pychrono as chrono

# Create a physical system,
my_system = chrono.ChSystemNSC()

# Create a contact material, shared by all collision shapes
material = chrono.ChMaterialSurfaceNSC()
material.SetFriction(0.3)
material.SetCompliance(0)

# Add two bodies
bodyA = chrono.ChBody()
bodyA.SetMass(20)
bodyA.SetName('BodyA')
bodyA.SetInertiaXX(chrono.ChVectorD(10, 10, 10))
print(bodyA.GetInertia())
bodyA.SetPos(chrono.ChVectorD(1, -1, 0))
bodyA.GetCollisionModel().AddBox(material, 10, 1, 10)
bodyA.SetBodyFixed(True)
bodyA.SetCollide(True)

bodyB = chrono.ChBody()
bodyB.SetName('BodyB')
bodyB.SetPos(chrono.ChVectorD(0, 2, 0))
bodyB.GetCollisionModel().AddBox(material, 1, 1, 1)
bodyB.SetCollide(True)
## 1. Create the physical system that will handle all bodies and constraints.

##    Specify the gravitational acceleration vector, consistent with the
##    global reference frame having Z up.
system = chrono.ChSystemNSC()
system.Set_G_acc(chrono.ChVectorD(0, 0, -9.81))

## 2. Create the rigid bodies of the slider-crank mechanical system.
##    For each body, specify:
##    - a unique identifier
##    - mass and moments of inertia
##    - position and orientation of the (centroidal) body frame
##    - visualization assets (defined with respect to the body frame)

## Ground
ground = chrono.ChBody()
system.AddBody(ground)
ground.SetIdentifier(-1)
ground.SetName("ground")
ground.SetBodyFixed(True)

cyl_g = chrono.ChCylinderShape()
cyl_g.GetCylinderGeometry().p1 = chrono.ChVectorD(0, 0.2, 0)
cyl_g.GetCylinderGeometry().p2 = chrono.ChVectorD(0, -0.2, 0)
cyl_g.GetCylinderGeometry().rad = 0.03
ground.AddAsset(cyl_g)

col_g = chrono.ChColorAsset()
col_g.SetColor(chrono.ChColor(0.6, 0.6, 0.2))
ground.AddAsset(col_g)
application.AddTypicalLogo()
application.AddTypicalSky()
application.AddTypicalLights()
application.AddTypicalCamera(chronoirr.vector3df(0.0, 0.6, -1.0))

L = 1
H = 0.25
K = 0.05
vA = chrono.ChVectorD(0, 0, 0)
vC = chrono.ChVectorD(L, 0, 0)
vB = chrono.ChVectorD(L, -H, 0)
vG = chrono.ChVectorD(L - K, -H, 0)
vd = chrono.ChVectorD(0, 0, 0.0001)

# Create a truss:
body_truss = chrono.ChBody()
body_truss.SetBodyFixed(True)

my_system.AddBody(body_truss)

# Attach a 'box' shape asset for visualization.
mboxtruss = chrono.ChBoxShape()
mboxtruss.GetBoxGeometry().Pos = chrono.ChVectorD(-0.01, 0, 0)
mboxtruss.GetBoxGeometry().SetLengths(chrono.ChVectorD(0.02, 0.2, 0.1))
body_truss.AddAsset(mboxtruss)

# Create body for crank
body_crank = chrono.ChBody()

body_crank.SetPos((vB + vG) * 0.5)
my_system.AddBody(body_crank)