public static RigidBodyExplorationConfigurationMessage createRigidBodyExplorationConfigurationMessage(RigidBodyBasics rigidBody, ConfigurationSpaceName[] degreesOfFreedomToExplore, double[] explorationRangeUpperLimits, double[] explorationRangeLowerLimits) { RigidBodyExplorationConfigurationMessage message = new RigidBodyExplorationConfigurationMessage(); if (degreesOfFreedomToExplore.length != explorationRangeUpperLimits.length || degreesOfFreedomToExplore.length != explorationRangeLowerLimits.length) throw new RuntimeException("Inconsistent array lengths: unconstrainedDegreesOfFreedom.length = " + degreesOfFreedomToExplore.length); message.setRigidBodyHashCode(rigidBody.hashCode()); byte[] degreesOfFreedomToExplore1 = ConfigurationSpaceName.toBytes(degreesOfFreedomToExplore); if (degreesOfFreedomToExplore1.length != explorationRangeUpperLimits.length || degreesOfFreedomToExplore1.length != explorationRangeLowerLimits.length) throw new RuntimeException("Inconsistent array lengths: unconstrainedDegreesOfFreedom.length = " + degreesOfFreedomToExplore1.length + ", explorationRangeLowerLimits.length = "); message.getConfigurationSpaceNamesToExplore().reset(); message.getExplorationRangeUpperLimits().reset(); message.getExplorationRangeLowerLimits().reset(); message.getConfigurationSpaceNamesToExplore().add(degreesOfFreedomToExplore1); message.getExplorationRangeUpperLimits().add(explorationRangeUpperLimits); message.getExplorationRangeLowerLimits().add(explorationRangeLowerLimits); return message; }
@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(); } }
public CameraInfo() { header_ = new std_msgs.msg.dds.Header(); distortion_model_ = new java.lang.StringBuilder(255); d_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6"); k_ = new double[9]; r_ = new double[9]; p_ = new double[12]; roi_ = new sensor_msgs.msg.dds.RegionOfInterest(); }
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); } }
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 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(); }
@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 static WaypointBasedTrajectoryMessage createWaypointBasedTrajectoryMessage(RigidBodyBasics endEffector, double[] waypointTimes, Pose3D[] waypoints, SelectionMatrix6D selectionMatrix) { WaypointBasedTrajectoryMessage message = new WaypointBasedTrajectoryMessage(); message.setEndEffectorHashCode(endEffector.hashCode()); if (waypointTimes.length != waypoints.length) throw new RuntimeException("Inconsistent array lengths."); message.getWaypointTimes().reset(); message.getWaypointTimes().add(waypointTimes); MessageTools.copyData(waypoints, message.getWaypoints()); if (selectionMatrix != null) { message.getAngularSelectionMatrix().setSelectionFrameId(MessageTools.toFrameId(selectionMatrix.getAngularSelectionFrame())); message.getAngularSelectionMatrix().setXSelected(selectionMatrix.isAngularXSelected()); message.getAngularSelectionMatrix().setYSelected(selectionMatrix.isAngularYSelected()); message.getAngularSelectionMatrix().setZSelected(selectionMatrix.isAngularZSelected()); message.getLinearSelectionMatrix().setSelectionFrameId(MessageTools.toFrameId(selectionMatrix.getLinearSelectionFrame())); message.getLinearSelectionMatrix().setXSelected(selectionMatrix.isLinearXSelected()); message.getLinearSelectionMatrix().setYSelected(selectionMatrix.isLinearYSelected()); message.getLinearSelectionMatrix().setZSelected(selectionMatrix.isLinearZSelected()); } return message; }
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); } }
@Override public boolean equals(Object other) { if(other == null) return false; if(other == this) return true; if(!(other instanceof IntrinsicParametersMessage)) return false; IntrinsicParametersMessage otherMyClass = (IntrinsicParametersMessage) other; if(this.sequence_id_ != otherMyClass.sequence_id_) return false; if(this.width_ != otherMyClass.width_) return false; if(this.height_ != otherMyClass.height_) return false; if(this.fx_ != otherMyClass.fx_) return false; if(this.fy_ != otherMyClass.fy_) return false; if(this.skew_ != otherMyClass.skew_) return false; if(this.cx_ != otherMyClass.cx_) return false; if(this.cy_ != otherMyClass.cy_) return false; if (!this.radial_.equals(otherMyClass.radial_)) return false; if(this.t1_ != otherMyClass.t1_) return false; if(this.t2_ != otherMyClass.t2_) return false; return true; }
@Override public boolean equals(Object other) { if(other == null) return false; if(other == this) return true; if(!(other instanceof KinematicsPlanningToolboxRigidBodyMessage)) return false; KinematicsPlanningToolboxRigidBodyMessage otherMyClass = (KinematicsPlanningToolboxRigidBodyMessage) other; if(this.sequence_id_ != otherMyClass.sequence_id_) return false; if(this.end_effector_hash_code_ != otherMyClass.end_effector_hash_code_) return false; if (!this.key_frame_times_.equals(otherMyClass.key_frame_times_)) return false; if (!this.key_frame_poses_.equals(otherMyClass.key_frame_poses_)) return false; if (!this.angular_selection_matrix_.equals(otherMyClass.angular_selection_matrix_)) return false; if (!this.linear_selection_matrix_.equals(otherMyClass.linear_selection_matrix_)) return false; if (!this.angular_weight_matrix_.equals(otherMyClass.angular_weight_matrix_)) return false; if (!this.linear_weight_matrix_.equals(otherMyClass.linear_weight_matrix_)) return false; if (!this.control_frame_position_in_end_effector_.equals(otherMyClass.control_frame_position_in_end_effector_)) return false; if (!this.control_frame_orientation_in_end_effector_.equals(otherMyClass.control_frame_orientation_in_end_effector_)) return false; if (!this.allowable_position_displacement_.equals(otherMyClass.allowable_position_displacement_)) return false; if (!this.allowable_orientation_displacement_.equals(otherMyClass.allowable_orientation_displacement_)) return false; return true; }
public static RigidBodyExplorationConfigurationMessage createRigidBodyExplorationConfigurationMessage(RigidBodyBasics rigidBody, ConfigurationSpaceName[] degreesOfFreedomToExplore, double[] explorationRangeAmplitudes) { RigidBodyExplorationConfigurationMessage message = new RigidBodyExplorationConfigurationMessage(); if (degreesOfFreedomToExplore.length != explorationRangeAmplitudes.length) throw new RuntimeException("Inconsistent array lengths: unconstrainedDegreesOfFreedom.length = " + degreesOfFreedomToExplore.length); message.setRigidBodyHashCode(rigidBody.hashCode()); byte[] degreesOfFreedomToExplore1 = ConfigurationSpaceName.toBytes(degreesOfFreedomToExplore); if (degreesOfFreedomToExplore1.length != explorationRangeAmplitudes.length) throw new RuntimeException("Inconsistent array lengths: unconstrainedDegreesOfFreedom.length = " + degreesOfFreedomToExplore1.length + ", explorationRangeLowerLimits.length = "); message.getConfigurationSpaceNamesToExplore().reset(); message.getExplorationRangeUpperLimits().reset(); message.getExplorationRangeLowerLimits().reset(); message.getConfigurationSpaceNamesToExplore().add(degreesOfFreedomToExplore1); for (int i = 0; i < degreesOfFreedomToExplore1.length; i++) { message.getExplorationRangeUpperLimits().add(explorationRangeAmplitudes[i]); message.getExplorationRangeLowerLimits().add(-explorationRangeAmplitudes[i]); } return message; }
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 boolean equals(Object other) { if(other == null) return false; if(other == this) return true; if(!(other instanceof WaypointBasedTrajectoryMessage)) return false; WaypointBasedTrajectoryMessage otherMyClass = (WaypointBasedTrajectoryMessage) other; if(this.sequence_id_ != otherMyClass.sequence_id_) return false; if(this.end_effector_hash_code_ != otherMyClass.end_effector_hash_code_) return false; if (!this.waypoint_times_.equals(otherMyClass.waypoint_times_)) return false; if (!this.waypoints_.equals(otherMyClass.waypoints_)) return false; if (!this.angular_selection_matrix_.equals(otherMyClass.angular_selection_matrix_)) return false; if (!this.linear_selection_matrix_.equals(otherMyClass.linear_selection_matrix_)) return false; if (!this.control_frame_position_in_end_effector_.equals(otherMyClass.control_frame_position_in_end_effector_)) return false; if (!this.control_frame_orientation_in_end_effector_.equals(otherMyClass.control_frame_orientation_in_end_effector_)) return false; if(this.weight_ != otherMyClass.weight_) return false; return true; }
@Override public boolean equals(Object other) { if(other == null) return false; if(other == this) return true; if(!(other instanceof LogData)) return false; LogData otherMyClass = (LogData) other; if(this.uid_ != otherMyClass.uid_) return false; if(this.timestamp_ != otherMyClass.timestamp_) return false; if(this.transmitTime_ != otherMyClass.transmitTime_) return false; if(this.type_ != otherMyClass.type_) return false; if(this.registry_ != otherMyClass.registry_) return false; if(this.offset_ != otherMyClass.offset_) return false; if(this.numberOfVariables_ != otherMyClass.numberOfVariables_) return false; if (!this.data_.equals(otherMyClass.data_)) return false; if (!this.jointStates_.equals(otherMyClass.jointStates_)) return false; return true; }
public void write() { if (fingers == null) { return; } HandJointAnglePacket packet = packetCopier.getCopyForWriting(); if (packet == null) { return; } packet.setRobotSide(side.toByte()); packet.setConnected(connected.get()); packet.setCalibrated(calibrated.get()); packet.getJointAngles().reset(); packet.getJointAngles().add(fingers); packetCopier.commit(); }
@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)); } }
public static KinematicsPlanningToolboxRigidBodyMessage createKinematicsPlanningToolboxRigidBodyMessage(RigidBodyBasics endEffector, TDoubleArrayList keyFrameTimes, List<Pose3DReadOnly> keyFramePoses) { KinematicsPlanningToolboxRigidBodyMessage message = new KinematicsPlanningToolboxRigidBodyMessage(); message.setEndEffectorHashCode(endEffector.hashCode()); if (keyFrameTimes.size() != keyFramePoses.size()) throw new RuntimeException("Inconsistent list lengths: keyFrameTimes.size() = " + keyFrameTimes.size() + ", keyFramePoses.size() = " + keyFramePoses.size()); for (int i = 0; i < keyFrameTimes.size(); i++) { message.getKeyFrameTimes().add(keyFrameTimes.get(i)); message.getKeyFramePoses().add().set(keyFramePoses.get(i)); } KinematicsPlanningToolboxMessageFactory.setDefaultAllowableDisplacement(message, keyFrameTimes.size()); return message; }
@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)); } }
public static IntrinsicParameters toIntrinsicParameters(IntrinsicParametersMessage message) { IntrinsicParameters intrinsicParameters = new IntrinsicParameters(); intrinsicParameters.width = message.getWidth(); intrinsicParameters.height = message.getHeight(); intrinsicParameters.fx = message.getFx(); intrinsicParameters.fy = message.getFy(); intrinsicParameters.skew = message.getSkew(); intrinsicParameters.cx = message.getCx(); intrinsicParameters.cy = message.getCy(); if (!message.getRadial().isEmpty()) intrinsicParameters.radial = message.getRadial().toArray(); intrinsicParameters.t1 = message.getT1(); intrinsicParameters.t2 = message.getT2(); return intrinsicParameters; }