def test_chain_masspoints(self): print "### Masspoint ###" 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()) task.update_chain(l_chain, 3) l_cmp = l_chain.get_joint(6).get_chain_masspoint() task.update_chain(r_chain, 3) r_cmp = r_chain.get_joint(6).get_chain_masspoint() r_cmp[1] = -r_cmp[1] diff = l_cmp - r_cmp diff = np.where(diff < 0, -diff, diff) max = diff.max() if(not diff.max() < 1e-5): print "Chain masspoints differ:\n%s\n%s\t%s" % (diff, l_cmp,r_cmp) print "\n FALSE DIFF\n %s \n" % diff
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