def test_com_motion_above_feet_WB(self): cs = ContactSequence() cs.loadFromBinary(str(PATH / "com_motion_above_feet_WB.cs")) self.assertEqual(cs.size(), 1) self.assertTrue(cs.haveConsistentContacts()) self.assertTrue(cs.haveTimings()) self.assertTrue(cs.haveCentroidalValues()) self.assertTrue(cs.haveCentroidalTrajectories()) self.assertTrue(cs.haveJointsTrajectories()) self.assertTrue(cs.haveJointsDerivativesTrajectories()) self.assertTrue(cs.haveContactForcesTrajectories()) self.assertTrue(cs.haveZMPtrajectories()) checkCS(self, cs, wholeBody=True)
def test_walk_20cm_quasistatic_WB(self): cs = ContactSequence() cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_WB.cs")) self.assertEqual(cs.size(), 23) self.assertTrue(cs.haveConsistentContacts()) self.assertTrue(cs.haveTimings()) self.assertTrue(cs.haveCentroidalValues()) self.assertTrue(cs.haveCentroidalTrajectories()) self.assertTrue(cs.haveEffectorsTrajectories(1e-1)) self.assertTrue(cs.haveJointsTrajectories()) self.assertTrue(cs.haveJointsDerivativesTrajectories()) self.assertTrue(cs.haveContactForcesTrajectories()) self.assertTrue(cs.haveZMPtrajectories()) self.assertTrue(cs.haveFriction()) self.assertTrue(cs.haveContactModelDefined()) checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True)
subprocess.run(["killall", "gepetto-gui"]) process_viewer = subprocess.Popen("gepetto-gui", stdout=subprocess.PIPE, stderr=subprocess.DEVNULL, preexec_fn=os.setpgrp) atexit.register(process_viewer.kill) # Load robot model in pinocchio rp = RosPack() urdf = rp.get_path(robot_package_name) + '/urdf/' + urdf_name + '.urdf' robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer()) robot.initDisplay(loadModel=True) robot.displayCollisions(False) robot.displayVisuals(True) # Load environment model cl = gepetto.corbaserver.Client() gui = cl.gui env_package_path = rp.get_path(env_package_name) env_urdf_path = env_package_path + '/urdf/' + env_name + '.urdf' gui.addUrdfObjects(scene_name + "/environments", env_urdf_path, True) # Load the motion from the multicontact-api file cs = ContactSequence() cs.loadFromBinary(cs_name) assert cs.haveJointsTrajectories( ), "The file loaded do not have joint trajectories stored." q_t = cs.concatenateQtrajectories() display_wb(robot, q_t)