def LinkBodies_Hinge(body1, body2, link_position, link_direction, MiroSystem=False):
linkdir = ChVecify(link_direction)
linkpos = ChVecify(link_position)
# Get the quaternion that represents the rotation of the global z-axis to the link direction
z_ax = chrono.ChVectorD(0,0,1)
q = chrono.ChQuaternionD(1 + z_ax.Dot(linkdir), z_ax.Cross(linkdir))
q.Normalize()
# Create a new ChFrame coordinate system at the link position and with the global_z-to-linkdir rotation
mframe = chrono.ChFrameD(linkpos, q)
# Create a revolute link between the components at the coordinate system
mlink = chrono.ChLinkRevolute()
mlink.Initialize(body1, body2, mframe)
if MiroSystem:
MiroSystem.Add(mlink)
return mlink
ee.SetRot(chrono.ChMatrix33D(0, chrono.ChVectorD(0, 0, 1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath + '_EE.obj')
meshRotation = chrono.ChMatrix33D(np.pi / 2, chrono.ChVectorD(0, 1, 0))
mesh_for_visualization.Transform(chrono.ChVectorD(0, 0, 0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
ee.AddAsset(visualization_shape)
#----------------------- create the revolute joints ---------------------------
#------------- GB <-> L1l --------------
jt = chrono.ChLinkRevolute() #create revolute joint object
local = True #we will use the local frame
GB_frame = chrono.ChFrameD(chrono.ChVectorD(0, -1 * (yr - yl) / 2,
0.01)) #local frame of attachment
L1l_frame = chrono.ChFrameD(chrono.ChVectorD(-1 * _L1l / 2, 0,
0)) #local frame of attachment
jt.Initialize(GB, L1l, local, GB_frame, L1l_frame) #init joint
mysystem.Add(jt) #add to system
##------------- L1l <-> L2l --------------
jt = chrono.ChLinkRevolute() #create revolute joint object
local = True #we will use the local frame
L1l_frame = chrono.ChFrameD(chrono.ChVectorD(_L1l / 2, 0,
0.01)) #local frame of attachment
L2l_frame = chrono.ChFrameD(chrono.ChVectorD(-1 * _L23l / 2, 0,
0)) #local frame of attachment
mbody2 = chrono.ChBody()
mbody2.SetBodyFixed(False)
mysystem.Add(mbody2)
mboxasset = chrono.ChBoxShape()
mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1)
mbody2.AddAsset(mboxasset)
mboxtexture = chrono.ChTexture()
mboxtexture.SetTextureFilename('../../../data/textures/concrete.jpg')
mbody2.GetAssets().push_back(mboxtexture)
# Create a revolute constraint
mlink = chrono.ChLinkRevolute()
# the coordinate system of the constraint reference in abs. space:
mframe = chrono.ChFrameD(chrono.ChVectorD(0.1, 0.5, 0))
# initialize the constraint telling which part must be connected, and where:
mlink.Initialize(mbody1, mbody2, mframe)
mysystem.Add(mlink)
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
#
myapplication = chronoirr.ChIrrApp(mysystem, 'PyChrono example',
def addRevJoints(sys):
jt = chrono.ChLinkRevolute()
mdls.add_θ1l_joint(sys,jt)
jt = chrono.ChLinkRevolute()
mdls.add_θ1r_joint(sys,jt)
def buildALEXR(system,
Xee = .5,
Yee = -.5,
eeMass = 1,
side = "right"):
"""
public interface for building the ALEXR
:param system: the Chrono system to which the ALEXR robot is added
"""
#--------------- state infromation for initialization ---------------------
#initial location of the end effector (EE) of the ALEX Robot
#Xee = .5
#Yee = -.5
#specify mass properties of the payload at the end effector.
#eeMass = 1
#set the elbow discrete vars this will flip for right vs. left side useage
#side = "right" # "left"
#----------- Calculate IK angles using custom Library ---------------------
a = ALEXR() #encodes robot state info
system.refs = {} #setup the dictionary of object references
#ALEXR link lengths and origins
xl = a.xl ; yl = a.yl
xr = a.xr ; yr = a.yr
#left robot
_L1l = a.L1l
_L2l = a.L2l
_L3l = a.L3l
_L23l= a.L23l
#right robot
_L1r = a.L1r
_L2r = a.L2r
#calculate IK robot angles
θ1l , θ2l, θ1r , θ2r = a.IK_2DOF(Xee,Yee,side)
#perform a check here, and throw an error if the original configuration can't be solved.
if not a.feasable_point(Xee, Yee,side):
raise ValueError("there is no solution to IK for the end-effector location specified (Xee,Yee)")
#--------------- create each link as a rigid body -------------------------
# chrono uses a right handed coordinate system, and the model is constructed to look like the plotly model,
# but the irrelict visualizer operates in the mirror world - with a left handed coordinate system
# the long axis of the link to the right is the x axis, up is y, z is link rotation direction out of the page
# R is the rotation matrix between the default link frame orientation, and the solidworks coordinate system
# R (solidworks - > link frame)
#------------- ground body ------------
GB = chrono.ChBodyAuxRef()
GB.SetPos(chrono.ChVectorD(0,(yl+yr)/2,0))
GB.SetBodyFixed(True)
#set mesh visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'ground.obj')
# Optionally: you can scale/shrink/rotate/translate the mesh using this:
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
# Now the triangle mesh is inserted in a ChTriangleMeshShape visualization asset,
# and added to the body
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
GB.AddAsset(visualization_shape)
system.Add(GB)
system.refs["GB"] = GB
#--------- coordinate frame ---------------
coord = chrono.ChBodyAuxRef()
coord.SetPos(chrono.ChVectorD(0,0,0))
coord.SetBodyFixed(True)
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'coords.obj')
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), chrono.ChMatrix33D(.01))
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
coord.AddAsset(visualization_shape)
system.Add(coord)
#----------- left link 1 ------------------
#add body
L1l = chrono.ChBodyAuxRef()
L1l.SetBodyFixed(False)
system.Add(L1l)
system.refs["L1l"] = L1l
#set position,orientation with FK (while REF frame and COG frame are coincident)
x = xl + (_L1l/2)*np.cos(θ1l)
y = yl + (_L1l/2)*np.sin(θ1l)
L1l.SetPos(chrono.ChVectorD(x,y,.01))
L1l.SetRot(chrono.ChMatrix33D(θ1l,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_L1l.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
L1l.AddAsset(visualization_shape)
texture = chrono.ChTexture()
texture.SetTextureFilename(assetsPath + 'blue.png')
L1l.GetAssets().push_back(texture)
#set the mass and inertial properties
# xs ys zs
R1 = np.array([[ 0, 0, -1], #xl #found by hand
[ 0, -1, 0], #yl
[-1, 0, 0]]) #zl
L1l.SetMass(3.642)
Is = np.array([[ 0.13286460, -0.00001280, 0.03326759], # centroidal moment of inertia
[-0.00001280, 0.15328523,-0.00014996],
[ 0.03326759, -0.00014996, 0.03071782]])
Il = R1 @ Is @ np.linalg.inv(R1) # rotate the inertia tensor into the link frame
Ilch = chrono.ChMatrix33D()
Ilch.SetMatr(Il.tolist())
L1l.SetInertia(Ilch)
# move the COG frame
c = R1 @ np.array([[.0904],[-.0004],[.1461]])
L1l.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(c[0,0],c[1,0],c[2,0])))
#----------- left link 2 ------------------
#add body
L2l = chrono.ChBodyAuxRef()
L2l.SetBodyFixed(False)
system.Add(L2l)
system.refs["L2l"] = L2l
#set position,orientation with FK
x = xl + (_L1l)*np.cos(θ1l) + (_L23l/2)*np.cos(θ1l + θ2l)
y = yl + (_L1l)*np.sin(θ1l) + (_L23l/2)*np.sin(θ1l + θ2l)
L2l.SetPos(chrono.ChVectorD(x,y,.02))
L2l.SetRot(chrono.ChMatrix33D(θ1l + θ2l,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_L2l.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
#mesh origin was slightly off, so I hand tuned it
mesh_for_visualization.Transform(chrono.ChVectorD(-.00775,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
L2l.AddAsset(visualization_shape)
texture = chrono.ChTexture()
texture.SetTextureFilename(assetsPath + 'blue.png')
L2l.GetAssets().push_back(texture)
#set the mass and inertial properties
# xs ys zs
R2 = np.array([[ 0, 0, 1], #xl #found by hand
[ 0, 1, 0], #yl
[-1, 0, 0]]) #zl
L2l.SetMass(1.158)
Is = np.array([[ 0.04061717, 0.00000000, 0.00000000], # centroidal moment of inertia
[ 0.00000000, 0.04040908, 0.00000000],
[ 0.00000000, 0.00000000, 0.00072961]])
Il = R2 @ Is @ np.linalg.inv(R2) # rotate the inertia tensor into the link frame
Ilch = chrono.ChMatrix33D()
Ilch.SetMatr(Il.tolist())
L2l.SetInertia(Ilch)
# move the COG frame
c = R2 @ np.array([[0],[0],[-.1192]])
L2l.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(c[0,0],c[1,0],c[2,0])))
#----------- right link 1 -----------------
#add body
L1r = chrono.ChBodyAuxRef()
L1r.SetBodyFixed(False)
system.Add(L1r)
system.refs["L1r"] = L1r
#set position,orientation with FK
x = xr + (_L1r/2)*np.cos(θ1r)
y = yr + (_L1r/2)*np.sin(θ1r)
L1r.SetPos(chrono.ChVectorD(x,y,.02))
L1r.SetRot(chrono.ChMatrix33D(θ1r,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_L1r.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
L1r.AddAsset(visualization_shape)
texture = chrono.ChTexture()
texture.SetTextureFilename(assetsPath + 'red.png')
L1r.GetAssets().push_back(texture)
#set the mass and inertial properties
L1r.SetMass(4.1637)
Is = np.array([[ 0.05261769, -0.00006255, 0.02546226], # centroidal moment of inertia
[-0.00006255, 0.09428792,-0.00007718],
[ 0.02546226, -0.00007718, 0.05243999]])
Il = R1 @ Is @ np.linalg.inv(R1) # rotate the inertia tensor into the link frame
Ilch = chrono.ChMatrix33D()
Ilch.SetMatr(Il.tolist())
L1r.SetInertia(Ilch)
# move the COG frame
c = R1 @ np.array([[0.1222],[-0.0004],[.0927]])
L1r.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(c[0,0],c[1,0],c[2,0])))
#----------- right link 2 -----------------
#add body
L2r = chrono.ChBodyAuxRef()
L2r.SetBodyFixed(False)
system.Add(L2r)
system.refs["L2r"] = L2r
#set position,orientation with FK
x = xr + (_L1r)*np.cos(θ1r) + (_L2r/2)*np.cos(θ1r + θ2r)
y = yr + (_L1r)*np.sin(θ1r) + (_L2r/2)*np.sin(θ1r + θ2r)
L2r.SetPos(chrono.ChVectorD(x,y,.03))
L2r.SetRot(chrono.ChMatrix33D(θ1r + θ2r,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_L2r.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
L2r.AddAsset(visualization_shape)
texture = chrono.ChTexture()
texture.SetTextureFilename(assetsPath + 'red.png')
L2r.GetAssets().push_back(texture)
#set the mass and inertial properties
L2r.SetMass(1.1947)
Is = np.array([[ 0.06453132, 0.00000000, 0.00101029], # centroidal moment of inertia
[ 0.00000000, 0.06454599, 0.00000000],
[ 0.00101029, 0.00000000, 0.00093856]])
Il = R1 @ Is @ np.linalg.inv(R1) #R1 is correct here, I checked, rotate the inertia tensor into the link frame
Ilch = chrono.ChMatrix33D()
Ilch.SetMatr(Il.tolist())
L2r.SetInertia(Ilch)
# move the COG frame
c = R1 @ np.array([[-0.0041],[0.0000],[-0.0499]])
L2r.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(c[0,0],c[1,0],c[2,0])))
#----------- end effector payload ---------
#add body
ee = chrono.ChBodyAuxRef()
ee.SetBodyFixed(False)
system.Add(ee)
system.refs["EE"] = ee
#add mass properties //improve these based on actual data...
ee.SetMass(eeMass)
#can leave the inertia large, as this frame doesn't rotate (it can be thought of as on a bearing)
#set position,orientation with FK
x = xl + (_L1l)*np.cos(θ1l) + (_L23l)*np.cos(θ1l + θ2l)
y = yl + (_L1l)*np.sin(θ1l) + (_L23l)*np.sin(θ1l + θ2l)
ee.SetPos(chrono.ChVectorD(x,y,.03))
ee.SetRot(chrono.ChMatrix33D(0,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_EE.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
ee.AddAsset(visualization_shape)
#----------------------- create the revolute joints ---------------------------
# joint frame naming conventions
# X_c_a
# X - body the frame is attached to in the joint
# c_a - c frame represented in the a frame
# potential frames
# j - the joint frame - where the joint marker and frame is located
# ref - the reference frame located at the center of the link
# cog - the cog of the link, which is offset from the reference frame in an ChAuxRefBody()
# example L1l_j_cog
# this refers to a frame on body L1l, attached at the joint location, represented
# relative to the COG of L1l. this is the objective, as joints must be formed relative to
# a bodies COG frame, not it's auxillary reference frame
#------------- GB <-> L1l --------------
# jt = chrono.ChLinkRevolute() #set higher up
# add_θ1l_joint(system,jt)
#
##------------- L1l <-> L2l --------------
jt = chrono.ChLinkRevolute() # create revolute joint object
local = True # we will use the local frame
L1l_j_r = chrono.ChFrameD(chrono.ChVectorD(_L1l/2,0,0.01)) # local frame of attachment
L2l_j_r = chrono.ChFrameD(chrono.ChVectorD(-1*_L23l/2,0,0)) # local frame of attachment
L1l_j_COG = L1l_j_r >> L1l.GetFrame_REF_to_COG() # express L1l <-> L2l joint relative to L1l COG frame
L2l_j_COG = L2l_j_r >> L2l.GetFrame_REF_to_COG() # express L1l <-> L2l joint relative to L12 COG frame
jt.Initialize(L1l,L2l,local,L1l_j_COG,L2l_j_COG) # init joint
system.Add(jt) # add to system
system.refs["L1l<->L2l"] = jt # maintain a reference to the joint
##------------- GB <-> L1r --------------
# jt = chrono.ChLinkRevolute()
# add_θ1r_joint(system,jt)
##------------- L1r <-> L2r --------------
jt = chrono.ChLinkRevolute() # create revolute joint object
local = True # we will use the local frame
L1r_j_r = chrono.ChFrameD(chrono.ChVectorD(_L1r/2,0,.01)) # local frame of attachment
L2r_j_r = chrono.ChFrameD(chrono.ChVectorD(-1*_L2r/2,0,0)) # local frame of attachment
L1r_j_COG = L1r_j_r >> L1r.GetFrame_REF_to_COG() # express L1l <-> L2l joint relative to L1l COG frame
L2r_j_COG = L2r_j_r >> L2r.GetFrame_REF_to_COG() # express L1l <-> L2l joint relative to L12 COG frame
jt.Initialize(L1r,L2r,local,L1r_j_COG,L2r_j_COG) # init joint
system.Add(jt) # add to system
system.refs["L1r<->L2r"] = jt # maintain a reference to the joint
##------------- L2l <-> L2r --------------
jt = chrono.ChLinkRevolute() # create revolute joint object
local = True # we will use the local frame
dj = -1*(_L23l/2 - _L2l) # distance from center to joint point
L2l_j_r = chrono.ChFrameD(chrono.ChVectorD(dj,0,.01)) # local frame of attachment
L2r_j_r = chrono.ChFrameD(chrono.ChVectorD(_L2r/2,0,0)) # local frame of attachment
L2l_j_COG = L2l_j_r >> L2l.GetFrame_REF_to_COG() # express L2l <-> L2r joint relative to L2l COG frame
L2r_j_COG = L2r_j_r >> L2r.GetFrame_REF_to_COG() # express L2l <-> L2r joint relative to L2r COG frame
jt.Initialize(L2l,L2r,local,L2l_j_COG,L2r_j_COG) # init joint
system.Add(jt) # add to system
system.refs["L2l<->L2r"] = jt # maintain a reference to the joint
#
##------------- ee <-> L2l --------------
jt = chrono.ChLinkRevolute() # create revolute joint object
local = True # we will use the local frame
L2l_j_r = chrono.ChFrameD(chrono.ChVectorD(_L23l/2,0,.01)) # local frame of attachment
ee_j_r = chrono.ChFrameD(chrono.ChVectorD(0,0,0)) # local frame of attachment # COG isn't displaced in GB
L2l_j_COG = L2l_j_r >> L2l.GetFrame_REF_to_COG() # express ee <-> L2l joint relative to L1l COG frame
ee_j_COG = ee_j_r # COG isn't displaced in ee frame
jt.Initialize(L2l,ee,local,L2l_j_COG,ee_j_COG) # init joint
system.Add(jt) # add to system
system.refs["EE<->L2l"] = jt # maintain a reference to the joint
#no need to return, as system is passed by reference and then modified.
return system
#
# This also applies to SetRot().
#
# Create a revolute joint to connect pendulum to ground. We specify the link
# coordinate frame in the absolute frame.
rev_2 = chrono.ChLinkLockRevolute()
local = True
frameShift = True
if not local:
rev_2.Initialize(
ground, pend_2,
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, -2),
chrono.ChQuaternionD(1, 0, 0, 0)))
elif local and not frameShift:
rev_2 = chrono.ChLinkRevolute()
ground_frame = chrono.ChFrameD(chrono.ChVectorD(
0, 0, -1)) # local frame of attachment
pend_2_frame = chrono.ChFrameD(chrono.ChVectorD(
-1, 0, 0)) # local frame of attachment
rev_2.Initialize(ground, pend_2, local, ground_frame,
pend_2_frame) # init joint
elif local and frameShift:
rev_2 = chrono.ChLinkRevolute()
ground_frame = chrono.ChFrameD(chrono.ChVectorD(
0, 0, -1)) # local frame of attachment
pend_2_frame = chrono.ChFrameD(chrono.ChVectorD(
0, 0, 0)) # local frame of attachment
COG_to_REF = pend_2.GetFrame_COG_to_REF()
rev_2.Initialize(ground, pend_2, local, ground_frame,
((pend_2_frame >> COG_to_REF.GetInverse()))) # i