public ControllerSpy(OneDoFJointBasics[] spineJoints, SimulationConstructionSet scs, double controllerDT) { this.spineJoints = spineJoints; this.controllerDT = controllerDT; spineJointClones = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(spineJoints); chestClone = spineJointClones[spineJointClones.length - 1].getSuccessor(); for (int jointIdx = 0; jointIdx < numberOfJoints; jointIdx++) { OneDoFJointBasics joint = spineJoints[jointIdx]; jointDesiredsMap.put(joint, findJointDesired(scs, joint)); jointControlEnabled.put(joint, findJointControlEnabled(scs, joint)); } orientationControlEnabled = findOrientationControlEnabled(scs, chest); desiredOrientation = findOrientationDesired(scs, chest); inconsistentControl.set(false); maxSpeed.set(0.0); }
List<OneDoFJointBasics> jointsInFuture = Arrays.asList(MultiBodySystemFactories.cloneOneDoFJointKinematicChain(joints.toArray(new OneDoFJointBasics[numberOfJoints])));
private ChestTrajectoryMessage createRandomChestMessage(double trajectoryTime, Random random) { OneDoFJointBasics[] spineClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(pelvis, chest); MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, spineClone); RigidBodyBasics chestClone = spineClone[spineClone.length - 1].getSuccessor(); FrameQuaternion desiredRandomChestOrientation = new FrameQuaternion(chestClone.getBodyFixedFrame()); desiredRandomChestOrientation.changeFrame(ReferenceFrame.getWorldFrame()); Quaternion desiredOrientation = new Quaternion(desiredRandomChestOrientation); return HumanoidMessageTools.createChestTrajectoryMessage(trajectoryTime, desiredOrientation, ReferenceFrame.getWorldFrame(), ReferenceFrame.getWorldFrame()); }
RigidBodyBasics chest = fullRobotModel.getChest(); OneDoFJointBasics[] spineClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(pelvis, chest);
OneDoFJointBasics[] spineClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(pelvis, chest);
RigidBodyBasics chest = fullRobotModel.getChest(); OneDoFJointBasics[] spineClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(pelvis, chest); MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, spineClone); RigidBodyBasics chestClone = spineClone[spineClone.length - 1].getSuccessor();
OneDoFJointBasics[] spineClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(pelvis, chest);
OneDoFJointBasics[] armClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(chest, hand); for (int jointIndex = 0; jointIndex < armOriginal.length; jointIndex++)
RigidBodyBasics hand = fullRobotModel.getHand(robotSide); OneDoFJointBasics[] arm = MultiBodySystemTools.createOneDoFJointPath(chest, hand); OneDoFJointBasics[] armClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(chest, hand); for (int i = 0; i < armClone.length; i++)
RigidBodyBasics hand = fullRobotModel.getHand(robotSide); OneDoFJointBasics[] arm = MultiBodySystemTools.createOneDoFJointPath(chest, hand); OneDoFJointBasics[] armClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(chest, hand); for (int i = 0; i < armClone.length; i++)
RigidBodyBasics hand = fullRobotModel.getHand(robotSide); OneDoFJointBasics[] arm = MultiBodySystemTools.createOneDoFJointPath(chest, hand); OneDoFJointBasics[] armClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(chest, hand); for (int i = 0; i < armClone.length; i++)