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