rushilwiz/engine-software
1
0
Fork 0
mirror of https://github.com/PotentiaRobotics/engine-software.git synced 2025-04-09 23:00:21 -04:00
Code Issues Actions Packages Projects Releases Wiki Activity

COM fix

Browse Source
This commit is contained in:
eugenechoi2004 2021-02-20 13:31:25 -05:00
parent 9fd64d47db
commit f505a89be4
3 changed files with 50 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
.DS_Store vendored
View File

Binary file not shown.

BIN
simulation/.DS_Store vendored Normal file
View File

Binary file not shown.

50
simulation/controllers/olympian/olympian.py
View File

@ -4,6 +4,55 @@
# from controller import Robot, Motor, DistanceSensor # from controller import Robot, Motor, DistanceSensor
from controller import Robot from controller import Robot
import time import time
import numpy as np
def calculateCOM(robot):
# links needed to calculate COM
torso = robot.getDevice("torsoMotor")
upperLeftLeg= robot.getDevice("upperLeftLegMotor")
upperRightLeg = robot.getDevice("upperRightLegMotor")
links = [torso, upperLeftLeg, upperRightLeg] # additional links will be added, when they're made
totalMass = len(links) # assumption that all links have mass of 1
centerOfMass = [0, 0, 0, 0]
for i in links:
# instantiation of values
# need to add code to get joint angles
if i == torso:
r = 0.25
theta0 = 0
theta1 = 0
else:
r = 0.225
theta0 = 0
theta1 = 0
# instantiation of matrices
# assumption that all angle values are in degrees
transformationMatrixToLink = [[np.cos(theta0*np.pi/180), -np.sin(theta0*np.pi/180),
0, 2*r*np.cos(theta0*np.pi/180)], [np.sin(theta0*np.pi/180),
np.cos(theta0*np.pi/180), 0, 2*r*np.sin(theta0*np.pi/180)],
[0, 0, 1, 0], [0, 0, 0, 1]]
vectorOfJointAngles = [2*r*np.cos(theta1*np.pi/180), 2*r*np.sin(theta1*np.pi/180), 0, 1]
transformationMatrixToLinkCOM = [[np.cos(theta1*np.pi/180), 0, -np.sin(theta1*np.pi/180),
2*r*np.cos(theta0*np.pi/180)], [0, 1, 0, 0],
[np.sin(theta1*np.pi/180), 0, np.cos(theta1*np.pi/180),
2*r*np.sin(theta0*np.pi/180)], [0, 0, 0, 1]]
# COM of each link is calculated forward kinematics equations
calc = np.dot(transformationMatrixToLink,vectorOfJointAngles)
print(calc)
calc=np.dot(calc,transformationMatrixToLinkCOM)
print(calc)
#calc = transformationMatrixToLink.dot(vectorOfJointAngles).dot(transformationMatrixToLinkCOM)
# COM of the entire rigid body is calculated using a weighted average
centerOfMass += (1/totalMass)*calc
return centerOfMass[0], centerOfMass[1], centerOfMass[2]
def main(): def main():
# create the Robot instance. # create the Robot instance.
@ -12,6 +61,7 @@ def main():
timestep = 64 timestep = 64
start = 0 start = 0
while robot.step(32) != -1: while robot.step(32) != -1:
print(calculateCOM(robot))
start += 32/1000.0 start += 32/1000.0
head_motor = robot.getDevice("torso_yaw") head_motor = robot.getDevice("torso_yaw")
# motor2 = robot.getDevice("neck_roll") # motor2 = robot.getDevice("neck_roll")