public static double kinematicsChainLimitScore(RigidBodyBasics start, RigidBodyBasics end) { return jointsLimitScore(MultiBodySystemTools.createOneDoFJointPath(start, end)); }
/** * Performs a deep copy of the kinematic chain described by the given {@code start} and {@code end}, * then filters the cloned joint to return only the ones implementing {@code OneDoFJoint}. * <p> * The clone of the kinematic chain has its own root body which reference frame is child of the * frame after the parent joint of {@code start}. As a result, the clone is an independent * multi-body system but its root is following the original multi-body system. * </p> * * @param start the rigid-body from where the kinematic chain starts. * @param end the rigid-body where the kinematic chain ends. * @return the clone kinematic chain. */ public static OneDoFJointBasics[] cloneOneDoFJointKinematicChain(RigidBodyBasics start, RigidBodyBasics end) { return cloneKinematicChainAndFilter(MultiBodySystemTools.createOneDoFJointPath(start, end), OneDoFJointBasics.class); }
private void randomizeKinematicsChainPositions(Random random, RigidBodyBasics base, RigidBodyBasics body, double percentOfMotionRangeAllowed) { percentOfMotionRangeAllowed = MathTools.clamp(percentOfMotionRangeAllowed, 0.0, 1.0); OneDoFJointBasics[] joints = MultiBodySystemTools.createOneDoFJointPath(base, body); randomizeJointPositions(random, joints, percentOfMotionRangeAllowed); }
private FullHumanoidRobotModel createFullRobotModelWithArmsAtMidRange() { FullHumanoidRobotModel robot = createFullRobotModelAtInitialConfiguration(); for (RobotSide robotSide : RobotSide.values) { RigidBodyBasics chest = robot.getChest(); RigidBodyBasics hand = robot.getHand(robotSide); Arrays.stream(MultiBodySystemTools.createOneDoFJointPath(chest, hand)).forEach(j -> setJointPositionToMidRange(j)); } return robot; }
private void setupTest() throws SimulationExceededMaximumTimeException { BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows()); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel()); drcSimulationTestHelper.createSimulation(getClass().getSimpleName()); ThreadTools.sleep(1000); FullHumanoidRobotModel fullRobotModel = drcSimulationTestHelper.getControllerFullRobotModel(); pelvis = fullRobotModel.getPelvis(); chest = fullRobotModel.getChest(); spineJoints = MultiBodySystemTools.createOneDoFJointPath(pelvis, chest); numberOfJoints = spineJoints.length; assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0)); SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); controllerSpy = new ControllerSpy(spineJoints, scs, getRobotModel().getControllerDT()); drcSimulationTestHelper.addRobotControllerOnControllerThread(controllerSpy); }
public void computeArmTrajectoryMessage(RobotSide robotSide) { RigidBodyBasics hand = fullRobotModelToUseForConversion.getHand(robotSide); RigidBodyBasics chest = fullRobotModelToUseForConversion.getChest(); OneDoFJointBasics[] armJoints = MultiBodySystemTools.createOneDoFJointPath(chest, hand); int numberOfArmJoints = armJoints.length; double[] desiredJointPositions = new double[numberOfArmJoints]; for (int i = 0; i < numberOfArmJoints; i++) { OneDoFJointBasics armJoint = armJoints[i]; desiredJointPositions[i] = MathTools.clamp(armJoint.getQ(), armJoint.getJointLimitLower(), armJoint.getJointLimitUpper()); } ArmTrajectoryMessage armTrajectoryMessage = robotSide == RobotSide.LEFT ? output.getLeftArmTrajectoryMessage() : output.getRightArmTrajectoryMessage(); armTrajectoryMessage.set(HumanoidMessageTools.createArmTrajectoryMessage(robotSide, trajectoryTime, desiredJointPositions)); }
PlanarRobotArm() { elevator = new RigidBody("elevator", worldFrame); elevatorFrame = elevator.getBodyFixedFrame(); centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", ReferenceFrame.getWorldFrame(), elevator); upperArm = createUpperArm(elevator); lowerArm = createLowerArm(upperArm); hand = createHand(lowerArm); scsRobotArm = new SCSRobotFromInverseDynamicsRobotModel("robotArm", elevator.getChildrenJoints().get(0)); scsRobotArm.setGravity(0); scsRobotArm.updateJointPositions_ID_to_SCS(); scsRobotArm.update(); addLinkGraphics(); addForcePoint(); oneDoFJoints = MultiBodySystemTools.createOneDoFJointPath(elevator, hand); elevator.updateFramesRecursively(); }
private void setupTest() throws SimulationExceededMaximumTimeException { BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows()); DRCObstacleCourseStartingLocation selectedLocation = DRCObstacleCourseStartingLocation.DEFAULT; FlatGroundEnvironment environment = new FlatGroundEnvironment(); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel()); drcSimulationTestHelper.setTestEnvironment(environment); drcSimulationTestHelper.setStartingLocation(selectedLocation); drcSimulationTestHelper.createSimulation(getClass().getSimpleName()); ThreadTools.sleep(1000); FullHumanoidRobotModel fullRobotModel = drcSimulationTestHelper.getControllerFullRobotModel(); head = fullRobotModel.getHead(); chest = fullRobotModel.getChest(); neckJoints = MultiBodySystemTools.createOneDoFJointPath(chest, head); numberOfJoints = neckJoints.length; drcSimulationTestHelper.getSimulationConstructionSet().hideAllYoGraphics(); drcSimulationTestHelper.getSimulationConstructionSet().setCameraPosition(5.0, 0.0, 2.0); drcSimulationTestHelper.getSimulationConstructionSet().setCameraFix(0.0, 0.0, 0.4); assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0)); }
RigidBodyBasics chest = fullRobotModel.getChest(); RigidBodyBasics head = fullRobotModel.getHead(); OneDoFJointBasics[] neckJoints = MultiBodySystemTools.createOneDoFJointPath(chest, head); int numberOfJoints = neckJoints.length; double[] desiredJointPositions = new double[numberOfJoints];
RigidBodyBasics chest = fullRobotModel.getChest(); RigidBodyBasics hand = fullRobotModel.getHand(robotSide); OneDoFJointBasics[] armJoints = MultiBodySystemTools.createOneDoFJointPath(chest, hand); int numberOfJoints = MultiBodySystemTools.computeDegreesOfFreedom(armJoints); double[] desiredJointPositions = generateRandomJointPositions(random, armJoints);
public static ArmTrajectoryMessage createArmMessage(FullHumanoidRobotModel fullRobotModel, RobotSide side) { RigidBodyBasics chest = fullRobotModel.getChest(); RigidBodyBasics hand = fullRobotModel.getHand(side); OneDoFJointBasics[] joints = MultiBodySystemTools.createOneDoFJointPath(chest, hand); ArmTrajectoryMessage message = HumanoidMessageTools.createArmTrajectoryMessage(side); for (int jointIdx = 0; jointIdx < joints.length; jointIdx++) { OneDoFJointBasics joint = joints[jointIdx]; double angle1 = MathTools.clamp(Math.toRadians(45.0), joint.getJointLimitLower() + 0.05, joint.getJointLimitUpper() - 0.05); double angle2 = MathTools.clamp(0.0, joint.getJointLimitLower() + 0.05, joint.getJointLimitUpper() - 0.05); OneDoFJointTrajectoryMessage jointTrajectoryMessage = message.getJointspaceTrajectory().getJointTrajectoryMessages().add(); jointTrajectoryMessage.getTrajectoryPoints().add().set(HumanoidMessageTools.createTrajectoryPoint1DMessage(0.2, angle1, 0.0)); jointTrajectoryMessage.getTrajectoryPoints().add().set(HumanoidMessageTools.createTrajectoryPoint1DMessage(0.4, angle2, 0.0)); } return message; } }
RobotArm() { elevator = new RigidBody("elevator", worldFrame); elevatorFrame = elevator.getBodyFixedFrame(); centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMass", ReferenceFrame.getWorldFrame(), elevator); shoulderDifferentialYaw = createDifferential("shoulderDifferential", elevator, new Vector3D(), Z); RigidBodyBasics shoulderDifferentialRoll = createDifferential("shoulderDifferential", shoulderDifferentialYaw, new Vector3D(), X); RigidBodyBasics upperArm = createUpperArm(shoulderDifferentialRoll); RigidBodyBasics lowerArm = createLowerArm(upperArm); RigidBodyBasics wristDifferentialRoll = createDifferential("wristDifferential", lowerArm, new Vector3D(0.0, 0.0, SHIN_LENGTH), X); //RigidBody wristDifferentialYaw = createDifferential("wristDifferential", wristDifferentialRoll, new Vector3d(), Z); hand = createHand(wristDifferentialRoll); scsRobotArm = new SCSRobotFromInverseDynamicsRobotModel("robotArm", elevator.getChildrenJoints().get(0)); scsRobotArm.setGravity(0); scsRobotArm.updateJointPositions_ID_to_SCS(); scsRobotArm.update(); addLinkGraphics(); addForcePoint(); oneDoFJoints = MultiBodySystemTools.createOneDoFJointPath(elevator, hand); elevator.updateFramesRecursively(); }
OneDoFJointBasics[] spineJoints = MultiBodySystemTools.createOneDoFJointPath(pelvis, chest); double[] chestDesiredJointAccelerations = RandomNumbers.nextDoubleArray(random, spineJoints.length, 0.1); SpineDesiredAccelerationsMessage desiredAccelerationsMessage = HumanoidMessageTools.createSpineDesiredAccelerationsMessage(chestDesiredJointAccelerations);
String handName = fullRobotModel.getHand(robotSide).getName(); OneDoFJointBasics[] armJoints = MultiBodySystemTools.createOneDoFJointPath(chest, hand); double[] armDesiredJointAccelerations = RandomNumbers.nextDoubleArray(random, armJoints.length, 0.1); ArmDesiredAccelerationsMessage armDesiredAccelerationsMessage = HumanoidMessageTools.createArmDesiredAccelerationsMessage(robotSide, armDesiredJointAccelerations);
RigidBodyBasics head = fullRobotModel.getHead(); String headName = head.getName(); OneDoFJointBasics[] neckJoints = MultiBodySystemTools.createOneDoFJointPath(chest, head);
RigidBodyBasics chest = fullRobotModel.getChest(); RigidBodyBasics hand = fullRobotModel.getHand(robotSide); OneDoFJointBasics[] armJoints = MultiBodySystemTools.createOneDoFJointPath(chest, hand); int numberOfJoints = armJoints.length; double[] desiredJointPositions = new double[numberOfJoints];
String handName = fullRobotModel.getHand(robotSide).getName(); OneDoFJointBasics[] armJoints = MultiBodySystemTools.createOneDoFJointPath(chest, hand);
OneDoFJointBasics[] controlledJoints = MultiBodySystemTools.createOneDoFJointPath(base, endEffector); GeometricJacobian jacobian = new GeometricJacobian(controlledJoints, base.getBodyFixedFrame()); jacobian.compute();
private void submitAndCheckVMC(RigidBodyBasics base, RigidBodyBasics endEffector, Wrench desiredWrench, SelectionMatrix6D selectionMatrix) OneDoFJointBasics[] controlledJoints = MultiBodySystemTools.createOneDoFJointPath(base, endEffector); GeometricJacobian jacobian = new GeometricJacobian(controlledJoints, base.getBodyFixedFrame()); jacobian.compute();
RigidBodyBasics hand = fullRobotModel.getHand(robotSide); OneDoFJointBasics[] armOriginal = MultiBodySystemTools.createOneDoFJointPath(chest, hand); OneDoFJointBasics[] armClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(chest, hand); for (int jointIndex = 0; jointIndex < armOriginal.length; jointIndex++)