public void set(ReferenceFrame dataFrame, ReferenceFrame trajectoryFrame, SE3TrajectoryMessage message) { this.trajectoryFrame = trajectoryFrame; clear(dataFrame); setFromMessage(message); }
@Override public void setFromMessage(QuadrupedBodyTrajectoryMessage message) { isExpressedInAbsoluteTime = message.getIsExpressedInAbsoluteTime(); se3Trajectory.setFromMessage(message.getSe3Trajectory()); }
@Override public void setFromMessage(FootTrajectoryMessage message) { se3Trajectory.setFromMessage(message.getSe3Trajectory()); robotSide = RobotSide.fromByte(message.getRobotSide()); }
@Override public void setFromMessage(HandTrajectoryMessage message) { se3Trajectory.setFromMessage(message.getSe3Trajectory()); wrenchTrajectory.setFromMessage(message.getWrenchTrajectory()); robotSide = RobotSide.fromByte(message.getRobotSide()); }
@Override public void setFromMessage(PelvisTrajectoryMessage message) { setEnableUserPelvisControlDuringWalking(message.getEnableUserPelvisControlDuringWalking()); setEnableUserPelvisControl(message.getEnableUserPelvisControl()); se3Trajectory.setFromMessage(message.getSe3Trajectory()); }
@Override public void setFromMessage(HandHybridJointspaceTaskspaceTrajectoryMessage message) { robotSide = RobotSide.fromByte(message.getRobotSide()); jointspaceTrajectoryCommand.setFromMessage(message.getJointspaceTrajectoryMessage()); taskspaceTrajectoryCommand.setFromMessage(message.getTaskspaceTrajectoryMessage()); }
@Override public void set(ReferenceFrameHashCodeResolver resolver, SE3TrajectoryMessage message) { FrameInformation frameInformation = message.getFrameInformation(); long trajectoryFrameId = frameInformation.getTrajectoryReferenceFrameId(); long dataFrameId = HumanoidMessageTools.getDataFrameIDConsideringDefault(frameInformation); this.trajectoryFrame = resolver.getReferenceFrameFromHashCode(trajectoryFrameId); ReferenceFrame dataFrame = resolver.getReferenceFrameFromHashCode(dataFrameId); clear(dataFrame); setFromMessage(message); ReferenceFrame angularSelectionFrame = resolver.getReferenceFrameFromHashCode(message.getAngularSelectionMatrix().getSelectionFrameId()); ReferenceFrame linearSelectionFrame = resolver.getReferenceFrameFromHashCode(message.getLinearSelectionMatrix().getSelectionFrameId()); selectionMatrix.setSelectionFrames(angularSelectionFrame, linearSelectionFrame); ReferenceFrame angularWeightFrame = resolver.getReferenceFrameFromHashCode(message.getAngularWeightMatrix().getWeightFrameId()); ReferenceFrame linearWeightFrame = resolver.getReferenceFrameFromHashCode(message.getLinearWeightMatrix().getWeightFrameId()); weightMatrix.setWeightFrames(angularWeightFrame, linearWeightFrame); }