Example #1
0
 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
Example #2
0
 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
Example #3
0
 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
Example #4
0
 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