def ui_P_cad2cm_lcs(_body_id_i, _body_id_j, _u_iP, _u_jP, _bodies):
"""
Function assigns the body properties to body (from list of bodies) and calculates u_P vector in CM LCS of a body for each body in joint
Args:
bodies - list of bodies
Returns:
body_i_
body_j_
u_iP_f_cm_
u_jP_f_cm_
"""
# check if id of body i is ground
if _body_id_i == "ground":
_u_iP_f_cm = _u_iP
else:
_body_i = _bodies[int(_body_id_i)]
_u_iP_f_cm = cad2cm_lcs(vector_in_CAD_LCS = _u_iP, CM_CAD_LCS = _body_i.CM_CAD_LCS[0:2])
if _body_id_j == "ground":
_u_jP_f_cm = _u_jP
else:
_body_j = _bodies[int(_body_id_j)]
_u_jP_f_cm = cad2cm_lcs(vector_in_CAD_LCS = _u_jP, CM_CAD_LCS = _body_j.CM_CAD_LCS[0:2])
return _u_iP_f_cm, _u_jP_f_cm
def u_P_cad2cm_lcs(_body_id, body, _u_P=np.zeros(2), theta=0):
"""
Function assigns the body properties to body (from list of bodies) and calculates u_P vector in CM LCS of a body for each body in joint
Args:
bodies - list of bodies
Returns:
body_id_
body_j_
u_iP_f_cm_
u_jP_f_cm_
"""
# check if id of body i is ground
if _body_id == "ground":
_u_P_f_cm = _u_P
else:
_u_P_f_cm = cad2cm_lcs(_u_P, body.CM_CAD_LCS[0:2], theta)
return _u_P_f_cm
def u_P_cad2cm_lcs(_body_id, body, _u_P=np.zeros(2), theta=0):
"""
Function assigns the body properties to body (from list of bodies) and calculates u_P vector in CM LCS of a body for each body in joint
Args:
bodies - list of bodies
Returns:
body_id_
body_j_
u_iP_f_cm_
u_jP_f_cm_
"""
# check if id of body i is ground
if _body_id == "ground":
_u_P_f_cm = _u_P
else:
_u_P_f_cm = cad2cm_lcs(_u_P, body.u_CAD[0:2], theta)
return _u_P_f_cm
