def rotateBody2Leg(self, leg_index, body_coord):
""" Takes in CARTESIAN coordinates in body space and returns a CARTESIAN
array of xyz that has been rotated such that is is aligned with that
leg's coordinate frame
"""
hip_offset = self.getHipOffset(leg_index)
leg_aligned_coord = rotateZ(body_coord, -hip_offset[2])
return leg_aligned_coord
def rotateBody2Leg(self, leg_index, body_coord):
""" Takes in CARTESIAN coordinates in body space and returns a CARTESIAN
array of xyz that has been rotated such that is is aligned with that
leg's coordinate frame
"""
hip_offset = self.getHipOffset(leg_index)
leg_aligned_coord = rotateZ(body_coord, -hip_offset[2])
return leg_aligned_coord
def transformLeg2Body(self, leg_index, leg_coord):
""" Takes in CARTESIAN coordinates in leg space and returns a CARTESIAN
array of xyz in the body frame
"""
hip_offset = self.getHipOffset(leg_index)
rotated = rotateZ(leg_coord, hip_offset[2])
body_coord = rotated + array([hip_offset[0], hip_offset[1], 0])
return body_coord
def transformLeg2Body(self, leg_index, leg_coord):
""" Takes in CARTESIAN coordinates in leg space and returns a CARTESIAN
array of xyz in the body frame
"""
hip_offset = self.getHipOffset(leg_index)
rotated = rotateZ(leg_coord, hip_offset[2])
body_coord = rotated+array([hip_offset[0], hip_offset[1], 0])
return body_coord