def left_foot_in_rf_frame_constraints(transform):
global __ineq_lf_in_rf
if __ineq_lf_in_rf is None:
obj = sl1m.load_obj(Robot.file_lf_in_rf)
__ineq_lf_in_rf = sl1m.as_inequalities(obj)
transform2 = transform.copy()
ine = sl1m.rotate_inequalities(__ineq_lf_in_rf, transform2)
return (ine.A, ine.b)
def left_foot_constraints(transform):
global __ineq_left_foot
if __ineq_left_foot is None:
obj = sl1m.load_obj(Robot.filekin_left)
__ineq_left_foot = sl1m.as_inequalities(obj)
transform2 = transform.copy()
transform2[2, 3] += 0.105
ine = sl1m.rotate_inequalities(__ineq_left_foot, transform2)
return (ine.A, ine.b)
def com_in_limb_effector_frame_constraint(Robot, transform, limbId):
"""
Generate the inequalities constraints for the CoM position given a contact position for one limb
:param Robot:
:param transform: Transformation to apply to the constraints
:param limbId: the Id of the limb used (see Robot.limbs_names list)
:return: [A, b] the inequalities, in the form Ax <= b
"""
global __ineq_com
assert (limbId <= len(Robot.limbs_names))
limb_name = Robot.limbs_names[limbId]
if __ineq_com[limbId] is None:
filekin = Robot.kinematic_constraints_path +"/COM_constraints_in_"+limb_name+ "_effector_frame_quasi_static_reduced.obj"
obj = sl1m.load_obj(filekin)
__ineq_com[limbId] = sl1m.as_inequalities(obj)
transform2 = transform.copy()
# ~ transform2[2,3] += 0.105
ine = sl1m.rotate_inequalities(__ineq_com[limbId], transform2)
return ine.A, ine.b
def foot_in_limb_effector_frame_constraint(Robot, transform, limbId, footId):
"""
Generate the constraints for the position of a given effector, given another effector position
:param Robot:
:param transform: The transform to apply to the constraints
:param limbId: the Id of the fixed limb (see Robot.limbs_names list)
:param footId: the Id of the limb for which the constraint are build
:return: [A, b] the inequalities, in the form Ax <= b
"""
global __ineq_relative
assert (limbId != footId)
limb_name = Robot.limbs_names[limbId]
eff_name = Robot.dict_limb_joint[Robot.limbs_names[footId]]
if __ineq_relative[limbId][realIdxFootId(limbId,footId)] is None:
filekin = Robot.relative_feet_constraints_path + "/" + eff_name + "_constraints_in_" + limb_name + "_reduced.obj"
obj = sl1m.load_obj(filekin)
__ineq_relative[limbId][realIdxFootId(limbId,footId)] = sl1m.as_inequalities(obj)
transform2 = transform.copy()
ine = sl1m.rotate_inequalities(__ineq_relative[limbId][realIdxFootId(limbId,footId)], transform2)
return ine.A, ine.b