import sys
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
from numpy import matrix
if len(sys.argv) < 4:
print "Usage: plot_body hide_labels (True / False) ip_address [LLeg / RLeg]"
sys.exit(1)
hide_labels = sys.argv[1]
ip_address = sys.argv[2]
leg = sys.argv[3]
from CoM import CenterOfMass
com = CenterOfMass(ip_address, 9559)
data = com.get_locations_dict(leg)
com_loc = com.get_CoM(leg)
# define a number of chains to plot
chains = [
#left leg
["LAnklePitch", "LKneePitch", "LHipPitch", "LHipRoll",
"LHipYawPitch", "Torso"],
# right leg
["RAnkleRoll", "RAnklePitch", "RKneePitch", "RHipPitch", "RHipRoll",
"RHipYawPitch", "Torso"],
# left arm
["LElbowRoll", "LElbowYaw",
"LShoulderPitch", "Torso"],
# right arm
