def test_interface_is_callable(self):
print "### interface ###"
robot = Robot()
robot.get_joint_by_id(0)
robot.get_joint_by_name("Root")
#robot.debugprint()
robot.update(PyPose())
robot.inverse_chain_t(3, (0,0,-300), 1, 100, 3)
robot.inverse_chain(3, np.array((0,0,-300)), 1, 100, 3)
robot.get_centre_of_gravity()
robot.set_angles_to_pose(PyPose(), 0, -1)
def test_maspoint_simple_r(self):
print "### Masspoint simple right ###"
config = get_config()
config["RobotTypeName"] = "Darwin"
robot = Robot()
task = KinematicTask(robot)
r_chain = task.create_cog_chain(3)
l_chain = task.create_cog_chain(4)
robot.update(PyPose())
cog=robot.get_centre_of_gravity()
lfi = robot.get_joint_by_id(35).get_chain_matrix(inverse=True)
rfi = robot.get_joint_by_id(34).get_chain_matrix(inverse=True)
rcog = np.dot(rfi, cog)
lcog = np.dot(lfi, cog)
task.update_chain(r_chain, 3)
r_cmp = r_chain.get_joint(6).get_chain_masspoint()
diff = r_cmp - rcog[0:3]
print diff
