@Override public void addTrajectoryPoint(P trajectoryPoint) { trajectoryPoints.add().set(trajectoryPoint); }
protected F addAndInitializeTrajectoryPoint() { F newTrajectoryPoint = trajectoryPoints.add(); newTrajectoryPoint.setToZero(referenceFrame); return newTrajectoryPoint; }
public void addJoint(OneDoFJoint joint, double desiredPosition, double desiredVelocity, double feedForwardAcceleration) { joints.add(joint); jointNames.add(joint.getName()); desiredPositions.add().setValue(desiredPosition); desiredVelocities.add().setValue(desiredVelocity); feedForwardAccelerations.add().setValue(feedForwardAcceleration); }
public void set(OneDoFJointTrajectoryMessage[] trajectoryPointListArray) { clear(); for (int i = 0; i < trajectoryPointListArray.length; i++) { SimpleTrajectoryPoint1DList simpleTrajectoryPoint1DList = jointTrajectoryInputs.add(); OneDoFJointTrajectoryMessage oneJointTrajectoryMessage = trajectoryPointListArray[i]; oneJointTrajectoryMessage.getTrajectoryPoints(simpleTrajectoryPoint1DList); } }
public void set(OneDoFJointTrajectoryMessage[] trajectoryPointListArray) { clear(); for (int i = 0; i < trajectoryPointListArray.length; i++) { SimpleTrajectoryPoint1DList simpleTrajectoryPoint1DList = jointTrajectoryInputs.add(); OneDoFJointTrajectoryMessage oneJointTrajectoryMessage = trajectoryPointListArray[i]; oneJointTrajectoryMessage.getTrajectoryPoints(simpleTrajectoryPoint1DList); } }
private void extendBySegment(YoVariableRegistry registry) { int size = waypointTimes.size() + 1; for (Direction axis : Direction.values) trajectories.get(axis).add(new YoPolynomial(namePrefix + "Segment" + size + "Axis" + axis.getIndex(), order.getCoefficients(), registry)); waypointTimes.add(new DoubleYoVariable(namePrefix + "WaypointTime" + size, registry)); waypointPositions.add(); }
private void copyExternalWrenchCommand(ExternalWrenchCommand commandToCopy) { ExternalWrenchCommand localCommand = externalWrenchCommands.add(); localCommand.set(commandToCopy); inverseDynamicsCommandList.addCommand(localCommand); }
private void copySpatialAccelerationCommand(SpatialAccelerationCommand commandToCopy) { SpatialAccelerationCommand localCommand = spatialAccelerationCommands.add(); localCommand.set(commandToCopy); inverseDynamicsCommandList.addCommand(localCommand); }
private void copyOrientationFeedbackControlCommand(OrientationFeedbackControlCommand commandToCopy) { OrientationFeedbackControlCommand localCommand = orientationFeedbackControlCommands.add(); localCommand.set(commandToCopy); feedbackControlCommandList.addCommand(localCommand); }
private void copyPointAcclerationCommand(PointAccelerationCommand commandToCopy) { PointAccelerationCommand localCommand = pointAccelerationCommands.add(); localCommand.set(commandToCopy); inverseDynamicsCommandList.addCommand(localCommand); }
private void copyJointAccelerationIntegrationCommand(JointAccelerationIntegrationCommand commandToCopy) { JointAccelerationIntegrationCommand localCommand = jointAccelerationIntegrationCommands.add(); localCommand.set(commandToCopy); inverseDynamicsCommandList.addCommand(localCommand); }
public void set(RobotSide robotSide, OneDoFJointTrajectoryMessage[] trajectoryPointListArray) { clear(robotSide); for (int i = 0; i < trajectoryPointListArray.length; i++) { SimpleTrajectoryPoint1DList simpleTrajectoryPoint1DList = jointTrajectoryInputs.add(); OneDoFJointTrajectoryMessage armOneJointTrajectoryMessage = trajectoryPointListArray[i]; armOneJointTrajectoryMessage.getTrajectoryPoints(simpleTrajectoryPoint1DList); } }
private void extendBySegment(YoVariableRegistry registry) { int size = waypointTimes.size() + 1; for (Direction axis : Direction.values) trajectories.get(axis).add(new YoPolynomial(namePrefix + "Segment" + size + "Axis" + axis.getIndex(), order.getCoefficients(), registry)); waypointTimes.add(new DoubleYoVariable(namePrefix + "WaypointTime" + size, registry)); waypointPositions.add(); }
public void set(RobotSide robotSide, double trajectoryTime, double[] desiredJointPositions) { clear(); for (int jointIndex = 0; jointIndex < desiredJointPositions.length; jointIndex++) { SimpleTrajectoryPoint1DList jointTrajectoryInput = jointTrajectoryInputs.add(); jointTrajectoryInput.clear(); jointTrajectoryInput.addTrajectoryPoint(trajectoryTime, desiredJointPositions[jointIndex], 0.0); } }
public void appendTrajectoryPoint(Point3d position) { FrameEuclideanTrajectoryPoint newTrajectoryPoint = trajectoryPoints.add(); newTrajectoryPoint.setToZero(referenceFrame); newTrajectoryPoint.setTimeToNaN(); newTrajectoryPoint.setPosition(position); newTrajectoryPoint.setLinearVelocityToNaN(); }
public void setPredictedContactPoints(RecyclingArrayList<Point2d> predictedContactPoints) { this.predictedContactPoints.clear(); for(int i = 0; i < predictedContactPoints.size(); i++) this.predictedContactPoints.add().set(predictedContactPoints.get(i)); }
public void setPredictedContactPoints(RecyclingArrayList<Point2d> predictedContactPoints) { this.predictedContactPoints.clear(); for(int i = 0; i < predictedContactPoints.size(); i++) this.predictedContactPoints.add().set(predictedContactPoints.get(i)); }
public void setSwingWaypoints(RecyclingArrayList<Point3d> trajectoryWaypoints) { swingWaypoints.clear(); for (int i = 0; i < trajectoryWaypoints.size(); i++) swingWaypoints.add().set(trajectoryWaypoints.get(i)); }
@Override public void set(LidarScanCommand other) { timestamp = other.timestamp; lidarPose.setIncludingFrame(other.lidarPose); scan.clear(); for (int i = 0; i < other.scan.size(); i++) scan.add().set(other.scan.get(i)); }
@Override public void set(AdjustFootstepCommand other) { robotSide = other.robotSide; origin = other.origin; adjustedPosition.set(other.adjustedPosition); adjustedOrientation.set(other.adjustedOrientation); RecyclingArrayList<Point2d> otherPredictedContactPoints = other.predictedContactPoints; predictedContactPoints.clear(); for (int i = 0; i < otherPredictedContactPoints.size(); i++) predictedContactPoints.add().set(otherPredictedContactPoints.get(i)); }