@Override public String toString() { StringBuilder builder = new StringBuilder(); builder.append("["); for(int i = 0; i < size(); i++) { if(i > 0) { builder.append(", "); } builder.append(get(i)); } builder.append("]"); return builder.toString(); }
public KinematicsToolboxOutputStatus getRobotConfiguration(WholeBodyTrajectoryToolboxOutputStatus solution, double time) { double minimumGap = Double.MAX_VALUE; int nodeIndex = 0; for (int i = 0; i < solution.getTrajectoryTimes().size(); i++) { double gap = Math.abs(time - solution.getTrajectoryTimes().get(i)); if (gap < minimumGap) { minimumGap = gap; nodeIndex = i; } } return solution.getRobotConfigurations().get(nodeIndex); } }
public KinematicsToolboxOutputStatus getRobotConfiguration(KinematicsPlanningToolboxOutputStatus solution, double time) { double minimumGap = Double.MAX_VALUE; int nodeIndex = 0; for (int i = 0; i < solution.getKeyFrameTimes().size(); i++) { double gap = Math.abs(time - solution.getKeyFrameTimes().get(i)); if (gap < minimumGap) { minimumGap = gap; nodeIndex = i; } } return solution.getRobotConfigurations().get(nodeIndex); } }
desiredOrientations[i] = new Quaternion(desiredOrientation); double time = firstTrajectoryPointTime + solution.getTrajectoryTimes().get(i); euclideanTrajectoryPointCalculator.appendTrajectoryPoint(time, new Point3D(desiredPosition)); orientationCalculator.appendTrajectoryPointOrientation(time, desiredOrientation);
private KinematicsToolboxOutputStatus findFrameFromTime(WholeBodyTrajectoryToolboxOutputStatus outputStatus, double time) { if (time <= 0.0) return outputStatus.getRobotConfigurations().get(0); else if (time >= outputStatus.getTrajectoryTimes().get(outputStatus.getTrajectoryTimes().size() - 1)) return outputStatus.getRobotConfigurations().get(outputStatus.getRobotConfigurations().size() - 1); else { double timeGap = 0.0; int indexOfFrame = 0; int numberOfTrajectoryTimes = outputStatus.getTrajectoryTimes().size(); for (int i = 0; i < numberOfTrajectoryTimes; i++) { timeGap = time - outputStatus.getTrajectoryTimes().get(i); if (timeGap < 0) { indexOfFrame = i; break; } } KinematicsToolboxOutputStatus frameOne = outputStatus.getRobotConfigurations().get(indexOfFrame - 1); KinematicsToolboxOutputStatus frameTwo = outputStatus.getRobotConfigurations().get(indexOfFrame); double timeOne = outputStatus.getTrajectoryTimes().get(indexOfFrame - 1); double timeTwo = outputStatus.getTrajectoryTimes().get(indexOfFrame); double alpha = (time - timeOne) / (timeTwo - timeOne); return MessageTools.interpolateMessages(frameOne, frameTwo, alpha); } }
@Override public void set(WaypointBasedTrajectoryMessage message, Map<Integer, RigidBodyBasics> rigidBodyNamedBasedHashMap, ReferenceFrameHashCodeResolver referenceFrameResolver) { clear(); endEffectorHashCode = message.getEndEffectorHashCode(); if (rigidBodyNamedBasedHashMap == null) endEffector = null; else endEffector = rigidBodyNamedBasedHashMap.get(endEffectorHashCode); for (int i = 0; i < message.getWaypoints().size(); i++) { waypointTimes.add(message.getWaypointTimes().get(i)); waypoints.add().set(message.getWaypoints().get(i)); } ReferenceFrame referenceFrame = endEffector == null ? null : endEffector.getBodyFixedFrame(); controlFramePose.setIncludingFrame(referenceFrame, message.getControlFramePositionInEndEffector(), message.getControlFrameOrientationInEndEffector()); selectionMatrix.resetSelection(); SelectionMatrix3DMessage angularSelection = message.getAngularSelectionMatrix(); SelectionMatrix3DMessage linearSelection = message.getLinearSelectionMatrix(); selectionMatrix.setAngularAxisSelection(angularSelection.getXSelected(), angularSelection.getYSelected(), angularSelection.getZSelected()); selectionMatrix.setLinearAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected()); weight = message.getWeight(); }
public static Pose3D unpackPose(WaypointBasedTrajectoryMessage waypointBasedTrajectoryMessage, double time) { if (time <= 0.0) return waypointBasedTrajectoryMessage.getWaypoints().get(0); else if (time >= waypointBasedTrajectoryMessage.getWaypointTimes().get(waypointBasedTrajectoryMessage.getWaypointTimes().size() - 1)) return waypointBasedTrajectoryMessage.getWaypoints().getLast(); else { double timeGap = 0.0; int indexOfFrame = 0; int numberOfTrajectoryTimes = waypointBasedTrajectoryMessage.getWaypointTimes().size(); for (int i = 0; i < numberOfTrajectoryTimes; i++) { timeGap = time - waypointBasedTrajectoryMessage.getWaypointTimes().get(i); if (timeGap < 0) { indexOfFrame = i; break; } } Pose3D poseOne = waypointBasedTrajectoryMessage.getWaypoints().get(indexOfFrame - 1); Pose3D poseTwo = waypointBasedTrajectoryMessage.getWaypoints().get(indexOfFrame); double timeOne = waypointBasedTrajectoryMessage.getWaypointTimes().get(indexOfFrame - 1); double timeTwo = waypointBasedTrajectoryMessage.getWaypointTimes().get(indexOfFrame); double alpha = (time - timeOne) / (timeTwo - timeOne); Pose3D ret = new Pose3D(); ret.interpolate(poseOne, poseTwo, alpha); return ret; } }
@Override public Pose3D compute(double time) { Pose3D current = new Pose3D(); Pose3D previous = null; Pose3D next = null; double t0 = Double.NaN; double tf = Double.NaN; for (int i = 1; i < message.getWaypoints().size(); i++) { t0 = message.getWaypointTimes().get(i - 1); tf = message.getWaypointTimes().get(i); previous = message.getWaypoints().get(i - 1); next = message.getWaypoints().get(i); if (time < message.getWaypointTimes().get(i)) break; } double alpha = (time - t0) / (tf - t0); alpha = MathTools.clamp(alpha, 0.0, 1.0); current.interpolate(previous, next, alpha); return current; } };
public void set(KinematicsPlanningToolboxCenterOfMassMessage message, Map<Long, RigidBodyBasics> rigidBodyNamedBasedHashMap, ReferenceFrameHashCodeResolver referenceFrameResolver) { for (int i = 0; i < message.getWayPointTimes().size(); i++) { waypointTimes.add(message.getWayPointTimes().get(i)); waypoints.add().set(message.getDesiredWayPointPositionsInWorld().get(i)); } selectionMatrix.clearSelectionFrame(); SelectionMatrix3DMessage linearSelection = message.getSelectionMatrix(); selectionMatrix.setAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected()); weightMatrix.clear(); WeightMatrix3DMessage linearWeight = message.getWeights(); weightMatrix.setWeights(linearWeight.getXWeight(), linearWeight.getYWeight(), linearWeight.getZWeight()); if (referenceFrameResolver != null) { ReferenceFrame linearSelectionFrame = referenceFrameResolver.getReferenceFrameFromHashCode(linearSelection.getSelectionFrameId()); selectionMatrix.setSelectionFrame(linearSelectionFrame); ReferenceFrame linearWeightFrame = referenceFrameResolver.getReferenceFrameFromHashCode(linearWeight.getWeightFrameId()); weightMatrix.setWeightFrame(linearWeightFrame); } }
private void visualizeSolution(WholeBodyTrajectoryToolboxOutputStatus solution, double timeResolution) throws UnreasonableAccelerationException { hideRobot(); robot.getControllers().clear(); FullHumanoidRobotModel robotForViz = getRobotModel().createFullRobotModel(); FloatingJointBasics rootJoint = robotForViz.getRootJoint(); OneDoFJointBasics[] joints = FullRobotModelUtils.getAllJointsExcludingHands(robotForViz); double trajectoryTime = solution.getTrajectoryTimes().get(solution.getTrajectoryTimes().size() - 1); double t = 0.0; while (t <= trajectoryTime) { t += timeResolution; KinematicsToolboxOutputStatus frame = findFrameFromTime(solution, t); MessageTools.unpackDesiredJointState(frame, rootJoint, joints); robotForViz.updateFrames(); snapGhostToFullRobotModel(robotForViz); scs.simulateOneTimeStep(); } }
@Override public void doControl() { if (toolboxOutputQueue.isNewPacketAvailable()) { KinematicsPlanningToolboxOutputStatus solution = toolboxOutputQueue.poll(); Double keyFrameTimes = solution.getKeyFrameTimes(); if (keyFrameTimes.size() != solution.getKeyFrameTimes().size()) throw new RuntimeException("size of key frames are not matched : (given) " + keyFrameTimes.size() + ", (output) " + solution.getKeyFrameTimes().size()); if (solution.getSolutionQuality() < 0) throw new RuntimeException("a key frame can not be accepted."); trajectoryTime = keyFrameTimes.get(keyFrameTimes.size() - 1); WholeBodyTrajectoryMessage message = new WholeBodyTrajectoryMessage(); message.setDestination(PacketDestination.CONTROLLER.ordinal()); outputConverter.setMessageToCreate(message); outputConverter.computeWholeBodyTrajectoryMessage(solution); wholeBodyTrajectoryPublisher.publish(message); deactivateKinematicsToolboxModule(); } }
@Override public void set(ReachingManifoldMessage message, Map<Integer, RigidBodyBasics> rigidBodyHashMap, ReferenceFrameHashCodeResolver referenceFrameResolver) { clear(); rigidBodyHashCode = message.getEndEffectorHashCode(); if (rigidBodyHashMap == null) rigidBody = null; else rigidBody = rigidBodyHashMap.get(rigidBodyHashCode); this.manifoldOriginPosition.set(message.getManifoldOriginPosition()); this.manifoldOriginOrientation.set(message.getManifoldOriginOrientation()); for (int i = 0; i < message.getManifoldConfigurationSpaceNames().size(); i++) { manifoldConfigurationSpaces.add(ConfigurationSpaceName.fromByte(message.getManifoldConfigurationSpaceNames().get(i))); manifoldLowerLimits.add(message.getManifoldLowerLimits().get(i)); manifoldUpperLimits.add(message.getManifoldUpperLimits().get(i)); } }
@Override public void set(RigidBodyExplorationConfigurationMessage message, Map<Integer, RigidBodyBasics> rigidBodyHashMap, ReferenceFrameHashCodeResolver referenceFrameResolver) { clear(); rigidBodyHashCode = message.getRigidBodyHashCode(); if (rigidBodyHashMap == null) rigidBody = null; else rigidBody = rigidBodyHashMap.get(rigidBodyHashCode); for (int i = 0; i < message.getConfigurationSpaceNamesToExplore().size(); i++) { degreesOfFreedomToExplore.add(ConfigurationSpaceName.fromByte(message.getConfigurationSpaceNamesToExplore().get(i))); explorationRangeUpperLimits.add(message.getExplorationRangeUpperLimits().get(i)); explorationRangeLowerLimits.add(message.getExplorationRangeLowerLimits().get(i)); } }
@Override public void setFromMessage(DesiredAccelerationsMessage message) { desiredJointAccelerations.reset(); for (int i = 0; i < message.getDesiredJointAccelerations().size(); i++) { desiredJointAccelerations.add(message.getDesiredJointAccelerations().get(i)); } setQueueableCommandVariables(message.getQueueingProperties()); }
private static Graphics3DObject createTrajectoryMessageVisualization(WaypointBasedTrajectoryMessage trajectoryMessage, double radius, AppearanceDefinition appearance) { double t0 = trajectoryMessage.getWaypointTimes().get(0); double tf = trajectoryMessage.getWaypointTimes().get(trajectoryMessage.getWaypoints().size() - 1); double timeResolution = (tf - t0) / trajectoryMessage.getWaypoints().size(); FunctionTrajectory trajectoryToVisualize = WholeBodyTrajectoryToolboxMessageTools.createFunctionTrajectory(trajectoryMessage); return createFunctionTrajectoryVisualization(trajectoryToVisualize, t0, tf, timeResolution, radius, appearance); }
public void set(Double other) { resetQuick(); for(int i = 0; i < other.size(); i++) { add(other.get(i)); } }
public static double unpackJointAngle(HandJointAnglePacket handJointAnglePacket, HandJointName jointName) { int index = jointName.getIndex(RobotSide.fromByte(handJointAnglePacket.getRobotSide())); if (index == -1) { return 0; } return handJointAnglePacket.getJointAngles().get(index); }
@Override public void writeElement(int i, CDR cdr) { cdr.write_type_6(get(i)); }